Method and apparatus for social perception model predictive control of robotic devices

Abstract

This invention relates to a method and apparatus for predictive control of a robot device using a social perception model. The invention relates to a computer-implemented method for determining the control trajectory of a robot device (1), comprising the following steps: - performing (S1-S8) information theory model predictive control, thereby applying prior control trajectory samples at each time step ( u * ) to obtain a given time range ( t f The control trajectory depends on a data-driven trajectory prediction model to determine (S21) the control trajectory sample prior. u * The trajectory prediction model is trained to output control trajectory samples based on the actual state of the robot device (1) as the prior of the control trajectory samples. u * ).

Description

Technical Field

[0001] This invention relates to model predictive control for planning the trajectory of robotic devices. Background Technology

[0002] For the autonomous control of robotic devices, motion planning is used to efficiently complete navigation tasks. Based on sensor input, the actual state of the robotic device and its environment is determined, and the trajectory or motion path is developed separately, taking into account dynamic obstacles (such as moving objects or individuals in the environment).

[0003] Generally speaking, model predictive control (MPC) is an efficient technique for solving open-loop optimal control problems based on receding horizon models. Classical model predictive control works well when the objective is to stabilize a constrained system around an equilibrium point or trajectory.

[0004] In G. Williams et al., “Informationtheoretic MPC for model-based reinforcement learning,” International Conference on Robotics and Automation, 2017, an information theory approach was disclosed to overcome some of the natural limitations of standard MPC techniques. Compared to standard MPC techniques, InformationTheoretic Model Predictive Control (IT-MPC) can operate under arbitrary system dynamics and arbitrary nonlinear cost definitions. Summary of the Invention

[0005] According to the present invention, a method for planning trajectories (particularly for robotic devices) according to claim 1 is provided, as well as a control unit and a robotic device according to further independent claims.

[0006] Further embodiments are indicated in the dependent claims.

[0007] According to the first aspect, a computer-implemented method for determining the control trajectory of a robotic device is provided, comprising the following steps:

[0008] - Implement information theory model predictive control, thereby applying control trajectory sample priors at each time step to obtain the control trajectory for a given time range;

[0009] - The control trajectory sample prior is determined by a data-driven trajectory prediction model, which is trained to output control trajectory samples as control trajectory sample priors based on the actual state of the robot device.

[0010] In general, IT-MPC generates open-loop sequences of sampled control by minimizing the Kübck-Leibler (KL) divergence between the current control distribution and the optimal control distribution derived from the desired cost function. The control samples for IT-MPC are generated from a normal distribution centered around the previous control sequences. Therefore, the method works well as long as the optimal control sequence changes only slightly from one step to another.

[0011] However, if the optimal control sequence changes significantly, for example due to a new target location or the unexpected appearance of dynamic obstacles, local sampling around the previous control sequence may lead to poor convergence to the optimal control sequence. Therefore, standard IT-MPC is often unsuitable for mobile robot navigation in congested environments.

[0012] Furthermore, the informed variant of the theoretical model predicts control can iteratively evaluate multiple control trajectory samples derived from the prior control trajectory samples based on a given distribution at each time step to obtain additional control trajectory samples, wherein the additional control trajectory samples are determined by a combination of multiple weighted control trajectory samples, where the weights are determined based on the cost of each of the multiple control trajectory samples.

[0013] What can be provided are data-driven trajectory prediction models including neural networks, specifically of one of the following types: flexible actuation-evaluation networks, trust region networks, policy optimization networks, proximal policy optimization networks, and deep deterministic policy gradient networks.

[0014] One approach to overcome the aforementioned limitation on poor convergence of optimal control sequences is to utilize an informed sampling process by providing predicted control trajectories as control trajectory priors using deep reinforcement learning algorithms. This can essentially involve a data-driven trajectory prediction model, acting as a machine learning model, that generates control trajectory priors based on the current state of the environment, including the robot's pose and the poses of detected obstacles, where poses comprise the position and orientation of the robot or a portion of it within the environment. The data-driven trajectory prediction model is trained to estimate the control trajectory priors, thereby applying a training dataset containing optimized control trajectories for different states of the environment. Optimized control trajectories can be provided by any kind of trajectory optimization process for robotic devices in congested environments. Sampling from this control trajectory prior distribution results in improved performance because fewer samples fall into the costly parts of the state space.

