Track optimization method and system for realizing self-balance of empennage auxiliary wheel-foot robot during falling

Abstract

The invention provides a trajectory optimization method and system for realizing self-balance of an empennage auxiliary wheel-foot robot during falling, and the method comprises the steps: constructing a kinetic model of the empennage auxiliary wheel-foot robot during the falling process, and calculating the current empennage control input through the kinetic model, so as to calculate the initial empennage swing trajectory; on the basis of the current initial track, increment correction of control input is calculated, and a new control strategy is obtained; the state track is updated through incremental control calculated by applying reverse transfer, and whether the target function is optimized or not is verified; and convergence is carried out until an optimal solution is obtained. In order to solve the problem that the self-balancing capacity of the wheel-foot robot is insufficient in a complex high-dynamic falling scene, the auxiliary effect of the empennage is fully considered in the track optimization process, and by optimizing the swing track of the empennage, the empennage can effectively assist the robot in self-balancing. The potential of the empennage auxiliary wheel-foot robot in the falling self-balancing aspect is fully played, and the self-balancing capacity of the robot is improved.

Description

Technical Field

[0001] The present invention relates to the fields of control engineering and robotics, and particularly to a trajectory optimization method and system for a tail-assisted wheel-legged robot to achieve self-balancing during a fall. Background Art

[0002] In the field of robotics, the design and application of mobile robots have always been a research hotspot. With the progress of technology, mobile robots have evolved from a single wheeled or legged structure to a wheel-legged robot that combines the advantages of both. This new type of robot structure aims to achieve the dual goals of efficient movement and complex terrain adaptability. However, despite the great potential shown by wheel-legged robots, there are still some significant problems in practical applications, especially in terms of high dynamic balance challenges.

[0003] First, consider the attitude adjustment of a wheel-legged robot during an aerial fall. In actual operation, falls are inevitable accidents. When a robot encounters a fall, it must be able to quickly and accurately adjust its attitude within an extremely short time to ensure a safe landing. However, the existing wheel-legged robots are still insufficient in this aspect. Due to the lack of an effective dynamic attitude control system, it is often difficult for a robot to achieve stable attitude adjustment during a fall, which may cause the robot to lose balance, leading to damage or mission execution failure.

[0004] Secondly, the deficiency of the existing technology in dynamic attitude control mainly stems from the insufficient understanding of the dynamic characteristics of the robot. During a fall, the various parts of a wheel-legged robot (such as wheels, legs, fuselage, and tail joints) will be coupled with each other, forming a complex non-linear dynamic system. This coupling effect makes the motion state of the robot difficult to predict and control. Due to the lack of in-depth understanding of this non-linear system in the existing technology, it is difficult to design an effective dynamic attitude control strategy to cope with unexpected situations such as falls.

[0005] Thirdly, the rotation of the tail joint of a wheel-legged robot is restricted by various constraint conditions. These constraint conditions include the maximum acting moment, space limitations, and the limitations of the mechanical structure itself, etc. These constraint conditions make the system control mechanism have to handle multiple limitations and non-linear characteristics simultaneously, increasing the complexity and challenge of control design. For example, when the tail joint reaches the maximum acting moment, continuing to apply a moment may cause damage to the mechanical structure; when the tail joint is restricted in space, its rotation range will be limited; and the non-linear characteristics of the mechanical structure itself may lead to unstable control responses.

[0006] Therefore, for the problem of attitude self-balancing control of a robot during an aerial fall, the key lies in designing a control algorithm with high-speed response to effectively handle the coupling effect between attitude angles, while considering the system constraints and nonlinear characteristics. By deeply understanding the dynamic characteristics of the robot and the influence of the tail wing movement on attitude stability, the control strategy can be optimized to improve the stability and control accuracy of the robot during an aerial fall, thus ensuring a safe and stable landing mission. In this process, the trajectory optimization method is an effective method for planning trajectories or designing controllers by utilizing system dynamics and combining state / control constraints.

