Robot motion planning method based on improved augmented differential dynamic programming

Abstract

The invention discloses a robot motion planning method based on improved augmented differential dynamic programming. According to the method, the Bellman optimality principle and the rapid convergence characteristic of differential dynamic programming are utilized, and the augmented Lagrange algorithm is introduced into the differential dynamic programming algorithm, so that the augmented-differential dynamic programming algorithm can process contact collision constraints and nonlinear constraints in robot motion programming; meanwhile, the thought of a multi-shooting method is introduced, and the trajectory is divided into a plurality of sub-intervals, so that the initial value sensitivity is reduced, and the convergence speed and robustness of the algorithm are improved. The method is high in universality and can be used for motion track planning of various robots.

Description

Technical Field

[0001] The invention relates to the technical field of humanoid robots, and in particular to a robot motion planning method based on improved augmented differential dynamic programming. Background Art

[0002] Some tasks in the fields of agriculture, disaster relief, construction, etc. require mobile robots to traverse irregular terrain or pass through narrow passages. Humanoid robots are the most similar type of robots to humans. Their high mobility makes them more suitable for the above scenarios. Therefore, they can easily replace humans in some dangerous environments and even complete tasks that ordinary humans cannot complete. The multi-contact dynamics between humanoid robots and the environment during movement are highly nonlinear, and the management of various constraints during movement is a challenge we actually face and need to solve. This is also the opportunity and challenge faced by the optimal control problem (OCP).

[0003] Model Predictive Control (MPC) is a powerful tool in optimization control and has been widely used in the field of humanoid robots in the past decade. In the past few years, several methods have been proposed to solve the nonlinear problems caused by multi-contact dynamics and constraint management in MPC, including adaptive pseudo-spectral method, sequential quadratic programming (SQP), sequential convex programming and differential dynamic programming. The dimensionality curse brought by the high-dimensional dynamics of humanoid robots has hindered its popularity. Differential dynamic programming shows great prospects for online full-body MPC of humanoid robots. The feasibility of differential dynamic programming has been confirmed in simulations and actual experiments of some quadruped and humanoid robots. Differential dynamic programming is a shooting method. The algorithm can terminate at any time and give a physically feasible motion trajectory. Traditional differential dynamic programming only has one constraint, namely the dynamic constraint. For complex dynamic constraints with multiple equality and inequality constraints and collision discontinuity problems, traditional differential dynamic programming cannot be effectively applied to its motion planning. Summary of the invention

[0004] In view of this, the present invention provides a robot motion planning method based on improved augmented differential dynamic programming, which can handle the multi-contact collision dynamic switching problem during the robot movement process. Compared with traditional algorithms for solving nonlinear problems, the method of the present invention guarantees the theoretical second-order convergence speed and improves the solution speed of the algorithm.

[0005] The robot motion planning method based on improved augmented differential dynamic programming of the present invention comprises:

[0006] Step 1: According to the robot's motion mode and contact collision conditions, establish the robot's contact kinematics and dynamics model, establish the cost function and constraint equation corresponding to the robot's motion; at the same time, discretize and linearize the robot system dynamics;

[0007] Step 2: Convert the constrained optimization problem into an unconstrained optimization problem and update the cost function;

[0008] Step 3, using a differential dynamic programming algorithm and combining it with a multiple shooting method to solve the unconstrained optimization problem obtained in step 2;

[0009] Specifically:

[0010] S31, based on the cost function and Bellman optimality criterion of the unconstrained optimization problem, define the value function, deductive matrix, and feedforward and feedback terms in control correction;

[0011] S32, using the multiple shooting method, divides the trajectory into multiple segments, introduces continuity constraints as equality constraints, and updates the deductive matrix, feedforward terms, and feedback terms of S31;

[0012] S33, update the control amount δu according to the deductive matrix updated in S32 k ; According to the updated control quantity, push forward to obtain the update of the state quantity;

[0013] S34, calculating the value of the cost function. If the value of the cost function is greater than the set threshold, returning to S31, until the value of the cost function is less than the set threshold or the set number of iterations is reached or the feedforward term approaches 0.

[0014] Furthermore, in step 1, according to the process from any ground action to standing, a cost function and constraint equation as shown in formulas (1) and (2) are established:

[0015]

[0016]