[0015] In other words, one idea is to combine deep reinforcement learning methods with the informed information theory (MIB) approach for trajectory planning in crowded environments. A trajectory prediction model is trained and applied to the network, which generates robot-controlled trajectories that perceive the motion of surrounding dynamic objects and / or individuals.

[0016] In general, the above method improves the information theory model predictive control technique for locally generating robot trajectories by considering stochastic nonlinear systems in dynamic environments. The obtained trajectory is the result of minimizing the KL divergence between the optimal sampling distribution and the actual sampling distribution.

[0017] Robotic devices as understood in this article include mobile robots, such as autonomous cleaning robots, autonomous lawnmowers, and robots with movable manipulators (such as robotic arms) configured to move within a given space.

[0018] As understood in this article, a trajectory is a planned path of motion along which a robotic device can move or along which a part of a robotic device (such as a manipulator) can move.

[0019] In particular, the method above uses a data-driven prediction model that leverages the potential of the IT-MPC framework.

[0020] Furthermore, the prior control trajectory samples can be obtained from the modeled control trajectory samples obtained by the trajectory prediction model and the control trajectory samples obtained in the final time step.

[0021] Specifically, the prior control trajectory samples can be obtained by summing the modeled control trajectory samples obtained from the trajectory prediction model and the control trajectory samples obtained at the final time step, each weighted according to its trajectory cost by a given cost function. The cost of the trajectory can be related to the expense of moving along the trajectory in terms of energy, time, trajectory length, etc.

[0022] According to the embodiment, modeled control trajectory samples can only be considered if the trajectory cost of the modeled control trajectory sample is higher than the trajectory cost of the control trajectory sample obtained in the last time step.

[0023] According to one aspect, a control unit, such as a data processing device, is provided for determining the control trajectory of a robotic device, the control unit being configured to perform the following steps:

[0024] - Implement information theory model predictive control, thereby applying control trajectory sample priors at each time step to obtain the control trajectory for a given time range;

[0025] - The control trajectory sample prior is determined by a data-driven trajectory prediction model, which is trained to output control trajectory samples as control trajectory sample priors based on the actual state of the robot device.

[0026] According to another aspect, the robotic device includes an actuation unit configured to move the robotic device according to a control trajectory; and a control unit configured to perform the following steps:

[0027] ○ Implement information theory model predictive control, thereby applying control trajectory sample priors at each time step to obtain the control trajectory for a given time range;

[0028] ○ The control trajectory sample prior is determined by relying on a data-driven trajectory prediction model, which is trained to output control trajectory samples as control trajectory sample priors based on the actual state of the robot device. Attached Figure Description

[0029] The embodiments are described in more detail with reference to the accompanying drawings, wherein:

[0030] Figure 1 A mobile robot is shown in a crowded environment.

[0031] Figure 2 A flowchart of the informed IT-MPC algorithm is shown.

[0032] Figure 3 A flowchart of a subroutine for an important sampling weight function in the informed IT-MPC algorithm is shown.

[0033] Figure 4 The flowchart of the informed control function is shown, which generates control trajectory sample priors based on a trained trajectory prediction model. Detailed Implementation

[0034] Figure 1 A system is illustrated with a robotic device 1 as a mobile robot, which has the task of moving along a planned trajectory from position A to position B through / within environment E. Within environment E, dynamic objects 2, 3, and 4 can move while performing their own tasks. Dynamic objects 2, 3, and 4 can be other mobile robots or individuals, each exhibiting an intelligent task-driven behavior. The intelligent behavior of dynamic objects 2, 3, and 4 can follow a strategy that takes into account the actions of robotic device 1 and / or other dynamic objects 2, 3, and 4.

[0035] Mobile robots can be autonomous cleaning robots, autonomous lawnmowers, autonomous service or care robots, etc. In other embodiments, the robotic device can be a robot with manipulators, such as a robotic arm, wherein the manipulators (such as a robotic gripper) will follow a planned trajectory.

