A two-wheeled and legged robot trajectory planning method and device

Abstract

The present invention relates to the field of machine control, and in particular to a two-wheeled legged robot trajectory planning method and device, comprising: constructing a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure of the two-wheeled legged robot; optimizing the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model constructed based on constraints required to be added for the task performed by the two-wheeled legged robot to obtain a preliminary optimized trajectory; optimizing the preliminary optimized trajectory using a target trajectory optimization model constructed based on the dynamic model of the two-wheeled legged robot to obtain a target optimized trajectory. The present invention can improve speed tracking performance, reduce controller parameter sensitivity, simplify the controller parameter optimization process, and improve controller robustness.

Description

Technical Field

[0001] The present invention relates to the field of machine control, and in particular to a trajectory planning method and device for a two-wheeled and footed robot. Background Art

[0002] With the continuous advancement of robotics technology, the application scope of mobile robots is becoming more and more extensive, covering multiple fields such as industry, medical care, logistics, etc. Two-wheeled legged robots have gradually become a hot topic of research and application because they combine the advantages of wheeled robots and legged robots. In two-wheeled legged robots, trajectory planning and control are two key issues to achieve their stable motion. Trajectory planning is responsible for generating the robot's motion path from the starting point to the end point, while the controller is responsible for tracking the trajectory and performing specific movements. However, due to the complexity of the structure of two-wheeled legged robots and their dynamic characteristics, how to plan a trajectory that is easy for the controller to track still faces great challenges.

[0003] Traditional trajectory planning methods usually focus on the feasibility and optimality of the path, such as the shortest path or minimum energy consumption. However, these planning methods may not be able to generate trajectories that are suitable for the motion characteristics of a two-wheeled legged robot. For example, the planned trajectory may contain a relatively complex motion curve, which makes it difficult for the controller to track, affecting the robot's smooth motion and control accuracy. In addition, when a two-wheeled legged robot travels on uneven terrain, it needs to pay special attention to the change of the center of gravity and stability, which makes the coupling problem of trajectory planning and control more complicated.

[0004] At present, most of the existing solutions focus on optimization at the control level, such as designing more complex control algorithms to deal with errors in trajectory tracking. However, this method usually increases the complexity of the control system, and also puts higher requirements on computing resources, resulting in poor real-time performance and easy control instability. In order to simplify the design of the controller and improve the tracking accuracy, a new trajectory planning method is urgently needed to make the planned trajectory more suitable for the execution of the controller, thereby improving the overall performance of the control system. Although the optimization of the control algorithm can solve the problems in trajectory tracking to a certain extent, most existing methods fail to fundamentally solve the coupling problem between trajectory planning and control systems. Traditional trajectory planning methods focus on path optimality and ignore the requirements of the controller for trajectory tracking, which makes the controller face large errors and burdens in actual execution. In order to solve this problem, a new trajectory planning method is urgently needed so that the planned trajectory not only conforms to the dynamic characteristics of the robot, but also has low requirements on the robot controller, making it easier to execute and track. Summary of the invention

[0005] In view of the above technical problems, the technical solution adopted by the present invention is:

[0006] According to a first aspect of the present invention, a two-wheeled legged robot trajectory planning method is provided, the method comprising the following steps:

[0007] S100, simplifying the two-wheeled legged robot into a structure including a base, legs and wheels as a target structure, and constructing a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure.

[0008] S200, optimizing the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model to obtain a preliminary optimized trajectory; wherein the initial trajectory optimization model is constructed based on constraints that need to be added for the task performed by the two-wheeled legged robot.

[0009] S300, optimizing the preliminary optimized trajectory using a target trajectory optimization model to obtain a target optimized trajectory; the target trajectory optimization model is constructed based on a dynamic model of the two-wheeled legged robot.

[0010] According to a second aspect of the present invention, there is provided a trajectory planning device for a two-wheeled legged robot, the device comprising:

[0011] The dynamic model building module is used to simplify the two-wheeled legged robot into a structure including a base, legs and wheels as a target structure, and to build a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure.

[0012] The initial trajectory optimization module is used to optimize the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model to obtain a preliminary optimized trajectory; wherein the initial trajectory optimization model is constructed based on the constraints that need to be added for the task performed by the two-wheeled legged robot.

[0013] The target trajectory optimization module is used to optimize the preliminary optimization trajectory using a target trajectory optimization model to obtain a target optimization trajectory; the target trajectory optimization model is constructed based on the dynamics model of the two-wheeled legged robot.

[0014] The present invention has at least the following beneficial effects:

[0015] The two-wheeled legged robot trajectory planning method provided by the embodiment of the present invention first constructs a dynamic model describing the core dynamic characteristics of the robot's balance state based on the target structure of the two-wheeled legged robot; then, the initial trajectory of the two-wheeled legged robot is optimized using an initial trajectory optimization model constructed based on the constraints required to be added for the task performed by the two-wheeled legged robot to obtain a preliminary optimized trajectory; finally, the preliminary optimized trajectory is optimized using a target trajectory optimization model constructed based on the dynamic model of the two-wheeled legged robot to obtain a target optimized trajectory. The present invention further optimizes the preliminary optimized trajectory by considering part of the dynamic model of the robot, expands the dimension of the trajectory, and makes the optimized target optimized trajectory not only contain the spatial information and time information of the center of gravity of the robot, but also contain other state information that can describe the main motion characteristics of the robot, which can ensure the balance and dynamic stability of the system, make the trajectory easier to be tracked by the controller, and improve the tracking accuracy of the robot to the trajectory, while also reducing the requirements of the robot on the controller to achieve high-precision trajectory tracking, and can enhance the robustness of the control part.