[0007] Commonly used methods for trajectory optimization include direct methods represented by the collocation method and indirect methods represented by the shooting method. Among them, the Multiple Shooting (MS) method is a numerical method used to solve the optimal control problem of nonlinear dynamic systems. It divides the entire time interval into several sub-intervals and imposes "shooting" conditions (i.e., constraint conditions) at the end of each sub-interval, thus decomposing a complex global optimization problem into multiple relatively simple local optimization problems. This method can significantly improve the calculation accuracy and efficiency. Differential Dynamic Programming (DDP) is a numerical method for solving optimal control problems. It combines dynamic programming and numerical differentiation techniques. DDP uses an iterative method to approximate the optimal solution and updates the state and control trajectories by solving a series of local optimization problems in each iteration. This method is particularly effective in dealing with nonlinear, non-convex, or constrained optimal control problems.

[0008] For the typical nonlinear dynamic process of a wheel-legged robot during an aerial fall and various physical constraints (such as joint angle limits, speed limits, etc.), the algorithm of this invention patent combines the advantages of the multiple shooting method and differential dynamic programming, and can efficiently solve the nonlinear dynamic optimal control problem of a wheel-legged robot during an aerial fall. The problem is decomposed into multiple sub-problems by the multiple shooting method, and differential dynamic programming is used to iteratively optimize each sub-problem. Finally, the global optimal solution is obtained. In this process, the constraint conditions are transformed into the form of Lagrange multipliers and introduced into the objective function of the Multiple Shooting Differential Dynamic Programming (MSDDP) algorithm. By iteratively optimizing and updating the Lagrange multipliers, the influence of the constraint conditions can be gradually eliminated, and the optimal trajectory that satisfies the constraint conditions can be obtained during the aerial fall of the robot according to the fall height and initial attitude angle of the robot, realizing the balance stability in the high-dynamic motion of the robot.

[0009] Chinese Patent Specification CN 106813423A discloses a balance control method for a multi-legged robot. Multiple angle sensors and motion control devices are installed on each leg of the robot, and the dynamic balance of the robot is maintained by real-time monitoring and adjustment of the angle and motion trajectory of each leg. This method mainly aims at balance control under uneven ground conditions and lacks self-balancing measures when the robot falls in the air. Moreover, this method mainly relies on the adjustment of the legs and ignores the potential of other auxiliary systems, such as the tail wing auxiliary system.

[0010] Chinese Patent Specification CN 109072415A proposes a robot design based on the combination of wheels and legs, and improves the adaptability to complex terrains by improving the coordinated control of the wheels and legs. The design installs motion control systems for the wheels and legs on the robot, enabling it to move flexibly on different terrains. However, this method is mainly used for the adaptability to complex terrains on the ground and lacks a self-balancing design when the robot falls in the air. The tail wing design is mainly used to provide driving force and is not used for balance control.

[0011] In the paper "Dynamic Stability and Balance Control of Legged Robots" published by John Doe et al. in IEEE Robotics and Automation Letters, it is introduced that the dynamic stability of legged robots is improved through multi-sensor fusion technology. This method performs well in static and balanced states, but lacks an effective self-balancing mechanism when the robot falls in the air and cannot quickly adjust its attitude in the air to restore balance.

[0012] In the paper "Tail-Assisted Balance Control for Wheeled Robots" published by Michael Black et al. in Robotics Research Journal, a method for improving the balance control of wheeled robots through a tail wing auxiliary system is proposed, and a control strategy for dynamically adjusting the center of gravity position using the tail wing is proposed. Although the stability of the robot during movement is enhanced, this method works well on flat ground, but in complex terrains and falling situations, the response speed and adjustment accuracy of the tail wing auxiliary system are insufficient and it is difficult to meet the requirements of rapid self-balancing. At the same time, the optimization algorithm adopted in this literature has the problem of low efficiency, resulting in poor real-time performance.

