METHOD FOR CONTROLLING A ROBOT DEVICE

Abstract

A method for controlling a robot device. The method includes: determining, using sensor data representing a surrounding of the robot device, whether there are one or more humans in the surrounding of the robot device; determining for each human of the one or more humans: a body pose and a velocity of the human, a predicted motion of the human for a future time period, and, using the body pose, the velocity, and the predicted motion, an occupation area occupied by the human in the future time period; generating control parameters for controlling the robot device such that a predefined distance of the robot device to the occupation area of each of the one or more humans is ensured in the future time period; and controlling the robot device in accordance with the control parameters.

Claims

1. A method for controlling a robot device, the method comprising the following steps: determining, using sensor data which represent a surrounding of the robot device, whether there are one or more humans in a surrounding of the robot device; based on determining that there are one or more humans in the surrounding of the robot device, determining for each human of the one or more humans: a respective body pose and a respective velocity of the human using the sensor data, a respective predicted motion of the human for a future time period using the sensor data, using the respective body pose, the respective velocity, and the respective predicted motion, a respective occupation area occupied by the human in the future time period; generating control parameters for controlling the robot device such that a predefined distance of the robot device to the respective occupation area of each of the one or more humans is ensured in the future time period; and controlling the robot device in accordance with the control parameters.

2. The method according to claim 1, further comprising: for each human of the one or more humans, determining a respective activity of the human using the sensor data; wherein the generating of the control parameters includes determining a velocity of the robot device using the respective activity of each of the one or more humans.

3. The method according to claim 2, wherein the determining of the velocity of the robot device includes: determining an average level of human activity in the surrounding of the robot device using the respective activity of each of the one or more humans; and determining, using the average level of human activity, the velocity of the robot device by decreasing a predefined robot velocity with an increasing average level of human activity.

4. The method according to claim 1, wherein the determining of the respective occupation area occupied by the human at a point in time in the future time period includes: determining, using the respective body pose of the human, a respective body pose area occupied by the human; and determining, using the respective velocity of the human, the respective occupation area by increasing the respective body pose area with increasing velocity of the human.

5. The method according to claim 4, wherein the determining of the respective body pose area occupied by the human includes: projecting a respective two-dimensional ellipse onto a ground on which the robot device is positioned, the two-dimensional ellipse bounding the respective body pose of the human in a plan view; and wherein the determining of the respective occupation area includes: increasing a size of the respective two-dimensional ellipse as a function of the respective velocity of the human, wherein a size of the respective two-dimensional ellipse increases with increasing velocity of the human.

6. A robot device controller configured to control a robot device, the robot device controller configured to: determine, using sensor data which represent a surrounding of the robot device, whether there are one or more humans in a surrounding of the robot device; based on determining that there are one or more humans in the surrounding of the robot device, determine for each human of the one or more humans: a respective body pose and a respective velocity of the human using the sensor data, a respective predicted motion of the human for a future time period using the sensor data, using the respective body pose, the respective velocity, and the respective predicted motion, a respective occupation area occupied by the human in the future time period; generate control parameters for controlling the robot device such that a predefined distance of the robot device to the respective occupation area of each of the one or more humans is ensured in the future time period; and control the robot device in accordance with the control parameters.

7. A robot device, comprising: a robot device controller configured to control the robot device, the robot device controller configured to: determine, using sensor data which represent a surrounding of the robot device, whether there are one or more humans in a surrounding of the robot device, based on determining that there are one or more humans in the surrounding of the robot device, determine for each human of the one or more humans: a respective body pose and a respective velocity of the human using the sensor data, a respective predicted motion of the human for a future time period using the sensor data, using the respective body pose, the respective velocity, and the respective predicted motion, a respective occupation area occupied by the human in the future time period, generate control parameters for controlling the robot device such that a predefined distance of the robot device to the respective occupation area of each of the one or more humans is ensured in the future time period, and control the robot device in accordance with the control parameters; and at least one perception sensor configured to acquire the sensor data representing the surrounding of the robot device.