[0016] It should be understood that the contents described in this section are not intended to identify the key or important features of the embodiments of the present invention, nor are they intended to limit the scope of the present invention. Other features of the present invention will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0018] Figure 1 A flow chart of a two-wheeled legged robot trajectory planning method provided by an embodiment of the present invention;

[0019] Figure 2 A simplified structural diagram of a two-wheeled legged robot provided in an embodiment of the present invention;

[0020] Figure 3 For Figure 2 A simplified structural diagram further simplified on the simplified structure;

[0021] Figures 4a to 4c A schematic diagram of the effect of the trajectory planning method for a two-wheeled legged robot provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0022] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0023] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art of the present invention. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more related listed items.

[0024] It should be noted that some exemplary embodiments are described as processes or methods depicted as flow charts. Although the flow charts describe the steps as sequential processes, many of the steps can be implemented in parallel, concurrently or simultaneously. In addition, the order of the steps can be rearranged. The process can be terminated when its operation is completed, but it can also have additional steps not included in the accompanying drawings. The process can correspond to a method, function, procedure, subroutine, subprogram, etc.

[0025] The present invention aims to provide a trajectory planning method for the motion stability and control accuracy of a two-wheeled legged robot in a complex environment. By optimizing the controllability of the trajectory, the method enables the two-wheeled legged robot controller to track the planned trajectory more accurately and smoothly, thereby improving the motion stability and control accuracy of the two-wheeled legged robot. Figure 1 As shown, a two-wheeled legged robot trajectory planning method provided by an embodiment of the present invention may include the following steps:

[0026] S100, simplifying the two-wheeled legged robot into a structure including a base, legs and wheels as a target structure, and constructing a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure.

[0027] In an embodiment of the present invention, the structure of the two-wheeled legged robot may be an existing structure, for example, including a base, two hip joints, left and right legs, and onboard hardware devices, wherein each hip joint is provided with three joint motors, one motor for controlling the forward and backward swinging of the leg, one motor for controlling the left and right swinging of the leg, and one motor for controlling the angle of the knee joint. A hub motor and a wheel are installed at the foot end of each leg, and the hub motor is used to control the movement of the wheel. The onboard hardware devices may include an IMU and an onboard computing unit, etc.

[0028] In the embodiment of the present invention, Figure 2As shown, the two-wheeled legged robot can be simplified into a structure including a base 1, legs 2 and wheels 3, wherein the legs 2 are virtual legs and can be formed by a line connecting the center of the base 1 and the center of the wheels 3.

[0029] In the embodiment of the present invention, the Lagrangian method is used to establish the dynamic model of the target structure in the standing stage, and the established dynamic model satisfies the following conditions:

[0030] M s q 2 +C s q 1 +G s =u (1)

[0031] Wherein, q is the state variable of the target structure, q 1 is the first-order derivative of q, q 2 is the second-order derivative of q, q=(θ w , L, θ p ,θ h ), θ w is the rotation angle of the wheel of the target structure, L is the leg length of the target structure, θ h is the angle of the hip joint, that is, the angle between the straight line where the leg is and the hip joint, θ p M is the angle of inclination of the center of gravity, that is, the angle between the straight line where the legs are located and the vertical direction. s is the inertia matrix of the stance phase, , J w is the moment of inertia of the wheels of the target structure, m w is the mass of the wheel of the target structure, R w is the radius of the wheel of the target structure, m b is the mass of the base of the target structure, J b is the moment of inertia of the base of the target structure; C s is the Coriolis force matrix during the stance phase, ,θ 1 p is θ p The first derivative of L 1 is the first derivative of L, G s is the gravity vector in the stance phase, , g is the gravitational acceleration, u is the control input variable of the target structure, , τ w is the wheel torque of the target structure, τ h is the moment of the hip joint of the target structure, F L It is the generalized force on the leg of the target structure, which can be calculated using the principle of virtual work according to the angle of the knee joint or the leg length and the torque of the motor controlling the knee joint.

[0032] The embodiment of the present invention simplifies the model (1) based on the above model (1). The model simplification of the present invention is implemented based on the following simplification method:

[0033] First, when simplifying the model, it is necessary to ensure that the key dynamic characteristics of the system are retained, that is, the core characteristics that are crucial to the overall behavior, control effect and stability of the system. These characteristics often directly affect the main functions and performance of the system. For example, the center of mass position, pitch angle, wheel rotation speed and control torque in a two-wheeled legged robot are all necessary for the system to maintain balance and stability. Therefore, during the simplification process, these critical characteristics must be accurately described and retained to ensure that the simplified model can still reflect the basic behavior of the system and does not lose control of key indicators such as control accuracy and motion effect.