[0017] Among them, the cost function J(X,U,t) is composed of the running cost and terminal cost Φ(x N ), the state quantity X=[x0 x1…x N ], control quantity U=[u0 u1…u N-1], u represents the joint torque, time t represents the total duration of the motion trajectory, h represents the step length, and the subscript N represents the number of time steps, N = t / h; where the six constraints in constraint equation (2) represent dynamic constraints, joint torque constraints, non-negative collision force constraints, friction cone constraints, and collision switching constraints (the latter two are both collision switching constraints), f(·) represents the dynamic equation, F c,z is the vertical collision force, F c,x is the horizontal ground force, μ is the friction coefficient, R(·) is the reset mapping function at the collision point, Represents an enforced collision constraint.

[0018] Furthermore, the second-order Taylor expansion method is used to discretize and linearize the dynamics of the robot system.

[0019] Furthermore, in step 2, the constrained optimization problem is transformed into an unconstrained optimization problem by using an augmented Lagrangian algorithm, a barrier penalty function method or an interior point method to obtain an updated cost function.

[0020] Furthermore, the augmented Lagrangian algorithm is used to transform the constrained optimization problem into an unconstrained optimization problem, and the updated cost function is obtained as follows:

[0021]

[0022]

[0023] where c k Represents the constraint equation, including equality and inequality constraints. The equality constraint is: j∈ε, the inequality constraint is:

[0024] Furthermore, in S31, according to the cost function of the unconstrained optimization problem and the Bellman optimality criterion, the deductive matrix is ​​defined as follows:

[0025]

[0026] Among them, Qx, Qu, Qxx, Quu, Qux, and Qxu are the deductive matrices of the differential dynamic programming algorithm under traditional dynamic constraints; I σ Used to turn on and off inequality constraints, the definition is shown in formula (12); parameter update is shown in formula (13);

[0027]

[0028]

[0029] in The subscript indicates the time index, and the superscript indicates a specific constraint. x and c u Respectively represent σ η and represents the penalty item, and λ η,i is the Lagrange multiplier, and β is the penalty term update parameter.

[0030] Furthermore, in S32, the multiple shooting method is adopted, and the deductive matrix is ​​updated as formula (14)

[0031]

[0032] in, They are the deductive matrices defined in S31 respectively;

[0033] The feedforward term in the correction is updated as Feedback item updated to

[0034] Furthermore, in S33, a linear forward deduction method, a nonlinear forward deduction method, or a mixed linear and nonlinear forward deduction method is adopted.

[0035] Beneficial effects:

[0036] The present invention utilizes the Bellman optimality principle and the fast convergence characteristics of differential dynamic programming, introduces the augmented Lagrangian algorithm into the differential dynamic programming algorithm, so that the augmented-differential dynamic programming algorithm can simultaneously handle the contact collision constraints and nonlinear constraints in the robot motion planning; at the same time, the idea of ​​multiple shooting method is introduced to divide the trajectory into multiple sub-intervals, which reduces the initial value sensitivity and improves the convergence speed and robustness of the algorithm. The present invention has strong versatility and can be applied to humanoid robots and quadruped robots, and can also be applied to motion trajectory planning for a variety of actions, including walking, crawling, falling and standing up. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 is a flow chart of the method of the present invention;

[0038] Figure 2 Schematic diagram of the robot structure and collision points;

[0039] Figure 3 Show the actions that the robot needs to perform;

[0040] Figure 4 Pseudocode for improving augmented differential dynamic programming algorithm;

[0041] Figure 5 To improve the backward pass pseudo code in the differential dynamic programming algorithm;

[0042] Figure 6 To improve the forward pseudocode in the differential dynamic programming algorithm;

[0043] Figure 7 are joint trajectory diagrams; among them, (a) is the trajectory diagram of the joints of both legs, (b) is the trajectory diagram of the waist joints, and (c) is the trajectory diagram of the joints of both arms;

[0044] Figure 8 To improve the augmented differential dynamic programming algorithm, the total cost changes after 100 iterations. DETAILED DESCRIPTION

[0045] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0046] The present invention provides a robot motion planning method based on improved augmented differential dynamic programming. The method flow chart is as follows: Figure 1 As shown, the following steps are included:

[0047] Step 1: First, you need to clarify the initial state and motion mode of the robot. Then, based on the robot's motion mode and contact collision conditions, establish the robot's contact kinematics and dynamics model, establish the cost function and its constraint equations corresponding to the robot's motion, and discretize and linearize the robot system dynamics.

[0048] This embodiment takes the robot's arbitrary ground movement to standing up movement as an example. Figure 3 shown.

