A robot motion planning method based on improved augmented differential dynamic programming

Abstract

The application discloses a robot motion planning method based on improved augmented differential dynamic programming. The application introduces the augmented Lagrange algorithm into the differential dynamic programming algorithm by using the Bellman optimality principle and the fast convergence characteristics of the differential dynamic programming, so that the augmented-differential dynamic programming algorithm can process the contact collision constraint and the nonlinear constraint in the robot motion planning; meanwhile, the idea of the multiple shooting method is introduced, the trajectory is divided into multiple subintervals, the initial value sensitivity is reduced, and the convergence speed and the robustness of the algorithm are improved. The application has strong universality and can be used for motion trajectory planning of various robots.

Description

Technical Field

[0001] The present 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 fields such as agriculture, disaster relief, and construction require mobile robots to traverse irregular terrain or navigate narrow passages. Humanoid robots are the most human-like type of robot, and their high mobility makes them ideally suited for these scenarios. They can easily replace humans in hazardous environments and even complete tasks that ordinary humans cannot. The highly nonlinear multi-contact dynamics between humanoid robots and their environment during locomotion, as well as the management of various constraints during motion, present practical challenges that need to be addressed. This presents both opportunities and challenges for 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 over the past decade. Several methods have been proposed in the past few years to address the nonlinear issues arising from multi-contact dynamics and constraint management in MPC, including adaptive pseudospectral methods, sequential quadratic programming (SQP), sequential convex programming, and differential dynamic programming (DDP). The curse of dimensionality imposed by the high-dimensional dynamics of humanoid robots has hindered their widespread adoption. DDP shows great promise for online, full-body MPC in humanoid robots. Its feasibility has been demonstrated in simulations and real-world experiments on several quadruped and humanoid robots. DDP is a target-shooting approach that can terminate at any time and generate physically feasible motion trajectories. Traditional DDP only addresses the dynamic constraints, i.e., the dynamics. It is ineffective for motion planning complex dynamic constraints involving multiple equality and inequality constraints and collision discontinuities. 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: Based on 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 equations 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 unconstrained optimization problem, define the value function, deduction matrix, and feedforward and feedback terms in control correction;

[0011] S32, adopts the multiple shooting method to divide 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, the state quantity is updated by forward push;

[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 of any ground action to standing, the cost function and constraint equations 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 ), 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 six constraints in the constraint equation (2) represent the dynamic constraint, joint torque constraint, non-negative collision force constraint, friction cone constraint and collision switching constraint (the latter two are 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 the augmented Lagrangian algorithm, the barrier penalty function method or the 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 used to update the deductive matrix to formula (14):

[0031]

[0032] in, are the deductive matrices defined for 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] This invention leverages the Bellman optimality principle and the rapid convergence of differential dynamic programming to introduce the augmented Lagrangian algorithm into the differential dynamic programming algorithm. This allows the augmented-differential dynamic programming algorithm to simultaneously handle contact and collision constraints and nonlinear constraints in robot motion planning. Furthermore, by incorporating the concept of multiple shooting techniques, the trajectory is divided into multiple subintervals, reducing initial value sensitivity and improving the algorithm's convergence speed and robustness. This invention is highly versatile and can be applied to humanoid robots and quadruped robots, as well as to trajectory planning for a variety of actions, including walking, crawling, and standing up after falling. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 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 To improve the pseudo code of the 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 derivation pseudocode in the differential dynamic programming algorithm;