[0036] Robotic device 1 can have the following features: Figure 1 The configuration is schematically shown in the diagram. The robot device 1 has a control unit 11, which, among other things, is configured to perform the methods described below and control the movement of the robot device 1 along a planned trajectory.

[0037] The control unit 11 may have a microprocessor or microcontroller and memory for storing data and algorithm code. Furthermore, the robot device 1 has an actuation unit 12 for interacting with the environment; for example, the actuation unit 12 may include traction motors for driving the wheels of the robot device 1 to move the robot device 1 within the environment E. The actuation unit 12 may also include a controllable robot arm with grippers, etc. The actuation unit 12 is controlled by the control unit 11.

[0038] Furthermore, the robotic device 1 may include a sensor system 13 for sensing the environment E, particularly for detecting other objects and structures that allow for the localization of dynamic objects 2, 3, 4 in the robotic device 1 and the environment E, such that their positions or their orientations are known in the environment E. The sensor system 13 may include radar, lidar, and / or imaging systems, such as one or more cameras for scanning the environment E surrounding the robotic device 1.

[0039] When configuring such a robot device 1, a control strategy must be installed that allows the robot device 1 to autonomously perform its tasks in environment E. The control strategy should have implemented collision avoidance / handling to prevent motion trajectories that may collide with another dynamic object, or to respond to collisions.

[0040] In simulation-based benchmarking of navigation algorithms for robotic devices, it is generally difficult to effectively model the behavior of dynamic objects 2. Such behavior of each dynamic object may either contribute to or hinder the performance of the robotic device 1's tasks.

[0041] Basically, the Informed Information Theory Model Predictive Control (IT-MPC) method is implemented and executed in the control unit 11 of the robotic device 1. The Informed Information Theory Model Predictive Control (IT-MPC) process considers... stochastic nonlinear discrete-time systems of the form, where It is the system state in n dimensions at time t. Corresponding to m-dimensional input control variables, where white noise must be nominally controlled. ,Right now as well as ,in Corresponding to the input sequence, while Corresponding to the average input, and it has a perturbation. .

[0042] The goal of IT-MPC is to solve stochastic optimal control problems of the following forms:

[0043]

[0044] in It is the set of possible input sequences for the system. It is a state-based cost. This represents the terminal cost, and the rate λ > 0. Optimal control distribution. Can be exported as

[0045]

[0046] in The state cost of the entire trajectory is indicated by p, where p can be the covariance. Gaussian distribution, and This is the normalization factor. To approximate the minimization problem above, it is recommended to minimize the optimal control distribution. and KL divergence between

[0047] .

[0048] When the optimal control distribution is Gaussian, the approximation is correct, and the obtained control trajectory is optimal. Considering the nominal control trajectory... Surrounding samples As described above, it has disturbances. The above approximate minimization problem can be solved using an importance sampling scheme.

[0049]

[0050] in These are the importance sampling weights. Preferably, the above formula is used iteratively as the update rule.

[0051] Combination Figure 2 The flowchart and pseudocode describe a modified informed IT-MPC algorithm, as follows:

[0052]

[0053] According to step S1 (line 1), the initial configuration of the process parameters is given. Therefore, u * Indicates the initial control sequence, K indicates the number of samples, t f Indicates the time range, F indicates the conversion model. ϕ c indicates terminal cost, ∑ and λ indicate hyperparameters, and C indicates decoder conditions.

[0054] In step S2, a control loop is started. This control loop is executed between lines 2 and 22 of the pseudocode of the informed IT-MPC algorithm. Once the robot device 1 reaches the target state, the control loop can be terminated.

[0055] In step S2 (line 3), the system state is obtained via the function "GetStateEstimate". The system state can be determined by processing sensor signals to obtain the attitude of robot device 1 in / relative to environment E, as well as the attitude and velocity of each dynamic obstacle 2, 3, 4 in environment E, as is well known in the art. Essentially, this is done by reading sensor signals such as lidar, radar, etc., which are further processed to obtain the corresponding attitude and velocity.

[0056] In step S3 (line 4), the prior control trajectory samples are obtained through the algorithm "InformControls". The following section combines... Figure 4 Describe the algorithm "InformControls".

[0057] Based on control trajectory sample prior For each sample k, perform a loop between lines 5 and 16.