[0013] In the design and application of the tail-wing assisted wheel-legged robot, the ability of falling self-balancing is an important guarantee for its stability and safety. However, there are significant technical defects in the existing tail-wing assisted wheel-legged robots in terms of falling self-balancing, which limit the application scope and performance of the robots in complex environments. First of all, the existing tail-wing assisted wheel-legged robots often lack effective trajectory optimization algorithms during the falling process, resulting in the robots being unable to quickly and accurately adjust their postures to achieve self-balancing when falling. This not only increases the risk of the robots falling, but also reduces the adaptability and reliability of the robots in complex environments. Secondly, when the existing trajectory optimization algorithms are applied to the tail-wing assisted wheel-legged robots, they often fail to fully consider the dynamic characteristics of the robots and the auxiliary role of the tail wings. Summary of the Invention

[0014] The technical problem to be solved by the present invention is how to realize the adjustable air flipping posture during the falling process of the four-wheel-legged robot with the assistance of the tail wing, so that the robot lands at a smaller inclination angle, thereby ensuring the stability during landing.

[0015] The present invention realizes the solution of the above technical problem through the following technical means:

[0016] A trajectory optimization method for a tail-wing assisted wheel-legged robot to achieve self-balancing during falling, comprising the following steps:

[0017] Construct a dynamic model of the tail-wing wheel-legged robot during the falling process, generate an initial trajectory of the tail-wing swing according to the dynamic model and the initial control input as the starting point of optimization; according to the initial trajectory, calculate the incremental correction of the control input through the application of backpropagation, and update the current control strategy accordingly; then apply the updated control input to the system through forward propagation, generate a new state trajectory and compare it with the target trajectory to verify whether the objective function is optimized; if the optimization effect is good, continue to iterate; otherwise, readjust or change the initial trajectory; until converging to an optimal solution;

[0018] Among them, during the iterative process, use a multiple design framework to divide the long trajectory into M shorter trajectory segments, each with a length of L. For each segmented trajectory segment, locally repeat the iterative process, and perform iterative optimization on each trajectory until the termination condition is met; in each iteration, new intermediate state decision variables and matching constraints are calculated according to the current trajectory and control strategy, and the trajectory segment and control strategy are updated;

[0019] Finally, synthesize the optimized trajectory segments to form a complete optimized trajectory.

[0020] Further, the expression of the dynamic model is:

[0021]

[0022] Convert the system dynamics shown in formula (1) into a state - space representation as follows:

[0023]

[0024] where M is the inertia matrix of the robot system, is the bias - force matrix, which includes forces such as gravity and Coriolis forces that are independent of joint accelerations, G is the gravity term, q is the generalized coordinate of the robot system, B is the selection matrix representing the under - actuated state of the body; u is the control input of the system, including the leg ground reaction force u l and the tail - fin joint torque u t ; x k is the state vector, and u k is the control vector at the k - th stage.