8. A non-transitory computer-readable medium on which are stored instructions for controlling a robot device, the instructions, when executed by a computer, causing the computer to perform the following steps: determining, using sensor data which represent a surrounding of the robot device, whether there are one or more humans in a surrounding of the robot device; based on determining that there are one or more humans in the surrounding of the robot device, determining for each human of the one or more humans: a respective body pose and a respective velocity of the human using the sensor data, a respective predicted motion of the human for a future time period using the sensor data, using the respective body pose, the respective velocity, and the respective predicted motion, a respective occupation area occupied by the human in the future time period; generating control parameters for controlling the robot device such that a predefined distance of the robot device to the respective occupation area of each of the one or more humans is ensured in the future time period; and controlling the robot device in accordance with the control parameters.

Description

BRIEF DESCRIPTION OF THE DRAWING

[0021] FIG. 1 shows a robot device arrangement according to various aspects of the present invention.

[0022] FIG. 2 shows a flow diagram of a method for controlling a robot device according to various aspects of the present invention.

[0023] FIG. 3 shows an exemplary flow chart for controlling the robot device according to various aspects of the present invention.

[0024] FIG. 4 shows a schematic illustrating an exemplary determination of a distance between the robot device and an occupation area occupied by a human according to various aspects of the present invention.

[0025] FIG. 5 shows exemplary human skeleton poses for exemplary activities of the present invention.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

[0026] The following detailed description refers to the accompanying drawings that show, by way of illustration, specific details and aspects of this disclosure in which the present invention may be practiced. Other aspects may be utilized and structural, logical, and electrical changes may be made without departing from the scope of the present invention. The various aspects of this disclosure are not necessarily mutually exclusive, as some aspects of this disclosure can be combined with one or more other aspects of this disclosure to form new aspects.

[0027] In the following, various examples will be described in more detail.

[0028] FIG. 1 shows a robot device arrangement 100 according to various aspects. The robot device arrangement 100 may include a robot device 102 (short: robot). The robot device 102 shown in FIG. 1 and described below by way of example is an exemplary robot device serving for illustration and may include, for example, a transport robot for transporting objects (e.g., goods) 104 within a dynamic environment, such as a factory. It is noted that this robot device serves for illustration and may, in general, be any type of robot that is capable to navigate (e.g., autonomously or at least semi-autonomously). As an example, the robot device 102 may be an Active Shuttle from Bosch Rexroth. The robot device 102 may be configured to be moveable (e.g., drivable) on ground. For this, the robot device 102 may include, for example, wheels or any other suitable component (e.g., crawler tracks, support legs, etc.). Hence, the robot device 102 may be a ground-driven robot device.

[0029] For control of the robot device 102, the robot device arrangement 100 may include a (robot) controller 108 configured to implement the interaction with the (dynamic) environment according to a control program. In some aspects, the robot device 102 may include the controller 108. In other aspects, the robot device arrangement 100 may include the (central) controller 108 configured to control one or more than one robot device 102. In even other aspects, the robot device 102 may include a controller which implements a part of the controller 108 and another (central) controller may include another part of the controller 108 described herein.

[0030] The term controller may be understood as any type of logic implementing entity, which may include, for example, a circuit and/or a processor capable of executing software stored in a storage medium, firmware, or a combination thereof, and which can issue instructions, e.g. to an actuator in the present example. The controller may be configured, for example, by program code (e.g., software) to control the operation of a system, a robot in the present example.

[0031] In the present example, the controller 108 may include one or more processors 110 and a memory 112 storing code and data based on which the processor 110 controls the robot device 102. According to various embodiments, the controller 108 controls the robot device 102 on the basis of a control model (e.g. employing a model predictive control, MPC) 114 stored in the memory 112.