[0049] The robot used in this example has 23 joints, and the initial state is set to x0, x0 = [q0; dq0] T , where q represents the 6 degrees of freedom of the floating base and 23 joint angles, dq represents the differential of q. Set the total duration to t and the step length to h, so the time step N = t / h, and randomly give the joint control torque sequence u represents the joint torque; the state quantity at the next moment is calculated according to the dynamic equation So as to obtain the initial reference state Robot schematic diagram Figure 2 As shown, Figure 2 The triangles in the figure represent the collision points between the robot and the ground during its movement.

[0050] According to the process from any ground action to standing, the cost function and constraint equation corresponding to the robot motion are established as shown in equations (1) and (2):

[0051]

[0052]

[0053] Among them, the cost function J(X,U,t) is composed of the running cost and terminal cost Φ(x N ), the state quantity X=[x0 x1 … x N ], control quantity U=[u0 u1 … u N-1 ], u represents the joint torque, time t represents the total duration of the motion trajectory, h represents the step length, and the subscript N represents the number of time steps, N = t / h. Among them. The five constraints in constraint equation (2) represent dynamic constraints, joint torque constraints, non-negative collision force constraints, friction cone constraints, and collision switching constraints. f(·) represents the dynamic equation, F c,z is the vertical collision force, F c,x is the horizontal ground force, μ is the friction coefficient, and R(·) is the reset mapping function at the collision point. Represents the mandatory collision constraint. It can be seen that the motion planning has multiple equality and inequality constraints, and there are multiple nonlinear constraints.

[0054] In order to enable the differential dynamic programming algorithm to solve the above cost function with multiple equalities and inequalities constraints, the present invention discretizes and linearizes the robot system dynamics. k ,u k ) is approximated by using the second-order Taylor expansion to realize its discretization and linearization. The calculation process is shown in formula (3):

[0055]

[0056] According to the derivation of formula (3), we can get δx k+1 ,δx k+1 The expression of is shown in formula (4), where f x , f u , f xx , f xu , f ux and f uu Both will be used for the calculation of the backward pass in the differential dynamic programming algorithm.

[0057]

[0058] in,

[0059] Step 2: Convert the constrained optimization problem into an unconstrained optimization problem and update the cost function.

[0060] The constraint optimization problem can be converted into an unconstrained optimization problem by using the barrier penalty function method, interior point method, augmented Lagrangian algorithm, etc. This embodiment adopts the augmented Lagrangian algorithm, and the unconstrained cost function after conversion is shown in formula (5), and the equality constraint and inequality constraint in formula (2) are rewritten as c(x)=0 and c(x)≤0 respectively as shown in formula (6);

[0061]

[0062]

[0063] Among them, c k Represents the constraint equation, including equality and inequality constraints. The equality constraint is: The inequality constraints are:

[0064] Step 3: Use the differential dynamic programming algorithm and combine it with the multiple shooting method to solve the unconstrained optimization problem obtained in step 2, which specifically includes:

[0065] S30, in the traditional differential dynamic programming algorithm for only dynamic constraints, firstly, according to its cost function and Bellman optimality criterion, the value function V is defined as shown in formula (7): k x k , where Q k x k ,u k It is called the state-action value function;

[0066]

[0067] Q k x k ,u k In the trajectory x k ,u k Perform a second-order Taylor expansion at δQ, that is, calculate k x k ,u k , as shown in formula (8), and the δx obtained in formula (4) k+1 Bring in δQ k x k ,u k In the formula (9),

[0068]

[0069]

[0070] According to matrix calculation, the following variables are defined, as shown in formula (10):

[0071]

[0072] in, V x Represents the first-order partial derivative of the value function at the next moment, V xx ′ represents the second-order partial derivative of the value function at the next moment.

[0073] S31, the present invention aims at the unconstrained optimization problem after the transformation in step 2, based on the Bellman optimality criterion, and with reference to the derivation process of formula (8) to formula (10), redefines the variables of matrix calculation, as shown in formula (11).

[0074]

[0075] c in formula (5) k It includes equality constraints and inequality constraints, where the equality constraints are: The inequality constraints are: The subscripts represent time indexes, and the superscripts represent specific constraints. σ Used to turn on and off inequality constraints, defined as shown in formula (12); σ η and represents the penalty item, and λ η,i is the Lagrange multiplier, and β is the penalty term update parameter. The parameter update is shown in formula (13). x and c u Respectively represent

[0076]

[0077]