[0058] In step S4 (lines 6 to 8), , and It was initialized.

[0059] In step S5, the control trajectory prior obtained in step S3 is... Initially, the control trajectory prior was modified in subsequent iterations. Predicting mobility costs. This is done by using a time range. t f It is implemented by predicting the future state of the system (line 10) and accumulating the corresponding movement costs. (Line 11). The mobility costs thus obtained are added to the terminal costs in line 13. .

[0060] In step S6 (line 14), importance sampling weights Through such Figure 3 The flowchart and the following pseudocode illustrate the algorithm "ImportanceSamplingWeights" to obtain:

[0061]

[0062] Cumulative trajectory cost based on each control trajectory sample And determine the importance sampling weights based on a given λ. .

[0063] The function that determines the importance sampling weights uses the minimum cost of all control trajectory samples considered so far in step S11 (line 2). ρ In step S12 (line 3), a normalization factor is calculated to make all movement costs of the considered samples comparable.

[0064] .

[0065] In step S13 (lines 4 to 6), the cost depends on the corresponding control trajectory cost. The importance sampling weight for each (currently determined) control trajectory sample is obtained by the difference between the minimum control trajectory cost and the minimum control trajectory cost across all samples. The loops in lines 4 and 6 are necessary because only [the loop is needed]. .

[0066] Basically, importance sampling weights The higher the cost of control trajectory samples and the lower the cost of minimum control trajectory samples, the better. ρ The greater the difference between them, the higher the value of the difference. Importance sampling weight Return to the informed IT-MPC algorithm in line 7.

[0067] In step S7 (line 15), based on importance sampling weights To obtain additional control trajectory samples for the loop between lines 5 and 16. .

[0068] In step S8, it is asked whether all samples have been considered. If the answer is yes (or, alternatively: yes), the process continues to step S9; otherwise (or, alternatively: no), the process continues to step S4.

[0069] In step S9 (line 17) of the informed IT-MPC algorithm, the first control data... This is applied to robot device 1. In step S10 (lines 18 to 20), the predicted trajectory is updated to the subsequent time step, such that... Set to the previous And so on.

[0070] In step S10, the process is asked whether the task is completed. If the answer is yes (or no), the process ends; otherwise, the process continues to step S2.

[0071] In step S3 (line 4), by combining Figure 4 The following pseudocode describes the algorithm for obtaining prior control trajectory samples:

[0072]

[0073] Control trajectory sample prior u *Based on the actual state of system x and the final time step u * The trajectory control set, the initial conditions required by the network (such as some sensor inputs) Current status of robot device 1 , Covariance measurement Terminal costs The state cost c and the transition model F are generated.

[0074] In step S21 (line 2), based on the data-driven trajectory prediction model (function call: DeepReinforcementLearning), , , To determine the modeled control trajectory samples This data-driven trajectory prediction model can be implemented as a machine learning model, such as a neural network. The input to the trajectory prediction model is... , , It indicates the actual state of the system, including the posture of the robot device 1 and the state of the environment E.

[0075] Neural networks can be of different types, such as flexible actuation-evaluation, trust region, policy optimization, proximal policy optimization, and deep deterministic policy gradient.

[0076] The trajectory prediction model is trained to generate modeled control trajectory samples based on the actual state of the system, and has been trained with optimized control trajectories. To obtain optimized control trajectories, classical deterministic MPC techniques, such as those 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, can be used.

[0077] In step S22, along a given time range, based on the actual state of system x, a modeled control trajectory sample predicted by the trajectory prediction model, including the corresponding terminal cost at the end of the time range, is determined. Cost (Lines 4, 5 and 9).

[0078] In step S23, along a given time range, based on the actual state of system x, the final time step, including the corresponding terminal cost at the end of the time range, is determined. u * The cost of the additional trajectory control samples (lines 6, 7 and 10).

[0079] In step S24 (line 12), the control trajectory sample prior is used to return to the main algorithm Alg1 in step S4. u * Trajectory controlled by model And additional trajectory control samples at the final time step determined in step S7. u * The combination determines this.