[0034] Secondly, some minor dynamic characteristics that have little impact on the overall performance of the system can be appropriately ignored according to the specific situation. These minor characteristics may include high-frequency vibrations, small-range friction changes, and extremely small nonlinear effects. Although they do exist in real systems, their impact on the main dynamic behavior of the system is relatively small. Ignoring these characteristics can reduce the complexity of the model, thereby simplifying the calculation and improving the operability and efficiency of the model. In practical applications, this simplification can not only speed up the calculation speed, especially in real-time control systems, but also make the controller design more intuitive and easy to implement.

[0035] Finally, in the process of simplifying the model, the key is to find a balance point, ensuring that the core dynamic characteristics of the system are not ignored, but also avoiding the introduction of too much complexity due to secondary characteristics. Through moderate simplification, the controllability of the system can be improved and the computational burden can be reduced without sacrificing performance or stability. This method is widely used in the modeling process of complex systems, so that the simplified model can meet the requirements of real-time control while retaining the accuracy and controllability of the system.

[0036] Based on the above simplified method, in the embodiment of the present invention, Figure 2 The simplified structure obtained by simplifying the structure shown in FIG. Figure 3 The present invention is based on Figure 3 The structure shown in the figure simplifies the above model (1) and obtains the model shown in the following model (2) as the dynamic model describing the core dynamic characteristics of the robot's equilibrium state:

