Method and device for socially aware model predictive control of a robotic device using machine learning

Abstract

A computer-implemented method for determining a control trajectory for a robotic device. The method includes: performing an information theoretic model predictive control applying a control trajectory sample prior in each time step to obtain a control trajectory for a given time horizon; determining the control trajectory sample prior depending on a data-driven trajectory prediction model which is trained to output a control trajectory sample as the control trajectory sample prior based on an actual state of the robotic device.

Claims

1. A computer-implemented method for determining a control trajectory for a robotic device, the method comprising the following steps: performing an information theoretic model predictive control applying a control trajectory sample prior in time steps to obtain the control trajectory for a given time horizon; determining the control trajectory sample prior depending on a data-driven trajectory prediction model which is trained to output a control trajectory sample as the control trajectory sample prior based on an actual state of the robotic device; wherein the control trajectory sample prior is obtained by a modelled control trajectory sample obtained using the trajectory prediction model and the control trajectory sample obtained in a last time step; wherein the control trajectory sample prior is obtained as a sum of the modelled control trajectory sample obtained by the trajectory prediction model and the control trajectory sample obtained in a last time step, each weighted according to their trajectory costs according to a given cost function; wherein the modelled control trajectory sample is only considered when the trajectory costs of the modelled control trajectory sample are higher than the trajectory costs of the control trajectory sample obtained in the last time step; and controlling the robotic device to act according to the determined control trajectory.

2. The method according to claim 1, wherein the information theoretic model predictive control iteratively evaluates a number of control trajectory samples derived from the control trajectory sample prior based on a given distribution at each time step to obtain a further control trajectory sample, wherein the further control trajectory sample is determined depending on a combination of a number of weighted control trajectory samples, and wherein weights of the weighted control trajectory samples are determined based on costs of each of the number of weighted control trajectory samples.

3. The method according to claim 1, wherein the data driven trajectory prediction model includes a machine learning mode, including a neural network, of a type of one of a soft actor-critic network, or a trust region network, or a policy optimization network, or a proximal policy optimization network, or a deep deterministic policy gradients network.

4. The method according to claim 1, wherein the robotic device is controlled to act according to the determined control trajectory.

5. A control unit configured to determining a control trajectory for a robotic device, the control unit configured to: perform an information theoretic model predictive control applying a control trajectory sample prior in time steps to obtain the control trajectory for a given time horizon; determine the control trajectory sample prior depending on a data driven trajectory prediction model which is trained to output a control trajectory sample as the control trajectory sample prior based on an actual state of the robotic device; wherein the control trajectory sample prior is obtained by a modelled control trajectory sample obtained using the trajectory prediction model and the control trajectory sample obtained in a last time step; wherein the control trajectory sample prior is obtained as a sum of the modelled control trajectory sample obtained by the trajectory prediction model and the control trajectory sample obtained in a last time step, each weighted according to their trajectory costs according to a given cost function; wherein the modelled control trajectory sample is only considered when the trajectory costs of the modelled control trajectory sample are higher than the trajectory costs of the control trajectory sample obtained in the last time step.

6. The control unit as recited in claim 5, wherein the control unit is a data processing device.

7. A robotic device, comprising: an actuation unit configured to move the robotic device according to a control trajectory; a control unit configured to: perform an information theoretic model predictive control applying a control trajectory sample prior in time steps to obtain the control trajectory for a given time horizon; determine the control trajectory sample prior depending on a data driven trajectory prediction model which is trained to output a control trajectory sample as the control trajectory sample prior based on an actual state of the robotic device; wherein the control trajectory sample prior is obtained by a modelled control trajectory sample obtained using the trajectory prediction model and the control trajectory sample obtained in a last time step; wherein the control trajectory sample prior is obtained as a sum of the modelled control trajectory sample obtained by the trajectory prediction model and the control trajectory sample obtained in the last time step, each weighted according to their trajectory costs according to a given cost function; wherein the modelled control trajectory sample is only considered when the trajectory costs of the modelled control trajectory sample are higher than the trajectory costs of the control trajectory sample obtained in the last time step.