[0043] Figure 7 are joint trajectory diagrams; (a) is the trajectory diagram of the joints of the legs, (b) is the trajectory diagram of the waist joints, and (c) is the trajectory diagram of the joints of the 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] In step one, we first need to clarify the initial state and motion mode of the robot. Then, based on the robot's motion mode and contact collision conditions, we establish the robot's contact kinematics and dynamics model, establish the cost function and 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 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 basis and 23 joint angles, dq represents the differential of q. Assume the total duration is t and the step length is h, so the time step N = t / h, and the joint control torque sequence is randomly given u represents the joint torque; the state quantity at the next moment is calculated according to the dynamic equation Thus, the initial reference state quantity is obtained 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 Equation (1) and Equation (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 plan 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 cost function with multiple equality and inequality 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. 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 the multiple shooting method to solve the unconstrained optimization problem obtained in step 2, which includes:

[0065] S30, in the traditional differential dynamic programming algorithm with only dynamic constraints, first, 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 On 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 Introducing δ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 subscript indicates the time index, and the superscript indicates a specific constraint. σ 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 we can calculate V corresponding to each time step 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 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 subintervals. The starting position of each subinterval is called the shooting state. The shooting state is used as the initial condition to perform numerical integration on each subinterval. When the shooting state is linked to the end of the previous subinterval, the continuity constraint converges, so that the generated trajectory is dynamically feasible. The direct discontinuity of each subinterval is a potential problem, called a defect. Therefore, the idea of ​​multiple shooting algorithm is introduced, and the defects between multiple subintervals 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 , add the defect function into the constraint equation; introduce the idea of ​​multiple shooting method into the differential dynamic programming algorithm, which not only requires minimizing the cost function shown in formula (5), but also requires reducing the defect function to 0.

[0083] Update the value function V(x k ) and the deductive matrix, we first need to update δx k+1 The calculation formula of δx in the single 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 the introduction of 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 dynamic discontinuity in the middle 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 feedback item updated to

[0088]

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

[0090] Specifically, to calculate the forward derivation equation of the improved augmented differential dynamic programming algorithm, it is first necessary to calculate the update δu of the control quantity based on 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 updated state quantity; due to the idea of ​​multiple shooting method, it is not possible to directly use the initial state and the updated control quantity to 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 formula (19), where α∈(0,1] is the step size used for global convergence in line search.

[0093]

[0094] The forward deduction method can adopt linear forward deduction, nonlinear forward deduction, mixed linear and nonlinear forward deduction and other methods. This embodiment adopts the nonlinear forward deduction method, 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 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 terminates the iteration after 100 iterations. The optimized motion trajectory is obtained, such as Figure 7 shown. Figure 8 The change in cost is shown. This demonstrates the versatility of the improved augmented differential dynamic programming algorithm, reduces initial value sensitivity, and improves the algorithm's convergence speed. This example arbitrarily selects a robot motion mode planning. The improved augmented differential dynamic programming algorithm proposed in this invention can be applied to robot crawling, climbing, jumping, and walking.

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

Claims

1. A robot motion planning method based on improved augmented differential dynamic programming, characterized in that: include: Step 1: Based on 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 equations 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 unconstrained optimization problem, define the value function, deduction matrix, and feedforward and feedback terms in control correction; S32, adopts the multiple shooting method to divide 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, the state quantity is updated by forward push; 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, wherein In step 1, according to the process from any ground action to standing, the cost function and constraint equations shown in formulas (1) and (2) are established: Among them, the cost function J(X,U,t) is composed of the running cost l(x k ,u k ) 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 six 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, R(·) is the reset mapping function at the collision point, Represents an enforced collision constraint.

3. The method according to any one of claims 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 any one of claims 1 or 2, characterized in that In step 2, the constrained optimization problem is transformed into an unconstrained optimization problem using the augmented Lagrangian algorithm, the barrier penalty function method or the interior point method to obtain an updated cost function.

5. The method according to claim 2, wherein 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, wherein In S31, according to the cost function and Bellman optimality criterion of the unconstrained optimization problem, the deduction 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); 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 any one of claims 5 or 6, characterized in that In S32, the multiple shooting method is used and the deductive matrix is ​​updated as formula (14) in, are the deductive matrices defined for S31 respectively; The feedforward term in the correction is updated as Feedback item updated to 8. The method according to claim 1, wherein In S33, a linear forward deduction method, a nonlinear forward deduction method, or a mixed linear and nonlinear forward deduction method is adopted.

Images