[0037] (J w +m w ×R w 2 +m b ×R w2 +m b ×R w 2 × L)×θ 2 w + (J b +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0(2);

[0038] Among them, θ 2 p is θ p The second derivative is the angular acceleration of the forward tilt angle of the center of gravity, θ 2 w is θ w The second derivative of is the angular acceleration of the wheel's rotation angle.

[0039] S200, optimizing the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model to obtain a preliminary optimized trajectory; wherein the initial trajectory optimization model is constructed based on constraints that need to be added for the task performed by the two-wheeled legged robot.

[0040] In an embodiment of the present invention, the initial trajectory of the two-wheeled legged robot can be obtained based on the starting point, end point and environmental constraints of the task performed by the two-wheeled legged robot. Specifically, a collision-free path can be generated as a preliminary path using a heuristic global path planning algorithm according to the given task objectives and environmental constraints; then, key path points on the path are selected and time is allocated to each path segment. The coordinates of each key path point define the position of the robot's center of gravity in three-dimensional space. Finally, a preliminary optimized trajectory is generated by a trajectory parameterization method using time and waypoints as parameters.

[0041] Those skilled in the art know that any method for generating a corresponding trajectory based on the starting point, end point and environmental constraints of the task performed by the two-wheeled and legged robot falls within the scope of protection of the present invention.

[0042] In the embodiment of the present invention, constraints can be added and target loss functions can be set according to the specific tasks of the robot, such as obstacle avoidance, speed limit, acceleration limit, time cost and energy cost, etc., to construct corresponding optimization problems and solve the optimal trajectory. Furthermore, in the embodiment of the present invention, the initial trajectory optimization model satisfies the following conditions:

[0043] min c，T J(c,T)=λ s ×J s +λ t ×Jt +λ m ×J m +λ d ×J d ;

[0044] Where c is the trajectory parameter, T is the time, J(c, T) is the total loss function of the trajectory generated during the optimization process of the initial trajectory optimization model, and J s is the loss function that characterizes the collision cost of the trajectory, λ s For J s The corresponding weight, J t is the loss function that characterizes the time cost of the trajectory, λ t For J t The corresponding weight, J m is the loss function that characterizes the smoothness cost of the trajectory, λ m For J m The corresponding weight, J d is the loss function that characterizes the dynamic feasibility cost of the trajectory, λ d For J d The corresponding weight.

[0045] In the embodiment of the present invention, a differentiable cost function is used to penalize the distance value. The collision cost of the trajectory increases rapidly to infinity near the obstacle and remains stable far away from the obstacle. Specifically, J s The following conditions must be met:

[0046] J s = exp(-(d i -d0) / γ1);

[0047] Among them, d i is the distance between the i-th sampling path point sampled on the trajectory during the optimization process of the initial trajectory optimization model and the nearest obstacle corresponding to the sampling path point, which can be the Euclidean distance. i It can be obtained based on the coordinates of the i-th sampling path point and the coordinates of the corresponding nearest obstacle. The coordinates of the nearest obstacle corresponding to the i-th sampling path point can be obtained by the existing technology. d0 is the minimum distance threshold, which can be set based on actual needs and can be an empirical value. z is the number of sampling path points on the trajectory during the optimization process of the initial trajectory optimization model, which can be determined based on actual needs. γ1 is the first smoothing factor, which can be an empirical value. In an illustrative embodiment, γ1=0.01. exp() is an exponential function, and the value range of i is 1 to z.

[0048] Furthermore, in the embodiment of the present invention, J t The following conditions must be met:

[0049] ;

[0050] Among them, t j The time corresponding to the jth path segment on the trajectory during the optimization process of the initial trajectory optimization model is the time required for the robot to move from the starting point of the jth path segment to the end point of the jth path segment. The value of j ranges from 1 to p, and p is the number of path segments on the trajectory during the optimization process of the initial trajectory optimization model. The path between two adjacent key path points forms a path segment.

[0051] Furthermore, in the embodiment of the present invention, the rate of change of acceleration, i.e., jerk, is used as an indicator of smoothness. Specifically, J m The following conditions must be met:

[0052] ;

[0053] Wherein, jerk(t) is the jump at the optimization time t during the optimization process of the initial trajectory optimization model, T is the total duration of the trajectory, and dt represents the integral. It is known to those skilled in the art that the jump at the optimization time t can be obtained by taking the third-order derivative of the position of the path point corresponding to the time.

[0054] Furthermore, in the embodiment of the present invention, J d The following conditions must be met:

[0055] J d = exp(-(v i -v0) / γ2)+ exp(-(a i -a0) / γ3);

[0056] Among them, v i is the velocity of the i-th sampling path point sampled on the trajectory during the optimization process of the initial trajectory optimization model, a i is the acceleration of the i-th sampling path point, v0 is the set minimum speed threshold, a0 is the set minimum acceleration threshold, γ2 is the second smoothing factor, γ3 is the third smoothing factor, γ2 and γ3 can be empirical values, and in an exemplary embodiment, can be 0.01 respectively.

[0057] In the embodiment of the present invention, collision avoidance and dynamic feasibility are used to ensure safety and feasibility, trajectory smoothness is used to improve the smoothness and controllability of movement, and time minimization is used to quickly complete the task. The priorities of different loss functions can be set according to the specific requirements of the robot to perform the task. For example, collision avoidance and dynamic feasibility are set to high priority, trajectory smoothness is set to medium priority, and time minimization is set to low priority. Among them, the weight of the loss function with a high priority is greater than that of other loss functions. In the embodiment of the present invention, λ s =λ d >λ m >λ t .

[0058] In the embodiment of the present invention, in the process of optimizing the initial trajectory of the two-wheeled legged robot using the initial trajectory optimization model, the quasi-Newton method can be used for optimization to obtain the preliminary optimized trajectory. It is known to those skilled in the art that any method of using the quasi-Newton method to solve the initial trajectory optimization model to obtain the preliminary optimized trajectory belongs to the protection scope of the present invention.

[0059] The preliminary optimized trajectory obtained by the initial trajectory optimization model in the embodiment of the present invention can ensure smooth collision avoidance and meet the constraints of speed and acceleration.

[0060] S300, optimizing the preliminary optimized trajectory using a target trajectory optimization model to obtain a target optimized trajectory; the target trajectory optimization model is constructed based on a dynamic model of the two-wheeled legged robot.

[0061] Specifically, the target trajectory optimization model meets the following conditions:

[0062] minJ= ([(x k -x kr ) T ]Q(x k -x kr )+(u k ) T R(u k ));

[0063] The constraints include:

[0064] (1)(J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2 × L)×θ 2 w + (Jb +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0;

[0065] (2) |v|<v max , |a|<a max ;

[0066] (3) x k (0) = x kr (0), x k (N) = x kr (N);

[0067] Among them, J is the total loss function of the trajectory generated during the optimization process of the target trajectory optimization model, u k represents the input of the target trajectory optimization model at the optimization time k, and the value range of k is 0 to N-1. In the embodiment of the present invention, the input of the target trajectory optimization model is acceleration.

[0068] Where N is the prediction step size of the target trajectory optimization model, which can be set based on actual needs. During the optimization process of the target trajectory optimization model, the optimization operation is performed at each optimization moment. The duration corresponding to the optimization moment k is k×dt, where dt is the time interval between two adjacent optimization moments, which can be an empirical value.

[0069] Among them, x k represents the state of the target trajectory optimization model at the optimization time k, x kr Represents x k The reference value of is the state at the time corresponding to the optimization time k on the preliminary optimization trajectory, and the state includes position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the center of gravity. That is, in this embodiment of the present invention, x k =(P k , v k ,θ pk ,θ 1 pk ), x kr =(P kr , v kr ,θ pkr ,θ 1 pkr ), where P k represents the position at the optimization time k, P kr Indicates P k The reference value is the position on the initial optimization trajectory corresponding to the optimization time k, v k represents the speed at optimization time k, v kr Indicates vk The reference value is the speed at the time corresponding to the optimization time k on the initial optimization trajectory, θ pk represents the forward tilt angle of the center of gravity at the optimization time k, θ pkr Represents θ pk The reference value is specifically the forward tilt angle of the center of gravity at the time corresponding to the optimization time k on the initial optimization trajectory, θ 1 pk represents the angular velocity of the forward tilt angle of the center of gravity at the optimization time k, θ 1 pkr Represents θ 1 pk The reference value is specifically the angular velocity of the forward tilt angle of the center of gravity at the time corresponding to the optimization time k on the preliminary optimization trajectory.

[0070] Wherein, v represents the velocity on the trajectory during the optimization process of the target trajectory optimization model, a represents the acceleration on the trajectory during the optimization process of the target trajectory optimization model, and v max is the maximum speed threshold, a max is the maximum acceleration threshold, which can be set based on actual conditions, for example, it can be an empirical value.

[0071] Among them, x k (0) represents the state at the starting point of the trajectory during the optimization process of the target trajectory optimization model, x kr (0) represents the state at the starting point of the initial optimization trajectory, x k (N) represents the state at the end point of the trajectory during the optimization process of the target trajectory optimization model, x kr (N) represents the state at the end point of the preliminary optimization trajectory.

[0072] It is known to those skilled in the art that x k (0) = x kr (0) indicates that the position, speed, center of gravity forward tilt angle and angular velocity of the center of gravity forward tilt angle at the starting point of the trajectory in the optimization process of the target trajectory optimization model are equal to the position, speed, center of gravity forward tilt angle and angular velocity of the center of gravity forward tilt angle at the starting point of the preliminary optimization trajectory, respectively. k (0) = x kr (0) indicates that the position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the end point of the trajectory in the optimization process of the target trajectory optimization model are respectively equal to the position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the end point of the preliminary optimization trajectory.

[0073] In the embodiment of the present invention, the forward tilt angle of the center of gravity and the acceleration of the forward tilt angle of the center of gravity on the preliminary optimization trajectory are both zero.

[0074] Furthermore, Q is the state weight matrix, Q=(w1,w2,w3,w4), w1 is the weight of the position, w2 is the weight of the velocity, w3 is the weight of the forward inclination of the center of gravity, and w4 is the weight of the angular velocity of the forward inclination of the center of gravity. R is the input weight matrix, that is, the weight of the acceleration.

[0075] In the embodiment of the present invention, w1, w2, w3, w4 and w u The weights can be set based on actual needs. In an exemplary embodiment, the weights of position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the center of gravity can be set to 1, 10, 1, 10, respectively. The weight of acceleration can be set to 1.

[0076] In an embodiment of the present invention, if the state weight matrix Q is large and the input weight matrix R is small, the controller will pay more attention to the tracking performance of the state, but the control input may become large or even aggressive. On the contrary, if R is large, the controller will be more inclined to reduce the amplitude of the control input to make the control smoother, but the tracking performance may be sacrificed. After the target trajectory optimization model is constructed, the target trajectory optimization model can be solved by combining the optimization software for quadratic programs (OSQP) and the optimization library of Eigen (abbreviated as: OSQP-Eigen optimization library) to obtain the target optimization model.

[0077] In the embodiment of the present invention, since the target trajectory optimization model takes into account a partial dynamic model of the robot, the preliminary optimized trajectory is further optimized to expand the dimension of the trajectory so that the optimized target optimized trajectory not only includes the spatial information and time information of the robot's center of gravity, but also includes other state information that can describe the main motion characteristics of the robot, such as the forward inclination angle of the center of gravity and the angular velocity of the forward inclination angle of the center of gravity. This can ensure the balance and dynamic stability of the system, make the trajectory easier to be tracked by the controller, and improve the robot's tracking accuracy of the trajectory while reducing the requirements of the robot on the controller to achieve high-precision trajectory tracking, thereby enhancing the robustness of the control part.

[0078] Furthermore, the method provided in the embodiment of the present invention further includes the following steps:

[0079] S500: Track the target optimization trajectory using a linear quadratic regulator.

[0080] In the embodiment of the present invention, the state space equation of the system corresponding to the robot is as follows:

[0081] x 1 s =Ax s +Bu s ,y s =xs ;

[0082] x s =[θ w ,θ p ,θ 1 w ,θ 1 p ]

[0083] Among them, x s represents the state variable of the system, u s represents the input of the system, y s represents the output of the system, A and B are based on x s The matrix obtained by the relationship between 1 s For x s The first derivative of represents the velocity corresponding to the state variable.

[0084] In the embodiment of the present invention, the target structure can be regarded as a system. The control law of the corresponding linear quadratic regulator can be obtained by solving the Riccati algebraic equation.

[0085] In the application of linear quadratic regulators, if the dynamic model of the robot is not considered and the predetermined trajectory is not properly processed, it is difficult to ensure the precision and accuracy of trajectory tracking by relying solely on the linear quadratic regulator. In addition, the performance of the linear quadratic regulator is very sensitive to parameter selection and needs to be carefully adjusted to achieve the best effect. This brings several problems. The first is the increased difficulty of debugging: a small change in the parameters may lead to a significant change in the performance of the controller, which makes it complicated and time-consuming to find the optimal parameter configuration; the second is the robustness problem: if the parameters are not optimized for all possible operating conditions, the linear quadratic regulator may perform poorly in the face of system parameter changes or external disturbances; and finally, the generalization problem: carefully adjusted parameters may only be applicable to specific systems or operating conditions, and may need to be readjusted when the system environment changes. If the optimization link of the target optimization model is added, the partial robot dynamic model is considered, and the trajectory is properly processed, not only can the accuracy of the linear quadratic regulator in trajectory tracking be significantly improved, but also the parameter sensitivity of the linear quadratic regulator can be effectively reduced. Such improvements make the system more stable in the face of changes and simplify the parameter optimization process of the controller.

[0086] Figures 4a to 4c The speed curve comparison results of the present invention in the physical simulation environment Gazebo are shown. Figure 4a To optimize the speed tracking curve of the front robot, Figure 4b The speed tracking curve of the robot after adopting the optimization method proposed by the present invention is shown. Figure 4cThe changes of robot speed tracking error before and after optimization are compared. Figures 4a to 4c It can be seen that the optimized tracking speed is closer to the reference speed and the error is significantly reduced, which shows that the method proposed in the present invention can significantly improve the speed tracking performance.

[0087] In summary, the method for trajectory planning of a two-wheeled legged robot provided in an embodiment of the present invention significantly improves the accuracy and stability of the robot controller by optimizing the controllability of the trajectory, so that it can track the planned trajectory more efficiently, thereby enhancing motion stability and control accuracy. In the trajectory planning process, multiple factors such as obstacle avoidance, speed limit, acceleration limit, time cost, etc. are fully considered to ensure the smoothness and feasibility of the trajectory. Especially in the trajectory adjustment and optimization stage, the present invention efficiently optimizes the planned trajectory by deeply combining the robot's dynamic model, so that the trajectory not only meets the dynamic constraints, but also facilitates the precise tracking of the controller. The optimization at this stage ensures that the robot can still achieve stable and precise movement in a dynamic environment. This method not only provides strong support for the practical application of two-wheeled legged robots, but also has versatility and can be extended to other types of mobile robots.

[0088] Based on the same inventive concept, the present invention provides a trajectory planning device for a two-wheeled legged robot, the device comprising:

[0089] The dynamic model building module is used to simplify the two-wheeled legged robot into a structure including a base, legs and wheels as a target structure, and to build a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure.

[0090] The initial trajectory optimization module is used to optimize the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model to obtain a preliminary optimized trajectory; wherein the initial trajectory optimization model is constructed based on the constraints that need to be added for the task performed by the two-wheeled legged robot.

[0091] The target trajectory optimization module is used to optimize the preliminary optimization trajectory using a target trajectory optimization model to obtain a target optimization trajectory; the target trajectory optimization model is constructed based on the dynamics model of the two-wheeled legged robot.

[0092] The kinetic model satisfies the following conditions:

[0093] (J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2× L)×θ 2 w + (J b +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0; where J w is the moment of inertia of the wheels of the target structure, m w is the mass of the wheel of the target structure, R w is the radius of the wheel of the target structure, m b is the mass of the base of the target structure, J b is the moment of inertia of the base of the target structure, θ w is the rotation angle of the wheel of the target structure, L is the leg length of the target structure, θ p is the forward tilt angle of the center of gravity of the target structure, g is the gravitational acceleration, θ 2 p is θ p The second derivative, θ 2 w is θ w The second derivative of .

[0094] Specifically, the target trajectory optimization model meets the following conditions:

[0095] minJ= ([(x k -x kr ) T ]Q(x k -x kr )+(u k ) T R(u k ));

[0096] The constraints include:

[0097] (1)(J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2 × L)×θ 2 w + (J b +m b ×L 2 )×θ 2 p -mb ×L×g×sinθ p =0;

[0098] (2) |v|<v max , |a|<a max ;

[0099] (3) x k (0) = x kr (0), x k (N) = x kr (N);

[0100] Among them, J is the total loss function of the trajectory generated during the optimization process of the target trajectory optimization model, u k represents the input of the target trajectory optimization model at the optimization time k, and the value range of k is 0 to N-1. In the embodiment of the present invention, the input of the target trajectory optimization model is acceleration.

[0101] Where N is the prediction step size of the target trajectory optimization model, which can be set based on actual needs. During the optimization process of the target trajectory optimization model, the optimization operation is performed at each optimization moment. The duration corresponding to the optimization moment k is k×dt, where dt is the time interval between two adjacent optimization moments, which can be an empirical value.

[0102] Among them, x k represents the state of the target trajectory optimization model at the optimization time k, x kr Represents x k The reference value of is the state at the time corresponding to the optimization time k on the preliminary optimization trajectory, and the state includes position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the center of gravity. That is, in this embodiment of the present invention, x k =(P k , v k ,θ pk ,θ 1 pk ), x kr =(P kr , v kr ,θ pkr ,θ 1 pkr ), where P k represents the position at the optimization time k, P kr Indicates P k The reference value is the position on the initial optimization trajectory corresponding to the optimization time k, v k represents the speed at optimization time k, v kr Indicates v k The reference value is the speed at the time corresponding to the optimization time k on the initial optimization trajectory, θ pk represents the forward tilt angle of the center of gravity at the optimization time k, θpkr Represents θ pk The reference value is specifically the forward tilt angle of the center of gravity at the time corresponding to the optimization time k on the initial optimization trajectory, θ 1 pk represents the angular velocity of the forward tilt angle of the center of gravity at the optimization time k, θ 1 pkr Represents θ 1 pk The reference value is specifically the angular velocity of the forward tilt angle of the center of gravity at the time corresponding to the optimization time k on the preliminary optimization trajectory.

[0103] Wherein, v represents the velocity on the trajectory during the optimization process of the target trajectory optimization model, a represents the acceleration on the trajectory during the optimization process of the target trajectory optimization model, and v max is the maximum speed threshold, a max is the maximum acceleration threshold, which can be set based on actual conditions, for example, it can be an empirical value.

[0104] Among them, x k (0) represents the state at the starting point of the trajectory during the optimization process of the target trajectory optimization model, x kr (0) represents the state at the starting point of the initial optimization trajectory, x k (N) represents the state at the end point of the trajectory during the optimization process of the target trajectory optimization model, x kr (N) represents the state at the end point of the preliminary optimization trajectory.

[0105] It is known to those skilled in the art that x k (0) = x kr (0) indicates that the position, speed, center of gravity forward tilt angle and angular velocity of the center of gravity forward tilt angle at the starting point of the trajectory in the optimization process of the target trajectory optimization model are equal to the position, speed, center of gravity forward tilt angle and angular velocity of the center of gravity forward tilt angle at the starting point of the preliminary optimization trajectory, respectively. k (0) = x kr (0) indicates that the position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the end point of the trajectory in the optimization process of the target trajectory optimization model are respectively equal to the position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the end point of the preliminary optimization trajectory.

[0106] In the embodiment of the present invention, the forward tilt angle of the center of gravity and the acceleration of the forward tilt angle of the center of gravity on the preliminary optimization trajectory are both zero.

[0107] Furthermore, Q is the state weight matrix, Q=(w1,w2,w3,w4), w1 is the weight of the position, w2 is the weight of the velocity, w3 is the weight of the forward inclination of the center of gravity, and w4 is the weight of the angular velocity of the forward inclination of the center of gravity. R is the input weight matrix, that is, the weight of the acceleration.

[0108] In the embodiment of the present invention, w1, w2, w3, w4 and w u The weights can be set based on actual needs. In an exemplary embodiment, the weights of position, speed, forward inclination angle of the center of gravity, and angular velocity of the forward inclination angle of the center of gravity can be set to 1, 10, 1, 10, respectively. The weight of acceleration can be set to 1.

[0109] The device can be used to perform Figure 1 Therefore, for the functions that can be realized by each functional module of the device, please refer to Figure 1 The description of the illustrated embodiment is not repeated in detail.

[0110] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps described in the present invention can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solution disclosed in the present invention can be achieved, and this document does not limit this.

[0111] The above specific implementations do not constitute a limitation on the protection scope of the present invention. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A trajectory planning method for a two-wheeled legged robot, characterized in that: The method comprises the following steps: S100, simplifying the two-wheeled legged robot into a structure including a base, legs and wheels as a target structure, and constructing a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure; S200, optimizing the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model to obtain a preliminary optimized trajectory; wherein the initial trajectory optimization model is constructed based on constraints that need to be added for the two-wheeled legged robot to perform a task; S300, optimizing the preliminary optimized trajectory using a target trajectory optimization model to obtain a target optimized trajectory; the target trajectory optimization model is constructed based on a dynamics model of the two-wheeled legged robot; The kinetic model meets the following conditions: (J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2 ×L)×θ 2 w +(J b +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0; Among them, J w is the moment of inertia of the wheels of the target structure, m w is the mass of the wheel of the target structure, R w is the radius of the wheel of the target structure, m b is the mass of the base of the target structure, J b is the moment of inertia of the base of the target structure, θ w is the rotation angle of the wheel of the target structure, L is the leg length of the target structure, θ p is the forward tilt angle of the center of gravity of the target structure, g is the gravitational acceleration, θ 2 p is θ p The second derivative, θ 2 w is θ w The second derivative of Wherein, the target trajectory optimization model satisfies the following conditions: ； The constraints include: (1)(J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2 ×L)×θ 2 w +(J b +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0; (2)|v|<v max ,|a|<a max ; (3)x k (0)=x kr (0),x k (N)=x kr (N); Among them, J is the total loss function of the trajectory, u k represents the input of the target trajectory optimization model at the optimization time k, the value range of k is 0 to N-1, N is the prediction step length of the target trajectory optimization model, x k represents the state of the target trajectory optimization model at the optimization time k, x kr Represents x k The reference value is the state on the preliminary optimization trajectory at the time corresponding to the optimization time k, and the state includes position, speed, center of gravity forward inclination angle, and angular velocity of the center of gravity forward inclination angle; v represents the speed on the trajectory during the optimization process of the target trajectory optimization model, a represents the acceleration on the trajectory during the optimization process of the target trajectory optimization model, and v max is the maximum speed threshold, a max is the maximum acceleration threshold, x k (0) represents the state at the starting point of the trajectory during the optimization process of the target trajectory optimization model, x kr (0) represents the state at the starting point of the initial optimization trajectory, x k (N) represents the state at the end point of the trajectory during the optimization process of the target trajectory optimization model, x kr (N) represents the state at the end point of the preliminary optimization trajectory, Q is the state weight matrix, and R is the input weight matrix.

2. The method according to claim 1, characterized in that: Also includes the following steps: S400: Track the target optimization trajectory using a linear quadratic regulator.

3. The method according to claim 1, characterized in that The initial trajectory optimization model meets the following conditions: minutes c，T J(c,T)=λ s ×J s +λ t ×J t +λ m ×J m +λ d ×J d ; Among them, c is the trajectory parameter, T is the total duration of the trajectory, J(c, T) is the total loss function of the trajectory, and J s is the loss function that characterizes the collision cost of the trajectory, λ s For J s The corresponding weight, J t is the loss function that characterizes the time cost of the trajectory, λ t For J t The corresponding weight, J m is the loss function that characterizes the smoothness cost of the trajectory, λ m For J m The corresponding weight, J d is the loss function that characterizes the dynamic feasibility cost of the trajectory, λ d For J d The corresponding weight.

4. The method according to claim 3, characterized in that J s The following conditions must be met: ； Among them, d i is the distance between the i-th sampling path point sampled on the trajectory during the optimization process of the initial trajectory optimization model and the nearest obstacle corresponding to the sampling path point, d0 is the set minimum distance threshold, z is the number of sampling path points on the trajectory during the optimization process of the initial trajectory optimization model, γ1 is the first smoothing factor, exp() is the exponential function, and the value range of i is 1 to z.

5. The method according to claim 3, characterized in that: J t The following conditions must be met: ； Among them, t j is the time corresponding to the jth path segment on the trajectory during the optimization process of the initial trajectory optimization model. The value of j ranges from 1 to p, and p is the number of path segments on the trajectory during the optimization process of the initial trajectory optimization model.

6. The method according to claim 3, characterized in that J m The following conditions must be met: ； Among them, jerk(t) is the jump at the optimization time t in the optimization process of the initial trajectory optimization model, and dt represents the integral.

7. The method according to claim 3, characterized in that J d The following conditions must be met: ； Among them, v i is the velocity of the i-th sampling path point sampled on the trajectory during the optimization process of the initial trajectory optimization model, a i is the acceleration of the i-th sampling path point, v0 is the set minimum speed threshold, a0 is the set minimum acceleration threshold, γ2 is the second smoothing factor, γ3 is the third smoothing factor, z is the number of sampling path points on the trajectory during the optimization process of the initial trajectory optimization model, and the value range of i is 1 to z.

8. A trajectory planning device for a two-wheeled and footed robot, characterized in that: The device comprises: A dynamic model building module is used to simplify the two-wheeled legged robot into a structure including a base, legs and wheels as a target structure, and to build a dynamic model describing the core dynamic characteristics of the robot's equilibrium state based on the target structure; An initial trajectory optimization module, used to optimize the initial trajectory of the two-wheeled legged robot using an initial trajectory optimization model to obtain a preliminary optimized trajectory; wherein the initial trajectory optimization model is constructed based on constraints that need to be added for the task performed by the two-wheeled legged robot; A target trajectory optimization module, used to optimize the preliminary optimization trajectory using a target trajectory optimization model to obtain a target optimization trajectory; the target trajectory optimization model is constructed based on a dynamic model of the two-wheeled legged robot; The kinetic model meets the following conditions: (J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2 ×L)×θ 2 w +(J b +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0; Among them, J w is the moment of inertia of the wheels of the target structure, m w is the mass of the wheel of the target structure, R w is the radius of the wheel of the target structure, m b is the mass of the base of the target structure, J b is the moment of inertia of the base of the target structure, θ w is the rotation angle of the wheel of the target structure, L is the leg length of the target structure, θ p is the forward tilt angle of the center of gravity of the target structure, g is the gravitational acceleration, θ 2 p is θ p The second derivative, θ 2 w is θ w The second derivative of Wherein, the target trajectory optimization model satisfies the following conditions: ； The constraints include: (1)(J w +m w ×R w 2 +m b ×R w 2 +m b ×R w 2 ×L)×θ 2 w +(J b +m b ×L 2 )×θ 2 p -m b ×L×g×sinθ p =0; (2)|v|<v max ,|a|<a max ; (3)x k (0)=x kr (0),x k (N)=x kr (N); Among them, J is the total loss function of the trajectory, u k represents the input of the target trajectory optimization model at the optimization time k, the value range of k is 0 to N-1, N is the prediction step length of the target trajectory optimization model, x k represents the state of the target trajectory optimization model at the optimization time k, x kr Represents x k The reference value is the state on the preliminary optimization trajectory at the time corresponding to the optimization time k, and the state includes position, speed, center of gravity forward inclination angle, and angular velocity of the center of gravity forward inclination angle; v represents the speed on the trajectory during the optimization process of the target trajectory optimization model, a represents the acceleration on the trajectory during the optimization process of the target trajectory optimization model, and v max is the maximum speed threshold, a max is the maximum acceleration threshold, x k (0) represents the state at the starting point of the trajectory during the optimization process of the target trajectory optimization model, x kr (0) represents the state at the starting point of the initial optimization trajectory, x k (N) represents the state at the end point of the trajectory during the optimization process of the target trajectory optimization model, x kr (N) represents the state at the end point of the preliminary optimization trajectory, Q is the state weight matrix, and R is the input weight matrix.

Images