8. A non-transitory machine readable medium on which are stored instructions for determining a control trajectory for a robotic device, the instructions, when executed by a computer, causing the computer to perform the following steps: performing an information theoretic model predictive control applying a control trajectory sample prior in time steps to obtain the control trajectory for a given time horizon; determining the control trajectory sample prior depending on a data-driven trajectory prediction model which is trained to output a control trajectory sample as the control trajectory sample prior based on an actual state of the robotic device; wherein the control trajectory sample prior is obtained by a modelled control trajectory sample obtained using the trajectory prediction model and the control trajectory sample obtained in a last time step; wherein the control trajectory sample prior is obtained as a sum of the modelled control trajectory sample obtained by the trajectory prediction model and the control trajectory sample obtained in a last time step, each weighted according to their trajectory costs according to a given cost function; wherein the modelled control trajectory sample is only considered when the trajectory costs of the modelled control trajectory sample are higher than the trajectory costs of the control trajectory sample obtained in the last time step.

9. The control unit as recited in claim 5, wherein the control unit is further configured to control the robotic device to act according to the determined control trajectory.

10. The non-transitory machine readable medium as recited in claim 8, wherein the instructions, when executed by the computer, further cause the computer to perform the following step: controlling the robotic device to act according to the determined control trajectory.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 shows a mobile robot in a crowded environment, in accordance with an example embodiment of the present invention.

(2) FIG. 2 shows a flowchart for an informed IT-MPC algorithm, in accordance with an example embodiment of the present invention.

(3) FIG. 3 shows a flowchart of a subroutine regarding the importance sampling weights function of the informed IT-MPC algorithm, in accordance with an example embodiment of the present invention.

(4) FIG. 4 shows a flow chart for the informed control function which generates a control trajectory sample prior based on a trained trajectory prediction model, in accordance with an example embodiment of the present invention.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

(5) FIG. 1 shows a system of a robotic device 1 as a mobile robot having a task to move through/within an environment E from position A to position B along a planned trajectory. In the environment E dynamic objects 2, 3, 4 may move performing their own tasks. The dynamic objects 2, 3, 4 may be further mobile robots or individuals each showing a kind of intelligent task driven behavior. The intelligent behavior of the dynamic objects 2, 3, 4 may follow a policy considering actions of the robotic device 1 and/or other dynamic objects 2, 3, 4.

(6) The mobile robot may be an autonomous cleaning robot, an autonomous lawn mower, an autonomous service or care robot or the like. In other embodiments, the robotic device may be a robot with a manipulator, such as a robot arm, wherein the the manipulating device, such as a robot gripper, shall follow a planned trajectory.

(7) The robotic device 1 may have a configuration as it is schematically shown in FIG. 1. The robotic device 1 has a control unit 11 that is configured to perform the subsequently described method and to control movement of the robotic device 1 along a planned trajectory among others.

(8) The control unit 11 may have a microprocessor or a microcontroller as well as a memory for storing data and an algorithm code. Furthermore, the robotic device 1 has an actuation unit 12 for interaction with the environment, for instance the actuation unit 12 may include a traction motor for driving wheels of the robotic device 1 to move the robotic device 1 within the environment E. The actuation unit 12 may also include a controllable robot arm with a gripper or the like. The actuation unit 12 is controlled by the control unit 11.

(9) Furthermore, the robotic device 1 may include a sensor system 13 for sensing the environment E, particularly to detect other objects and structures which may allow the localization of the robotic device 1 and of the dynamic objects 2, 3, 4 in the environment E such that their positions or their poses are known in the environment E. The sensor system 13 may include radar, Lidar, and/or imaging systems, such as one or more cameras to scan the environment E surrounding the robotic device 1.

(10) When configuring such a robotic device 1, a control strategy has to be installed which allows the robotic device 1 to autonomously perform its task in the environment E. The control strategy should have implemented a collision avoidance/handling to prevent motion trajectories where a collision with another dynamic object is likely or to cope with collisions.

(11) In simulation-based benchmarking of navigation algorithms for robotic devices, it is generally difficult to effectively model the behavior of the dynamic objects 2.

(12) This behavior of each of the dynamic objects may contribute to or disturb the performing of the task of the robotic device 1.