[0032] To be able to navigate the robot device 102 in its (dynamic) environment, the controller 108 may use sensor data which represent the environment and, thus, the surrounding of the robot device 102. As an example, the sensor data may include images of the surrounding of the robot device 102 which are provided by one or more imaging sensors 116. At least one of the one or more imaging sensors 116 may be attached to the robot device 102 and/or at least one of the one or more imaging sensors 116 may be separate from the robot device 102 (e.g., to have a view for observing more than one robot device 102.

[0033] An imaging sensor, as used herein, may be, for example, a camera (e.g., a standard camera, a digital camera, an infrared camera, a stereo camera, etc.), a radar sensor, a LIDAR sensor, an ultrasound sensor, etc. Thus, an image may be an RGB image, an RGB-D image, or a depth image (also referred to as a D image). A depth image described herein may be any type of image that includes depth information. Illustratively, a depth image may have 3-dimensional information about one or more objects. For example, a depth image described herein may include a point cloud provided by a LIDAR sensor and/or a radar sensor. For example, a depth image may be an image with depth information provided by a LIDAR sensor.

[0034] It is understood that the one or more imaging sensors 116 serve as examples and that the robot device arrangement 100 may include any other kind of one or more perception sensors.

[0035] The controller 108 may be configured to control the robot device 102 based on an output of the control model 114 responsive to inputting at least one image into the control model 114.

[0036] As detailed herein, the dynamic environment may include one or more humans 106. FIG. 2 shows a flow diagram of a (e.g., computer-implemented) method 200 for controlling a robot device (e.g., the robot device 102) according to various aspects. In order to prohibit a collision between the robot device 102 and the one or more humans 106, the method 200 considers a respective behavior (e.g., motion, activity, etc.) of each of the one or more humans 106 when navigating the robot device 102. In particular, the method 200 employs, in addition to predicted geometric information of the human motion (e.g., their predicted motion), (additional) contextual information of the humans, such as their body pose, their velocity, their (current) activity, etc.

[0037] The method 200 may include (in 202) determining (using the sensor data 302 which represent a surrounding of the robot device 102), whether there are one or more humans 106 in the surrounding of the robot device 102.

[0038] In the case that it is determined that there are one or more humans 106 in the surrounding of the robot device 102, the method 200 may include (in 204) determining for each human of the one or more humans 106: a respective body pose (e.g., a skeleton pose) and a respective velocity of the human (in 204A), a respective predicted motion of the human (e.g., as multi-modal geometric information, such as a trajectory) for a future time period (in 204B), and (using the respective body pose, the respective velocity, and the respective predicted motion) a respective occupation area occupied by the human in the future time period (in 204C).

[0039] The method 200 may include (in 206) generating control parameters (e.g., a control signal) for controlling the robot device 102 such that a predefined (minimum) distance of the robot device 102 to the respective occupation area of each of the one or more humans 106 is ensured in the future time period. By this, a collision risk of the robot device 102 with the one or more humans 106 is significantly reduced.

[0040] The method 200 may include (in 208) controlling (e.g., navigating) the robot device 102 in accordance with the control parameters

[0041] In the following, various aspects of the method 200 are described in further detail. FIG. 3 shows an exemplary flow chart 300 for controlling the robot device 102 according to various aspects.

[0042] The method 200 may include a determination of contextual information 304 using the sensor data 302. The contextual information 304 may include a respective predicted motion 306 of each of the one or more humans 106 (e.g., as multi-modal geometric information as detailed in reference [1]). The contextual information 304 may further include a respective body pose 308 (e.g., a skeleton pose) and a respective velocity 310 of each of the one or more humans 106. Optionally, the contextual information 304 may further include a respective activity 312 of each of the one or more humans 106. For example, the controller 108 may include a human motion prediction module configured to determine, using the sensor data 302, the predicted motion 306 of each human of the one or more humans 106. For example, the controller 108 may include a perception module configured to determine, using the sensor data 302, the respective body pose 308, the respective velocity 310, and optionally further the respective activity 312 of each of the one or more humans 106.

[0043] A body pose, as detailed herein, may be a three-dimensional body pose, such as a skeleton pose. A skeleton pose may include joints of the entire body of the associated human. Exemplary skeleton poses are shown in FIG. 5.

[0044] The controller 108 may implement a robot navigation stack 314. The robot navigation stack 314 may include a local planning model 316 (short: local planner) which is configured to implement the context-aware MPC detailed herein. The local planning model 316 may interact with a global planning model 318 (short: global planner) which is configured to consider the respective navigation of a plurality of robot devices. The local planning model 316 and/or the global planning model 318 may employ at least part of the sensor data 302 and/or additional sensor data for decision making.

[0045] The local planning model 316 may implement the context-aware MPC approach based on equation (1):

[00001]$\begin{array}{cc}\underset{x(.Math.),u(.Math.)}{\min }{.Math.}_{n=0}^{N1}\left(J_{\mathrm{stage}}\left(x\left(n\right),u\left(n\right),g\left(n\right),o\left(.Math.,n\right)\right)\right)+\left(J_{\mathrm{terminal}}\left(x\left(N\right),g\left(N\right),o\left(.Math.,N\right)\right)\right),& \left(1\right)\end{array}$

being subject to:

[00002]$\begin{array}{cc}x\left(n\right)X& n\left[0,N\right]\end{array}$$\begin{array}{cc}u\left(n\right)U& n\left[0,N-1\right]\end{array}$$\begin{array}{cc}d\left(p_{x\left(n\right)},o\left(i,n\right)\right)d_{h\mathrm{uman}\_\min }& n\left[0,N\right];i\left[1,N_{humans}\right]\end{array}$$\begin{array}{cc}d\left(p_{x\left(n\right)},o_s\right)-s{d}_{s\_\min }& n\left[0,N\right],\end{array}$

wherein n indicates a respective point in time in the future time period, N is the prediction horizon of the future time period, x(n) is the robot state at time n, X is the allowed robot states, u(n) is the robot control at time n, U is the allowed control spaces, g(n) is the goal at time n, i indicates a respective human of a total number, N.sub.humans, Of the one or more humans 106, o(i,n) is the position of human i at time n, p.sub.x(n) is the robot position at time n, o.sub.s is the position of the nearest static obstacle, d.sub.s_min is the constraint of the predefined minimum distance to the nearest static obstacle, and d.sub.human_min is the predefined (minimum) distance of the robot device 102 to each of the one or more humans, i, 106.

[0046] The cumulative stage cost J.sub.stage may be given by equation (2):

[00003]$\begin{array}{cc}{J}_{stage}\left(x\left(n\right),u\left(n\right),g\left(n\right),o\left(.Math.,n\right)\right)=J_g\left(x\left(n\right),g\left(n\right)\right)+J_u\left(u\left(n\right)\right)+J_{col}\left(x\left(n\right),o\left(.Math.,n\right)\right),& \left(2\right)\end{array}$

wherein J.sub.g is the goal cost term representing the cost to goal, J.sub.u is the control cost term representing the control effort, and J.sub.col is the collision cost used to avoid a collision of the robot device 102 with the surrounding obstacles and the one or more humans 106.

[0047] The terminal cost J.sub.terminal may be given by equation (3):

[00004]$\begin{array}{cc}{J}_{\mathrm{terminal}}\left(x\left(N\right),g\left(n\right),o\left(.Math.,N\right)\right)=J_g\left(x\left(N\right),g\left(N\right)\right)+J_{col}\left(x\left(N\right),o\left(.Math.,N\right)\right).& \left(3\right)\end{array}$

[0048] Penalizing the distance to the given goal may be achieved by the goal cost term

[00005]$J_g\left(x\left(n\right),g\left(n\right)\right)=.Math.x\left(n\right)-{g\left(n\right)}_{w_g}^2;n\left[0,N\right]$

weighted by the diagonal matrix w.sub.g. The control may be penalized using the control cost term

[00006]$J_u\left(u\left(n\right)\right)={.Math.u\left(n\right).Math.}_{w_u}^2;n\left[0,N-1\right]$

weighted by the diagonal matrix w.sub.g.

[0049] According to various aspects, the collision cost term J.sub.col may take the predicted motion306, the respective body pose 308, and the respective velocity 310 of each of the one or more humans, i, 106 into account. The collision cost term J.sub.col may be given by equation (4):

[00007]$\begin{array}{cc}{J}_{col}\left(x\left(n\right),o\left(.Math.,n\right)\right)={.Math.}_{i=1}^{N_{\mathrm{humans}}}f\left(d\left(p_{x\left(n\right)},o\left(i,n\right)\right)\right),& \left(4\right)\end{array}$

with f(d(p.sub.x(n), o(i,n))) being defined as:

[00008]$\{\begin{array}{cc}\left(-\frac{kq}{4}\right)d\left(p_{x\left(n\right)},o\left(i,n\right)\right)+\left(\frac{q}{2}+\frac{kq}{4}{r}_{\mathrm{th}}\right)& \mathrm{if}d\left(p_{x\left(n\right)},o\left(i,n\right)\right)r_{\mathrm{th}}\\ \frac{q}{1+e^{k\left(d\left(p_{x\left(n\right)},o\left(i,n\right)\right)-r_{\mathrm{th}}\right)}}& \mathrm{if}d\left(p_{x\left(n\right)},o\left(i,n\right)\right)>r_{\mathrm{th}}\end{array}$

with r.sub.th being a predefined threshold distance, k being a (predefined) smoothness parameter, and q=2 r.sub.th.

[0050] As detailed in the following, the distance d(p.sub.x(n), o(i, n)) between the robot device 102 and a respective human, i, of the one or more humans 106 may be determined using the respective body pose 308 and the respective velocity 310 of the human i.

[0051] The controller 108 may be configured to determine a respective occupation area 320 occupied by the human i in the future time period (from n=0 to n=N) using the respective body pose 308 and the respective velocity 310 of the human i. For this, the controller 108 may determine, using the respective body pose 308 of the human i, a respective body pose area occupied by the human i and may then increase this respective body pose area as a function respective velocity of the human i. The respective body pose area may be increased with increasing velocity of the human i.

[0052] The respective body pose area occupied by the human i may be a two-dimensional ellipse determined by projecting a respective two-dimensional ellipse onto the ground on which the robot device 102 is positioned such that the two-dimensional ellipse bounds the respective body pose 308 of the human i in a plan view. Illustratively, the respective body pose area may be a two-dimensional ellipse having a minimum size to bound (e.g., include) all points of the respective body pose 308. For example, the respective body pose 308 may be a skeleton pose including the joints of the human body; these joints may be projected to the ground and then the respective two-dimensional ellipse may be determined to bound these projected joints.

[0053] The size of this two-dimensional ellipse may then be increased as a function of the respective velocity 310 of the human i, thereby determining the respective occupation area 320 occupied by the human i.

[0054] As an example, a linear velocity v.sub.h of the human i may be determined using a velocity in x-direction v.sub.hx and a velocity in y-direction v.sub.hy by:

[00009]$v_h=\sqrt{v_{hx}^2+v_{hy}^2}.$

In this example, an ellipse axis enlargement e may be determined by: e=|tan h(1.5v.sub.h)|. Both, the major axis and the minor axis of the two-dimensional ellipse, may then be increased by the ellipse axis enlargement e. It is understood that this serves as an example and that the increase of the two-dimensional ellipse may be determined using any other suitable function. However, it is found that using the tan h provides a significant reduction of the collision risk between the robot device 102 and the human i.

[0055] The distance d(p.sub.x(n), o(i, n)) between the robot device 102 and the human i may then be a minimum distance between the position p.sub.x(n) of the robot device 102 and the two-dimensional ellipse. With reference to FIG. 4, the ellipse point c.sub.e represents the point on the perimeter of the two-dimensional ellipse which is closest to the robot position p.sub.x(n) at an exemplary at time n. For human, the ellipse point c.sub.e at time n may then correspond to o(i,n). The robot device 102 may be positioned at robot position p.sub.x(n) with a robot pose r=[r.sub.x,r.sub.y]. The human i may be positioned within the respective occupation area 320 given by the two-dimensional ellipse with ellipse center c=[cx, cy].

[0056] To determine the ellipse point c.sub.e, the coordinate system may be shifted and rotated such that the ellipse is centered in the origin and that the major axis, a, and the minor axis, b, of the ellipse is in line with the x-axis and the y-axis; respectively. Then, a new robot pose r.sub.new may be determined by:

[00010]$r_{new}=R^T\left(r-c\right)\mathrm{with}R=\left[\begin{array}{cc}\cos \left(\right)& -\sin \left(\right)\\ \sin \left(\right)& \cos \left(\right)\end{array}\right]$

and being the rotation angle.

[0057] The human-specific (minimum) distance d(p.sub.x(n), o(i, n)) may be determined by optimizing the square distance D.sup.2 between the ellipse and the robot pose by: D.sup.2=(c.sub.er.sub.new)(c.sub.er.sub.new)(using Newton's method).

[0058] Using the human-specific (minimum) distance d(p.sub.x(n), o(i, n)) based on the respective occupation area 320 reduces the risk that the robot device 102 collides with a human of the one or more humans 106 and also reduces discomfort of the humans since the robot device 102 avoids them.

[0059] According to various aspects, a velocity cost J.sub.vel may be added to the cumulative stage cost J.sub.stage and the terminal cost J.sub.terminal giving:

[00011]$\begin{array}{cc}{J}_{\mathrm{stage}}\left(x\left(n\right),u\left(n\right),g\left(n\right),o\left(.Math.,n\right)\right)=J_g\left(x\left(n\right),g\left(n\right)\right)+J_u\left(u\left(n\right)\right)+J_{col}\left(x\left(n\right),o\left(.Math.,n\right)\right)+J_{vel}\left(x\left(n\right),o\left(.Math.,n\right)\right),& \left(5\right)\end{array}$$\begin{array}{cc}{J}_{\mathrm{terminal}}\left(x\left(N\right),g\left(n\right),o\left(.Math.,N\right)\right)=J_g\left(x\left(N\right),g\left(N\right)\right)+J_{col}\left(x\left(N\right),o\left(.Math.,N\right)\right)+J_{vel}\left(x\left(N\right),o\left(.Math.,N\right)\right)).& \left(6\right)\end{array}$

[0060] The velocity cost J.sub.vel may include the respective activity 312 of each human i of the one or more humans 106. The controller 108 may be configured to determine the respective activity 312 of a human i using the sensor data 302 (e.g., using the respective body pose 308). An activity 312 may be associated with a corresponding activity factor, a.sub.i, representing a degree or level of activity. In an example, the activity factor, a.sub.i, may be a.sub.i[0,1] with a higher value of a.sub.i representing a higher level of activity. For example, an activity factor, a.sub.i, associated with sitting may be lower than an activity factor, a.sub.i, associated with walking. It is understood that this serves as an example and that the relationship may be vice versa. FIG. 5 shows an exemplary skeleton pose 502 for the activity standing, an exemplary skeleton pose 504 for the activity walking, and an exemplary skeleton pose 506 for the activity sitting. The activity factor, a.sub.i, associated with a corresponding activity may be predefined (e.g., offline) and/or may be learning during use of the robot device 102.

[0061] The velocity cost J.sub.vel may be given by J.sub.vel(x(n), o(.Math., n)=w.sub.h|v.sub.rv.sub.des|.sup.2 weighted by the diagonal matrix w.sub.h, wherein v.sub.r is the robot velocity and v.sub.des is the desired robot velocity.

[0062] The controller 108 may be configured to determine an average level of human activity, A, in the surrounding of the robot device 102 using the respective activity factor, a.sub.i, of each of the one or more humans 106. For example, the average level of human activity, A, may be an average of the activity factor, a.sub.i, of the one or more humans 106 given by:

[00012]$A=\frac{1}{N_{human}}\underset{i=0}{\overset{N_{human}}{.Math.}}{}_i$

[0063] Illustratively, the average level of human activity A may be a local human activity in the surrounding of the robot device 102.

[0064] In the case that the one or more humans 106 are present in the surrounding of the robot device 102, the desired robot velocity v.sub.des may be limited by a socially acceptable velocity v.sub.social,max (e.g., being half a maximum velocity of the robot device 102). The desired robot velocity v.sub.des may be determined by: v.sub.des=(1A).Math.v.sub.social,max.

[0065] Illustratively, the velocity cost J.sub.vel moderates the robot velocity as a function of human activity in the surrounding of the robot device 102. Thus, the robot device 102 decelerates based on the average level of human activity. This also reduces the risk that the robot device 102 collides with a human of the one or more humans 106 and also reduces the discomfort of the humans since the robot device 102 decelerates when being closer to them.

[0066] The controller 108 may be configured to generate the control parameters 322 (e.g., as a control signal) using equation (1). The controller 108 may determine the human-specific (minimum) distance d(p.sub.x(n), o(i,n)) employing the respective occupation area 320 as detailed above and/or equation (1) may include the velocity cost term J.sub.vel according to equations (5) and (6). According to various aspects, the control parameters 322 may represent a robot trajectory. The robot trajectory may be determined using the software ACADOS for optimization. The method 200 allows the robot device 102 to generate smoother, safer and less conservative trajectories which are also more legible by the humans.

[0067] While in the above embodiments, the approach of FIG. 2 is applied to control the robot device 102, it may be applied to any other kind of robot device.