[0078] The backward pass process first needs to calculate V N and V NN , then V corresponding to each time step can be calculated in the backward transfer equation x and V xx , formula (14) shows that V N 、V NN 、V x 、V xx and ΔV calculation formula.

[0079]

[0080] During the backward pass, the feedforward term in the control correction is also obtained. and feedback items

[0081] S32. At the same time, the present invention introduces the idea of ​​multiple target shooting algorithm into the differential dynamic programming algorithm, adds the defect function as a new constraint condition, and updates the deductive matrix in the backward transmission process to improve the convergence rate of the differential dynamic programming algorithm.

[0082] The idea of ​​multiple shooting algorithm is introduced into the differential dynamic programming algorithm. It mainly adopts the idea of ​​multiple shooting algorithm to divide a trajectory into several sub-intervals. The starting position of each sub-interval is called the shooting state. The shooting state is used as the initial condition to perform numerical integration on each sub-interval. When the shooting state is linked to the end of the previous sub-interval, the continuity constraint converges, so that the generated trajectory is dynamically feasible. The direct discontinuity of each sub-interval is a potential problem, called a defect. Therefore, the idea of ​​multiple shooting algorithm is introduced, and the defects between multiple sub-intervals need to be considered. Define the defect function d(x k ,u k ,x k+1 ) is d(x k ,u k ,x k+1 )=f(x k ,u k )-x k+1 , adding the defect function into the constraint equation; introducing the idea of ​​multiple shooting method into the differential dynamic programming algorithm, it is necessary not only to minimize the cost function shown in formula (5), but also to reduce the defect function to 0.

[0083] Update the value function V(x) in the backward pass k ) and the deductive matrix, we first need to update δx k+1 The calculation formula for δx in the single-shot shooting method k+1 The calculation formula is: k+1 =f(x k +δx k ,u k +δu k )-f(x k ,u k ); after introducing the multiple shooting method, δx k+1 The calculation formula is updated to formula (15)

[0084] δx k+1 =f(x k +δx k ,u k +δu k )-x k+1 (15)

[0085] The updated equation (15) allows for the dynamic discontinuity of the intermediate process of the initial guess trajectory. In order to obtain the update strategy related to (15), the calculation ideas of equations (7)-(9) are adopted, and the matrix calculation variables are updated to equation (16),

[0086]

[0087] In addition, V in the backward pass is updated x 、V xx and ΔV, as shown in formula (17), and the feedforward term in the control correction is updated as and the feedback item is updated to

[0088]

[0089] S33, calculate the forward deduction equation of the improved augmented differential dynamic programming algorithm, update the control law, and forward deduced the new motion trajectory.

[0090] Specifically, to calculate the forward derivation equation of the improved augmented differential dynamic programming algorithm, we first need to calculate the update δu of the control variable according to the matrix calculation variables in the backward transfer: k , as shown in formula (18)

[0091]

[0092] According to the updated control quantity, it is necessary to push forward to obtain the state quantity update; due to the idea of ​​multiple shooting method, it is impossible to directly use the initial state and the updated control quantity to directly update the dynamic state. To perform forward push, it is necessary to update the control first. The updated control is given by u′ k It is represented as shown in formula (19), where α∈(0,1] is the step size used for global convergence in line search.

[0093]

[0094] The forward deduction method may adopt linear forward deduction, nonlinear forward deduction, mixed linear and nonlinear forward deduction, etc. This embodiment adopts nonlinear forward deduction, as shown in formula (20), through forward deduction, a new state trajectory X can be obtained, X = [x′0 x′1 … x′ N ].

[0095]

[0096] After each iteration, that is, after completing a backward pass and a forward push, the total cost J is calculated. cost,ii , ii represents a recursive improved augmented differential dynamic programming algorithm; Figures 4 to 6Pseudocode for the improved augmented differential dynamic programming algorithm is presented.

[0097] S34, setting the termination condition of the improved augmented differential dynamic programming algorithm to obtain the final optimized robot motion trajectory.

[0098] The improved augmented differential dynamic programming algorithm of the present invention has three termination conditions. The algorithm can be terminated if any one of the conditions is met:

[0099] Condition 1: The cost during the iteration is reduced to a very small number ε tol ,

[0100] Condition 2: The maximum set number of iterations is reached;

[0101] Condition 3: The feedforward gain tends to 0, that is

[0102] After multiple backward passes and forward pushes, if the termination condition is met, the improved augmented differential dynamic programming algorithm is terminated. If the condition is not met, the backward pass and forward push process needs to be continued until the termination condition is met.