(13) Basically, in the control unit 11 of the robotic device 1, an informed information theoretic model predictive control (IT-MPC) approach is implemented and performed. The informed information theoretic model predictive control (IT-MPC) process considers stochastic non-linear discrete time systems of the form of x.sub.t+1=F(x.sub.t, v.sub.t), where x.sub.t∈.sup.n is the system state at time t of dimension n. v.sub.t∈.sup.m corresponds to the input control variable of dimension m with white noise obliged to nominal control u.sub.t i.e. v.sub.t=u.sub.t+ε.sub.t with ε.sub.t˜(0,Σ), wherein (ν.sub.0, ν.sub.1, . . . , ν.sub.t.sub.f.sub.−1)=V∈.sup.m×t.sup.f corresponds to the input sequence and (u.sub.0, u.sub.1, . . . , u.sub.t.sub.f.sub.−1)=U∈.sup.m×t.sup.f to the input with its disturbance (ε.sub.0, ε.sub.1, . . . , ε.sub.t.sub.f.sub.−i)=ε.

(14) The goal of IT-MPC is to solve the stochastic optimal control problem of the form

(15) $U^{*}\approx \underset{U\in 𝒰}{\arg \min }{E}_{{U,.Math.}_{}}(\stackrel{t_{f-1}}{\underset{t=0}{.Math.}}c\left(x_t\right)+\varphi \left(x_{t_f}\right)+\lambda u_{t-1}^T\stackrel{-1}{.Math.}{u}_t$
where is the set of possible input sequences for the system, c(x.sub.t) is a state-based cost, ϕ(x.sub.t.sub.f) is a terminal cost and a rate λ>0. The optimal control distribution *can be derived as

(16) $q^{*}\left(V\right)=\frac{1}{\eta }\exp (-\frac{1}{\lambda }S\left(V\right)p\left(V.Math.0,.Math.\right)$
where S(V) denotes the state cost of an entire trajectory, p may be a Gaussian with covariance Σ and η is a normalization factor. To approximate the above minimization problem, it is proposed to minimize the KL divergence between the optimal control distribution * and .sub.U,Σ
U*≈argmin .sub.KL(*,.sub.U,Σ)

(17) In case the optimal control distribution is Gaussian, the approximation is correct, and the resulting control trajectory is optimal. Given samples V.sub.k around a nominal control trajectory .Math. as described above with disturbances ε.sub.k above approximative minimization problem can be solved by an importance sampling scheme

(18) $U^{*}\approx \hat{U}+\stackrel{K}{\underset{k=1}{.Math.}}{w}_k{\epsilon }_k$
where w.sub.k are importance sampling weights. It is preferable to use above formula as an update rule in an iterative manner.

(19) A modified informed IT-MPC algorithm is described in conjunction with the flow chart of FIG. 2 and the pseudocode as follows:

(20) TABLE-US-00001 Algorithm: Informed IT-MPC algorithm.  1. Input: u*: Initial control sequence, K: Number of samples, t.sub.f: Time horizon, F: Transition model, ϕ, c: State cost, Σ, λ: Hyper-parameter, C decoder conditions  2 . While task not completer do  3.  x.sub.0 ← GetStateEstimate( )  4 .    U* ← InformControls (x0, u*, C, Σ, ϕ, c, F)  5.    for k ← 0 to K-1 do  6.   x ← x.sub.0  7 .     S.sub.k ← 0  8.   ε ← (ε.sub.0.sup.k, ..., ε.sub.t.sub.f.sub.−1.sup.k), ε.sub.t.sup.k ∈ (0, Σ)  9.      For t ← 1 to t.sub.f: do 10      x ← F (x, u.sub.t−1 + (ε.sub.t−1.sup.k) 11      $S_k\leftarrow S_k+c\left(x\right)+\lambda u_{t-1}^T\stackrel{-1}{.Math.}{\epsilon }_{t-1}^k$ 12       end for 13     S.sub.k ← S.sub.k + ϕ(x) 14     w.sub.k ← ImportanceSamplingWeights(S.sub.k, λ) 15     $U^{*}\leftarrow U^{*}+\underset{k\leftarrow 1}{\overset{K}{.Math.}}{w}_k{\epsilon }_k$ 16      end for 17      ApplyControl (u.sub.0.sup.*) 18      For t ← 1 to t.sub.f − 1 do 19     u.sub.t−1.sup.* ← u.sub.t.sup.* 20.      end for 21.      u.sub.t.sub.f.sub.−1.sup.* ← Initialize(u.sub.t.sub.f.sub.−1.sup.*) 22 .  End while 23.

(21) According to step S1 (line 1), an initial configuration of parameters of the process is given. So, u* indicates an initial control sequence, K a number of samples, t.sub.f a time horizon, F a transition model, ϕ terminal costs, c state costs, Σ,λ hyper-parameters and C decoder conditions.

(22) A control loop is started in step S2 which is performed between lines 2 to 22 of pseudocode of the Informed-IT-MPC algorithm which may be terminated once the goal state is reached by the robotic device 1.

(23) In step S2 (line 3), the state of the system is obtained by the function “GetStateEstimate”. The state of the system may be determined by processing sensor signals to obtain a pose of the robotic device 1 in/relative to the environment E and the poses and velocities of each of dynamic obstacles 2, 3, 4 in the environment E as well known in the art. Basically, this is accomplished by reading the sensor signals such as Lidar, radar and the like which are further processed to obtain the respective.

(24) In step S3 (line 4), a control trajectory sample prior is obtained by the algorithm “InformControls” which is described below in conjunction with FIG. 4.

(25) Based on the control trajectory sample prior U*, a loop between lines 5 and 16 is performed for each of the samples k.

(26) In step S4 (lines 6 to 8) x, S.sub.k and ε.sub.k are initialized.

(27) In step S5, starting with the control trajectory prior U* as obtained in step S3, but in further iterations modified control trajectory prior U*, movement costs are predicted. This is performed by predicting future states of the system (line 10) with the time horizon t.sub.f, and the corresponding movement costs S.sub.k are accumulated (line 11). The so obtained movement costs are added to the terminal costs ϕ(x) in line 13.

(28) In step S6 (line 14), importance sampling weights w.sub.k are obtained by an algorithm “ImportanceSamplingWeights” as shown in the flowchart of FIG. 3 and the following pseudocode:

(29) TABLE-US-00002 Algorithm: ImportanceSamplingWeights (S.sub.k, λ).. 1: Input: S.sub.k, λ 2 : ρ ← min.sub.k (S.sub.k) 3 : $\eta \leftarrow \underset{k=0}{\overset{K-1}{.Math.}}\exp \left(-\frac{1}{\lambda }\left(S_k-\rho \right)\right)$ 4 : for k ← 0 to K − 1 do 5:  $w_k\leftarrow \frac{1}{\eta }\exp \left(-\frac{1}{\lambda }\left(S_k-\rho \right)\right)$ 6: end for 7 : return w.sub.k

(30) The importance sampling weights w.sub.k are determined based on the accumulated trajectory costs S.sub.k of each control trajectory sample and the given λ.

(31) The function to determine the importance sampling weights uses the minimum costs ρ of all control trajectory samples considered so far in step S11 (line 2). In step S12 (line 3), a normalization factor

(32) $\eta \leftarrow \stackrel{K-1}{\underset{k=0}{.Math.}}\exp (-\frac{1}{\lambda }\left(S_k-\rho \right)$
is calculated for making all movement costs of the considered samples comparable. In step S13 (lines 4 to 6), importance sampling weights w.sub.k for each of the control trajectory samples (determined so far) are obtained depending on a difference between the respective control trajectory costs S.sub.k and the minimum control trajectory costs over all samples. Loop of lines 4 and 6 is necessary, as only S.sub.k is needed.

(33) Basically, the importance sampling weights w.sub.k are the higher, the higher the difference between the control trajectory sample costs and the minimum control trajectory sample costs ρ. The importance sampling weights w.sub.k for S.sub.k is returned to the Informed IT-MPC algorithm in line 7.

(34) In step S7 (line15), based on the importance sampling weights w.sub.k, a further control trajectory sample U* is obtained for the loop between lines 5 and 16.

(35) In step S8 it is queried if all samples are considered. If positive (alternative: yes), the process is continued with step S9, otherwise (alternative: no) the process is continued with step S4.

(36) In step S9 (line 17) of the informed IT-MPC algorithm, the first control data (u*.sub.0) is applied to the robotic device 1. The predicted trajectory is updated to the following time step in step S10 (lines 18 to 20) so that (u*.sub.0) is set to the former U*.sub.1 and so on.

(37) In step S10 it is queried if the task is completed. If positive (alternative: yes), the process is ended, otherwise (alternative: no) the process is continued with step S2.

(38) In step S3 (line 4), a control trajectory sample prior is obtained by the algorithm which is described below in conjunction with FIG. 4 and the following pseudocode:

(39) TABLE-US-00003 Algorithm InformControls (u*, σ, φ.sub.0, v.sub.0, Σ, ϕ, q, F).  1: Input: u*, σ, φ.sub.0, v.sub.0, Σ, ϕ, q, F  2: û = DeepReinforcementLearning (σ, φ.sub.0, v.sub.0)  3: for t ← 1 to t.sub.f do  4 :  {circumflex over (x)} = F(x, g (û.sub.t−1) )  5:  Ŝ.sub.k = Ŝ.sub.k + c({circumflex over (x)}) + λû.sub.t−1.sup.T Σ.sup.−1 û.sub.t−1  6:  x* = F(x,g(u.sub.t−1*))  7:  S.sub.k.sup.* = S.sub.k* + c(x*) + λu.sub.t−1*.sup.T Σ.sup.−1 u.sub.t−1*  8 : end for  9: Ŝ = Ŝ + ∅ ({circumflex over (x)}) 10: S* = S* + ∅(x*) 11: if Ŝ > S*then 12 :  $u^{*}=\left(1-\frac{\hat{S}}{\hat{S}+}\right)\hat{u}+\left(1-\frac{\hat{S}}{\hat{S}+}\right)u^{*}$ 13: end if 14 : return u*

(40) The control trajectory sample prior u* is generated based on the actual state of the system x, the trajectory control set of the last time step u*, initial conditions needed by the network, such as some sensor inputs σ, the robotic device 1 current state φ.sub.0, ν.sub.0, a covariance metric Σ, terminal costs ϕ, state costs c and the transition model F.

(41) In step S21 (line 2), a modelled control trajectory sample û is determined based on a data-driven trajectory prediction model (function call: DeepReinforcementLearning (σ, φ.sub.0, ν.sub.0)) which may be implemented as a machine learning model such as a neural network or the like. Inputs for the trajectory prediction model are σ, φ.sub.0, ν.sub.0 indicating the actual state of the system including the pose of the robotic device 1 and a state of the environment E.

(42) The neural network can use different types, such as soft actor-critic, trust region, policy optimization, proximal policy optimization, deep deterministic policy gradients or the like.

(43) The trajectory prediction model is trained to generate the modelled control trajectory sample based on the actual state of the system and has been trained by optimized control trajectories. To obtain the optimized control trajectories classical deterministic MPC techniques can be used such as described in Schoels, T. et al., “An NMPC Approach using Convex Inner Approximations for Online Motion Planning with Guaranteed Collision Freedom”, arXiv preprint arXiv:1909.08267, 2019.

(44) In step S22 along the given time horizon, based on the actual state of the system x, the costs Ŝ of the modelled control trajectory sample û predicted by the trajectory prediction model including the respective terminal costs at the end of the time horizon is determined (lines 4, 5 and 9)

(45) In step S23 along the given time horizon, based on the actual state of the system x, the costs S* of the further trajectory control sample of the last time step u* including the respective terminal costs at the end of the time horizon is determined (lines 6, 7 and 10)

(46) In step S24 (line 12), the control trajectory sample prior u* for returning to step S4 of the main algorithm Alg1 is determined by a combination of the modelled control trajectory u and the further trajectory control sample u* of the last time step as determined in step S7.

(47) For instance, the returned control trajectory sample prior u* can be generated from the lastly generated further control trajectory sample u* and the modelled control trajectory sample û. If the cost Ŝ of the modelled control trajectory sample û is higher than the costs S* of the lastly generated control trajectory sample u*, the returned control trajectory sample u* is generated according to line 12 based on a sum of the weighted control trajectories, wherein the weights are generated by the corresponding costs Ŝ and S*.

(48) Otherwise the further control trajectory sample u* as determined by step S7 of the main algorithm is used for the following iteration.

(49) The above algorithm aims to find a control trajectory distribution that minimizes the expected costs. The control trajectory distribution depends strongly on the cost function and on the prediction of the trajectory prediction model. The sampling-based approach is even more accurate, if the initial control trajectory sample distribution is already close to the optimal one. Instead of a naïve initialization such as a zero or constant velocity of the obstacles, the control trajectory sample distribution is guided towards low-cost areas of the state space. While using the data driven trajectory prediction model, control trajectory samples may be generated that optimize the original objective.