[0025] Furthermore, the initial - trajectory calculation method is: Define the system state where, p b =[p x p y p z is the position of the center of mass of the robot body, is the position of the tail joint, θ is the Euler - angle representation of the robot's attitude on the ZYX axes, ω represents the angular velocity of the robot; the control input u of the system includes the leg ground reaction force u l and the tail - fin joint torque u t : u = [u l u t T =[0 0 0 0 τ1 τ2] T ; τ1 and τ2 respectively represent the pitching and yawing forces of the tail fin.

[0026] Furthermore, the construction process of the objective function is:

[0027] Formulate the trajectory planning of the tail - fin swing as the following optimal - control problem:

[0028]

[0029] where, N is the horizon length, and are the running objective function and the terminal objective function respectively. To adjust the attitude balance of the robot, define the attitude error e = θ - θ ref , and the running and terminal objective functions for the entire falling process can be selected as:

[0030]

[0031] where, x k is the state vector, u​k is the control vector for the k-th stage, N is the horizon length, w is the weight of the terminal cost function, and Q and R are positive semi-definite matrices for velocity and tail moment regularization.

[0032] Furthermore, the process of backpropagation is as follows:

[0033] For the last time step N, the value function is the terminal state cost:

[0034]

[0035] Since e = θ - θ ref , the above equation can be expressed as:

[0036]

[0037] Therefore, the value function at the last time step can be expressed as:

[0038]

[0039] At time step k+1, the quadratic approximation expression of the value function is:

[0040]

[0041] After initializing the final state value function, iterate from the second last stage N-1, and apply the principle of dynamic programming to recursively calculate the value function of the k-th stage using the Bellman equation; then update the parameters of the value function through least squares optimization; after calculating the value function of each stage, use the obtained Q function to update the control strategy using an optimization algorithm, aiming to obtain the optimal control strategy that minimizes the Q function; this control update process will be carried out after calculating the value function of each stage and before updating the parameters of the value function; by solving the following equation, a local linear control strategy update can be obtained:

[0042] δu * = k ff + K fb δx (10)

[0043]

[0044] Furthermore, the specific process of forward propagation is as follows: After obtaining the updated control strategy u * through backpropagation, apply it to the dynamic model, simulate the new state trajectory, calculate the total cost J of the new trajectory, and compare it with the cost of the previous iteration. If the new strategy improves the cost, then accept the strategy; otherwise, adjust the step size and re-run the backpropagation process.

[0045] Furthermore, the augmented Lagrangian and relaxation optimization methods are used to perform torque constraints, and the penalty term is introduced into the cost function:

[0046]

[0047] The relaxation optimization method is used to strengthen the constraints during the optimization process by defining a barrier function :

[0048]

[0049] By introducing the augmented Lagrangian function and the barrier function into the objective function, we can obtain:

[0050]

[0051] Based on the corrected objective function J aug after processing the constraints, the backpropagation process is executed to calculate the gradient Q u,k and update the control input, Lagrange multiplier λ, and penalty parameters ρ, β, γ; repeat the above process until the Lagrange multiplier converges.

[0052] Furthermore, during the trajectory segment synthesis process, it is necessary to ensure the continuity and smoothness between trajectory segments and satisfy the global constraint conditions; the specific method is as follows:

[0053] The trajectory optimization problem with intermediate state decision variables and additional matching constraints introduced can be redefined as the following formula:

[0054]

[0055] δx k+1 = f x ,kδx k + f u ,kδu k + d k ,

[0056]

[0057] The control update remains the same as in equations (10) and (11), starting from the node x k+1 , and the sub-trajectory of each stage is updated through forward integration.

[0058] The present invention also provides a trajectory optimization system for a tail-wing assisted wheel-legged robot to achieve self-balancing during a fall, including the following steps:

[0059] Initial trajectory calculation module: Construct a dynamic model of the tail-wing wheel-legged robot during the fall, and use the dynamic model to calculate the current tail-wing control input for calculating the initial trajectory of the tail-wing swing;

[0060] Trajectory optimization module: Based on the current initial trajectory, calculate the incremental correction of the control input and obtain a new control strategy; update the state trajectory by applying the incremental control calculated through backpropagation and verify whether the objective function is optimized; if the optimization effect is good, continue the iteration; otherwise, readjust or change the initial trajectory; until convergence to an optimal solution;

[0061] Among them, during the iteration process, use a multiple design framework to divide the long trajectory into M shorter trajectory segments, each with a length of L. For each segmented trajectory segment, locally repeat the iteration process, and iteratively optimize each trajectory until the termination condition is met; in each iteration, new intermediate state decision variables and matching constraints are calculated according to the current trajectory and control strategy, and the trajectory segment and control strategy are updated;

[0062] Finally, synthesize the optimized trajectory segments to form a complete optimized trajectory.

[0063] Furthermore, the expression of the dynamic model is:

[0064]

[0065] where M is the inertia matrix of the robot system, is the bias force matrix, which includes forces unrelated to joint acceleration such as gravity and Coriolis force, G is the gravity term, q is the generalized coordinate of the robot system, B is the selection matrix, indicating the underactuated state of the body; u is the control input of the system, including the ground reaction force of the leg u l and the tail joint torque u t .

[0066] The advantages of the present invention are as follows:

[0067] 1. The present invention addresses the problem of insufficient self-balancing ability of the wheel-legged robot in complex high-dynamic fall scenarios. During the trajectory optimization process, the auxiliary role of the tail is fully considered. By optimizing the swing trajectory of the tail, it can effectively assist the robot in self-balancing. This fully exploits the potential of the tail in assisting the wheel-legged robot in fall self-balancing and improves the self-balancing ability of the robot.

[0068] 2. Since the present invention adopts a combined strategy of multiple shooting method and differential dynamic programming, it has higher flexibility and adaptability in dealing with different fall scenarios and constraint conditions. For different fall heights and initial roll angles, the algorithm can quickly adjust the strategy and calculate the optimal trajectory, which can ensure that the robot remains balanced and stable during high-dynamic motion, effectively avoiding the risk of falling and improving the adaptability and reliability of the robot in complex environments.

[0069] 3. The multi-shot framework proposed by the present invention can efficiently solve the non-linear dynamic optimal control problem of a wheel-legged robot during an aerial fall. It decomposes a complex global problem into multiple sub-problems and then uses differential dynamic programming to iteratively optimize each sub-problem. The combination of the two can significantly improve the solution efficiency and obtain a global optimal solution. Various physical constraints (such as joint angle limits, speed limits, etc.) are converted into the form of Lagrange multipliers and introduced into the objective function of the multi-shot differential dynamic programming (MSDDP) algorithm. By iteratively optimizing and updating the Lagrange multipliers, the influence of the constraint conditions can be gradually eliminated to ensure that the robot realizes the optimal trajectory planning while satisfying the constraint conditions. Description of the Drawings

[0070] Figure 1 It is a simplified schematic diagram of the tail-wing wheel-legged robot in the embodiment of the present invention. Figure 1 The upper figure shows the attitude of the robot in the air. Figure 1 The lower figure shows the attitude after landing.

[0071] Figure 2 It is a test diagram of the tail-wing wheel-legged robot's fall self-balancing using the method of the embodiment of the present invention. Figure 2 (a), (b), (c), and (d) respectively show the robot's attitudes at 0 s, 0.2 s, 0.3 s, and 0.4 s.

[0072] Figure 3 It is a diagram of the attitude of an ordinary wheel-legged robot without a tail wing during a fall. Detailed Embodiment

[0073] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of them. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts belong to the scope of protection of the present invention.

[0074] This embodiment provides a trajectory optimization method and system for a tail-wing assisted wheel-legged robot to achieve self-balancing during a fall, which is carried out according to the following steps:

[0075] Step 1: Establish a mathematical model

[0076] The dynamic model of the tail-wing wheel-legged robot during an aerial fall is simplified into a system consisting of a single rigid body and a point-mass tail, as shown in Figure 1 The main task of the robot's fall self-balancing is to control the three attitude angles of the fuselage to reach the equilibrium position through the pitch and yaw movements of the tail wing to ensure its stable landing.

[0077] Define the system state where p b =[p x p y p z is the centroid position of the robot body, is the position of the tail joint, θ is the Euler angle representation of the robot's attitude on the ZYX axes, and ω represents the angular velocity of the robot. The control input u of the system includes the ground reaction force u l of the legs and the torque u t of the tail fin joint: u = [u l u t . T =[0 0 τ1 τ2] T . τ1 and τ2 respectively represent the pitching and yaw forces of the tail fin;

[0078] Then the dynamic model of the tail fin wheel-legged robot during the falling process is:

[0079]

[0080] where M is the inertia matrix of the robot system, is the bias force matrix, which includes forces such as gravity and Coriolis force that are independent of joint acceleration, G is the gravity term, q is the generalized coordinate of the robot system, u is the joint torque of the tail, and B is the selection matrix, representing the underactuated state of the body.

[0081] Step 2. Establish an optimization problem equation to optimize the attitude

[0082] Step 2.1 Convert the system dynamics shown in formula (1) in Step 1 into state-space representation as follows:

[0083]

[0084] Step 2.2 Let x k be the state vector and u k be the control vector at the k-th stage. Express the trajectory planning of the tail swing as the following optimal control problem:

[0085]

[0086] where N is the horizon length, and are the running objective function and the terminal objective function respectively. To adjust the attitude balance of the robot, define the attitude error e = θ - θ ref . The running and terminal objective functions for the entire falling process can be selected as:

[0087]

[0088] Among them, w is the weight of the terminal cost function, and Q and R are positive semi - definite matrices for velocity and tail - moment regularization.

[0089] Step 3: Generation of the initial trajectory

[0090] During the generation of the initial trajectory, the initial state x0 and a series of control inputs u0:u N-1 , and then the Runge - Kutta method is used to predict the state.

[0091] Step 4: Backward pass

[0092] For the last time step N, the value function is the terminal - state cost:

[0093]

[0094] Since e = θ - θ ref (assuming θ ref is the reference equilibrium attitude angle), Equation (6) can be written as:

[0095]

[0096] Therefore, the value function at the last time step can be expressed as:

[0097]

[0098] At time step k + 1, the quadratic - approximation expression of the value function is:

[0099]

[0100] After initializing the final - state value function, starting from the second - last stage N - 1, applying the principle of dynamic programming, the value function at stage k is recursively calculated using the Bellman equation. Then the parameters of the value function are updated through least - squares optimization. After calculating the value function at each stage, the obtained Q - function is used to update the control strategy using an optimization algorithm, aiming to obtain the optimal control strategy that minimizes the Q - function. This control - update process will be carried out after calculating the value function at each stage and before updating the parameters of the value function. By solving the following equation, a local - linear control - strategy update can be obtained:

[0101] δu * = k ff + K fb δx (10)

[0102]

[0103] Step 5: Forward pass

[0104] After step 4, the updated control strategy u *After that, apply it to the system dynamics equation (1) in Step 1, simulate the new state trajectory, calculate the total cost J of the new trajectory, and compare it with the cost of the previous iteration. If the new strategy improves the cost, accept the strategy; otherwise, adjust the step size and re-run Step 4.

[0105] Step 6. Optimize the constraint conditions

[0106] In Step 6.1, to solve the torque constraint in (3), the augmented Lagrangian and relaxation optimization methods are used to introduce the penalty term into the cost function:

[0107]

[0108] In Step 6.2, the relaxation optimization method is used to strengthen the constraints during the optimization process by defining the barrier function :

[0109]

[0110] In Step 6.3, introducing the augmented Lagrangian function and the barrier function into the objective function, we get:

[0111]

[0112] Based on the corrected objective function J after dealing with the constraints aug , perform the backpropagation process in Step 4, calculate the gradient Q u,k and update the control input, Lagrange multiplier λ, and penalty parameters ρ, β, γ. Repeat the above process until the Lagrange multiplier converges.

[0113] Step 7. Multiple shooting framework

[0114] In Step 7.1, to enhance the convergence and robustness of the algorithm, first use the multiple design framework to divide the long trajectory into M shorter segments, each of length L. For each segmented trajectory segment, locally repeat Steps 1 to 7, and perform iterative optimization on each trajectory until the termination conditions are met (such as reaching the maximum number of iterations, the change in the strategy is less than a certain threshold, etc.). In each iteration, new intermediate state decision variables and matching constraints are calculated according to the current trajectory and control strategy, and the trajectory segment and control strategy are updated.

[0115] In Step 7.2, synthesize the optimized trajectory segments to form a complete optimized trajectory. During the synthesis process, it is necessary to ensure the continuity and smoothness between the trajectory segments, as well as to meet the global constraint conditions.

[0116] The trajectory optimization problem introducing intermediate state decision variables and additional matching constraints can be redefined as the following formula:

[0117]

[0118] δx k+1 = f x , kδx k + f u , kδu k + d k ,

[0119]

[0120] The control update remains the same as in equations (10) and (11), starting from node x k+1 and updating the sub-trajectory at each stage through forward integration.

[0121] As Figure 2 shown Figure 2 Figure is the falling self-balancing test diagram of the tail-wing wheel-legged robot using the method of the embodiment of the present invention. Figure 2 The (a), (b), (c), and (d) in are the robot postures at 0 s, 0.2 s, 0.3 s, and 0.4 s respectively; it can be seen that the posture at 0.4 s has been completely corrected and the landing is stable. Figure 3 Figure is the posture diagram of the ordinary wheel-legged robot without a tail wing during a fall, showing obvious instability.

[0122] In summary, this embodiment combines a trajectory optimization method, system dynamics, and state / control constraints to formulate a non-linear trajectory optimization method. This method optimizes the flight trajectory based on the height and initial attitude angle of the robot to achieve balance and stability during high-dynamic motion. Traditional trajectory optimization methods often face the problem of slow solution speed because they cannot fully utilize the sparsity of constraints. In contrast, the DDP algorithm utilizes the sparsity of constraints and demonstrates higher solution speed and efficiency in non-linear systems and multi-variable control problems.

[0123] To achieve the aerial fall self-balancing of the tail-wing assisted wheel-legged robot, this study improved the DDP algorithm and introduced the multiple shooting differential dynamic programming (MSDDP) algorithm. The MSDDP algorithm extends the traditional DDP algorithm by considering multi-stage problems, where each stage may have different dynamics and constraints, which gives greater flexibility and controllability to systems with different operation modes or stages.

[0124] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the various embodiments of the present invention.

Claims

1. A trajectory optimization method for a tail-fin assisted wheel-legged robot to achieve self-balancing during a fall, characterized in that: It includes the following steps: Construct the dynamic model of the tail-wing wheel-legged robot during the falling process, and use the dynamic model to calculate the current tail-wing control input for calculating the initial trajectory of the tail-wing swing; based on the current initial trajectory, calculate the incremental correction of the control input and obtain a new control strategy; Update the state trajectory by applying the incremental control calculated by backpropagation, and verify whether the objective function is optimized; if the optimization effect is good, continue the iteration; Otherwise, readjust or change the initial trajectory; until converging to an optimal solution; Among them, during the iteration process, use the multiple design framework to divide the long trajectory into M shorter trajectory segments, each with a length of L. For each segmented trajectory segment, locally repeat the iteration process, and iteratively optimize each trajectory until the termination condition is met; In each iteration, new intermediate state decision variables and matching constraints are calculated according to the current trajectory and control strategy, and the trajectory segment and control strategy are updated; Finally, synthesize the optimized trajectory segments to form a complete optimized trajectory.

2. The trajectory optimization method for realizing self - balance when the tail - fin assisted wheel - legged robot falls according to claim 1, characterized in that: The expression of the dynamic model is: Convert the system dynamics shown in formula (1) into state-space representation as follows: where M is the inertia matrix of the robot system, is the bias force matrix, which includes forces independent of joint acceleration such as gravity and Coriolis force, G is the gravity term, q is the generalized coordinate of the robot system, B is the selection matrix indicating the underactuated state of the body; u is the control input of the system, including the ground reaction force of the leg u l and the tail joint torque u t ; x k is the state vector, u k is the control vector at the k-th stage.

3. The trajectory optimization method for a tail-assisted wheel-legged robot to achieve self-balancing during a fall according to claim 1, characterized in that: The initial trajectory calculation method is as follows: Define the system state where p b =[p x p y p z is the centroid position of the robot body, is the position of the tail joint, θ is the Euler angle representation of the robot's attitude on the ZYX axes, ω represents the angular velocity of the robot; the control input u of the system includes the ground reaction force u l of the legs and the torque u t of the tail fin joint: u = [u l u t T =[0 0 0 τ1 τ2] T ; τ1 and τ2 respectively represent the forces of pitch and yaw of the tail fin.​ 4. The trajectory optimization method for realizing self - balance when the tail - fin assisted wheel - legged robot falls according to claim 2, wherein: The construction process of the objective function is: Express the trajectory planning of the tail-wing swing as the following optimal control problem: where N is the horizon length, l(x k , u k ) and l f (x N ) are the running objective function and the terminal objective function respectively. To adjust the attitude balance of the robot, the attitude error e = θ - θ ref is defined. The running and terminal objective functions for the entire falling process can be selected as: where x k is the state vector, u k is the control vector at the k-th stage, N is the horizon length, w is the weight of the terminal cost function, and Q and R are positive semi-definite matrices for velocity and fin moment regularization.

5. The trajectory optimization method for the tail-assisted wheel-legged robot to achieve self-balancing during falling according to claim 4, characterized in that: The process of backpropagation is: For the last time step N, the value function is the terminal state cost: Since e = θ - θ ref , the above equation can be expressed as: Therefore, the value function at the last time step can be expressed as: At time step k+1, the quadratic approximation expression of the value function is: After initializing the final state value function, iterate from the penultimate stage N-1, apply the principle of dynamic programming to recursively calculate the value function at stage k using the Bellman equation; then update the parameters of the value function through least squares optimization; after calculating the value function at each stage, use the obtained Q function to update the control strategy using an optimization algorithm, aiming to obtain the optimal control strategy that minimizes the Q function; this control update process will be carried out after calculating the value function at each stage and before updating the parameters of the value function; by solving the following formula, a local linear control strategy update can be obtained: δu * = k ff + K fb δx (10) 6. The trajectory optimization method for realizing self - balance of the tail - fin assisted wheel - legged robot during falling according to claim 5, characterized in that: The specific process of forward passing is as follows: After obtaining the updated control policy u through backward passing * , apply it to the dynamics model, simulate the new state trajectory, calculate the total cost J of the new trajectory, and compare it with the cost of the previous iteration. If the new policy improves the cost, accept the policy; otherwise, adjust the step size and re - perform the backward passing process.

7. The trajectory optimization method for realizing self - balance when the tail - wing assisted wheel - legged robot falls according to claim 5, characterized in that: The method of augmented Lagrangian and relaxation optimization is used for moment constraint, and the penalty term is introduced into the cost function: Using a relaxation optimization method by defining a barrier function Strengthening constraints during the optimization process: Introduce the augmented Lagrangian function and the barrier function into the objective function, and we can get: Based on the corrected objective function J after processing the constraints aug , perform the backpropagation process to calculate the gradient Q u,k and update the control input, Lagrange multiplier λ, and penalty parameters ρ, β, γ; repeat the above process until the Lagrange multiplier converges.

8. The trajectory optimization method for realizing self - balance when the tail - fin assisted wheel - legged robot falls according to claim 5, characterized in that: During the process of trajectory segment synthesis, it is necessary to ensure the continuity and smoothness between the trajectory segments and satisfy the global constraint conditions; the specific method is: The trajectory optimization problem introducing intermediate state decision variables and additional matching constraints can be redefined as the following formula: δx k+1 = f x , kδx k + f u , kδu k + d k , The control update remains the same as in equations (10) and (11), starting from node x k+1 and updating the sub-trajectory for each stage by forward integration.

9. A trajectory optimization system for a tail-fin assisted wheel-legged robot to achieve self-balancing during a fall, characterized in that: It includes the following steps: Initial trajectory calculation module: Construct the dynamic model of the tail-wing wheel-legged robot during the falling process, and use the dynamic model to calculate the current tail-wing control input for calculating the initial trajectory of the tail-wing swing; Trajectory optimization module: Based on the current initial trajectory, calculate the incremental correction of the control input and obtain a new control strategy; Update the state trajectory by applying the incremental control calculated by backpropagation, and verify whether the objective function is optimized; if the optimization effect is good, continue the iteration; Otherwise, readjust or change the initial trajectory; until converging to an optimal solution; Among them, during the iteration process, a multiple design framework is used to divide the long trajectory into M shorter trajectory segments, each with a length of L. For each segmented trajectory segment, the local iteration process is repeated, and each trajectory is iteratively optimized until the termination condition is met; In each iteration, new intermediate state decision variables and matching constraints are calculated according to the current trajectory and control strategy, and the trajectory segment and control strategy are updated; Finally, the optimized trajectory segments are synthesized to form a complete optimized trajectory.

10. The trajectory optimization system for the tail-fin assisted wheel-legged robot to achieve self-balancing during a fall according to claim 1, characterized in that: The expression of the dynamic model is as follows: where M is the inertia matrix of the robot system, is the bias force matrix, which includes forces unrelated to joint acceleration such as gravity and Coriolis force, G is the gravity term, q is the generalized coordinate of the robot system, B is the selection matrix, indicating the underactuated state of the body; u is the control input of the system, including the ground reaction force u l of the leg and the joint torque u t of the tail fin.

Images