[0103] This example uses a clear number of iterations as the termination condition, and the iteration is terminated after 100 iterations. The optimized motion trajectory is obtained, such as Figure 7 shown. Figure 8 The change of cost is shown. This shows that the improved augmented differential dynamic programming algorithm has strong versatility, reduces the sensitivity of initial values ​​and improves the convergence speed of the algorithm. This example arbitrarily selects a robot motion mode planning, and the robot crawling, climbing, jumping, and walking can all adopt the improved augmented differential dynamic programming algorithm proposed in the present invention.

[0104] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A robot motion planning method based on improved augmented differential dynamic programming, characterized in that: include: Step 1: According to the robot's motion mode and contact collision conditions, establish the robot's contact kinematics and dynamics model, and establish the cost function and constraint equation corresponding to the robot's motion; at the same time, discretize and linearize the robot system dynamics; Step 2: Convert the constrained optimization problem into an unconstrained optimization problem and update the cost function; Step 3, using a differential dynamic programming algorithm and combining it with a multiple shooting method to solve the unconstrained optimization problem obtained in step 2; Specifically: S31, based on the cost function and Bellman optimality criterion of the unconstrained optimization problem, define the value function, deductive matrix, and feedforward and feedback terms in control correction; S32, using the multiple shooting method, divides the trajectory into multiple segments, introduces continuity constraints as equality constraints, and updates the deductive matrix, feedforward terms, and feedback terms of S31; S33, update the control amount δu according to the deductive matrix updated in S32 k ; According to the updated control quantity, push forward to obtain the update of the state quantity; S34, calculating the value of the cost function. If the value of the cost function is greater than the set threshold, returning to S31, until the value of the cost function is less than the set threshold or the set number of iterations is reached or the feedforward term approaches 0.

2. The method according to claim 1, characterized in that In step 1, according to the process from any ground action to standing, a cost function and constraint equation as shown in formulas (1) and (2) are established: The cost function J(X,U,t) is calculated by running the cost l(x k ,u k ) and terminal cost Φ(x N ), the state quantity X=[x0x1…x N ], control quantity U=[u0 u1…u N-1 ], u represents the joint torque, time t represents the total duration of the motion trajectory, h represents the step length, and the subscript N represents the number of time steps, N = t / h; where the six constraints in constraint equation (2) represent dynamic constraints, joint torque constraints, non-negative collision force constraints, friction cone constraints, and collision switching constraints, respectively, f(·) represents the dynamic equation, F c,z is the vertical collision force, F c,x is the horizontal ground force, μ is the friction coefficient, R(·) is the reset mapping function at the collision point, Represents an enforced collision constraint.

3. The method according to claim 1 or 2, characterized in that The second-order Taylor expansion method is used to discretize and linearize the dynamics of the robot system.

4. The method according to claim 1 or 2, characterized in that: In the step 2, the constrained optimization problem is transformed into an unconstrained optimization problem by using an augmented Lagrangian algorithm, a barrier penalty function method or an interior point method to obtain an updated cost function.

5. The method according to claim 4, characterized in that The augmented Lagrangian algorithm is used to transform the constrained optimization problem into an unconstrained optimization problem, and the updated cost function is: where c k Represents the constraint equation, including equality and inequality constraints. The equality constraint is: The inequality constraints are:

6. The method according to claim 5, characterized in that In S31, according to the cost function of the unconstrained optimization problem and the Bellman optimality criterion, the deductive matrix is ​​defined as follows: Among them, Qx, Qu, Qxx, Quu, Qux, and Qxu are the deductive matrices of the differential dynamic programming algorithm under traditional dynamic constraints; I σ Used to turn on and off inequality constraints, the definition is shown in formula (12); the parameter update is shown in formula (13); in The subscript indicates the time index, and the superscript indicates a specific constraint. x and c u Respectively represent σ η and represents the penalty item, and λ η,i is the Lagrange multiplier, and β is the penalty term update parameter.

7. The method according to claim 5 or 6, characterized in that In S32, the multiple shooting method is used, and the deductive matrix is ​​updated as formula (14): in, They are the deductive matrices defined in S31 respectively; The feedforward term in the correction is updated as Feedback item updated to 8. The method according to claim 1, characterized in that In S33, a linear forward deduction method, a nonlinear forward deduction method, or a mixed linear and nonlinear forward deduction method is adopted.

Images