[0080] For example, the prior of the returned control trajectory samples u * From the additional control trajectory samples generated last u * and modeled control trajectory samples Generate. If model control trajectory samples Cost Higher than the final generated control trajectory sample u * Cost Then, based on line 12, the returned control trajectory sample is generated according to the sum of the weighted control trajectories. u * The weights are determined by the corresponding costs. and Generate. Otherwise, additional control trajectory samples determined by step S7 of the main algorithm. u * It was used in subsequent iterations.

[0081] The algorithm above aims to find the control trajectory distribution that minimizes the expected cost. The control trajectory distribution is strongly dependent on the cost function and the predictions of the trajectory prediction model. If the initial control trajectory sample distribution is already close to the optimal distribution, the sampling-based approach is even more accurate. Instead of simple initialization (such as zero or constant velocity of obstacles), the control trajectory sample distribution is guided towards the low-cost region of the state space. When using a data-driven trajectory prediction model, control trajectory samples that optimize the original objective can be generated.

Claims

1. A computer-implemented method for determining the control trajectory of a robotic device (1), comprising the following steps: - Implement (S1-S8) information theory model predictive control, thereby applying prior control trajectory samples at each time step. u * ) to obtain a given time range ( t f The control trajectory; - Relying on a data-driven trajectory prediction model to determine (S21) the prior of the control trajectory samples ( u * The trajectory prediction model is trained to output control trajectory samples based on the actual state of the robot device (1) as the prior of the control trajectory samples. u * ), The control trajectory sample prior ( u * The control trajectory samples were obtained through modeled control trajectory samples from the trajectory prediction model and control trajectory samples obtained at the final time step. The control trajectory sample prior ( u * The control trajectory samples are obtained by summing the modeled control trajectory samples obtained from the trajectory prediction model and the control trajectory samples obtained at the last time step, and each is weighted according to its trajectory cost based on a given cost function. The modeled control trajectory sample is only considered if the trajectory cost of the modeled control trajectory sample is higher than the trajectory cost of the control trajectory sample obtained in the last time step.

2. The method of claim 1, wherein the information theory model predictive control iteratively evaluates prior information from control trajectory samples based on a given distribution at each time step. u * The number of control trajectory samples derived is used to obtain additional control trajectory samples, wherein the additional control trajectory samples are determined by a combination of multiple weighted control trajectory samples, wherein in particular, the weights are determined based on the cost of each of the multiple control trajectory samples.

3. The method according to claim 1 or 2, wherein the data-driven trajectory prediction model comprises a machine learning model, particularly a neural network, specifically of the type of a flexible actuation-evaluation network, a trust region network, a policy optimization network, a proximal policy optimization network, or a deep deterministic policy gradient network.

4. The method according to claim 1 or 2, wherein the robot device (1) is controlled to act according to a determined control trajectory.

5. A control unit (11), such as a data processing device, for determining the control trajectory of the robot device (1), said control unit (11) being configured to perform the following steps: - Implement predictive control using information theory models, thereby applying prior control trajectory samples at each time step. u * ) to obtain a given time range ( t f The control trajectory; - Relying on a data-driven trajectory prediction model to determine the prior of control trajectory samples ( u * The trajectory prediction model is trained to output control trajectory samples based on the actual state of the robot device (1) as the prior of the control trajectory samples. u * ), The control trajectory sample prior ( u * The control trajectory samples were obtained through modeled control trajectory samples from the trajectory prediction model and control trajectory samples obtained at the final time step. The control trajectory sample prior ( u * The control trajectory samples are obtained by summing the modeled control trajectory samples obtained from the trajectory prediction model and the control trajectory samples obtained at the last time step, and each is weighted according to its trajectory cost based on a given cost function. The modeled control trajectory sample is only considered if the trajectory cost of the modeled control trajectory sample is higher than the trajectory cost of the control trajectory sample obtained in the last time step.

6. Robotic equipment (1), including: - Actuation unit, configured to move the robot device according to the control trajectory (1); - The control unit (11) according to claim 5.

7. A computer program product comprising instructions which, when executed by a computer, cause the computer to perform the steps of the method according to any one of claims 1 to 4.

8. A machine-readable medium comprising instructions that, when executed by a computer, cause the computer to perform the steps of the method according to any one of claims 1 to 4.

Images