A method, system, device and terminal for planning motion trajectory of a legged robot

Through the three-layer planning method, combined with nonlinear dynamic optimization and model prediction control, the problem of slow motion speed and single trajectory of foot robots is solved, and efficient, safe and precise motion in complex environments is achieved, which is suitable for a variety of practical purposes Application scenario.

CN117572773BActive Publication Date: 2024-11-22HUBEI QILING ROBOT CO LTD
View PDF 5 Cites 0 Cited by

Patent Information

Application Number
CN202311582605.1
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2023-11-24
Publication Date
2024-11-22
Estimated Expiration
2043-11-24

AI Technical Summary

Technical Problem

The existing foot-type robot motion control has problems such as slow motion speed, single motion trajectory, and difficult to guarantee real-time performance of planning algorithms, especially in complex dynamic models and high-dimensional planning problems.

Method used

Three-layer planning method is adopted, including upper-level global collision-free trajectory generation, middle-level nonlinear dynamic trajectory optimization, and underlying model prediction controller state trajectory tracking. Optimization indicators include obstacle avoidance cost, state trajectory smoothness, robot dynamics limitation and omnidirectional Motion constraints, robot motion control is performed through polynomial trajectory and time optimization variables, combined with model prediction controllers.

Benefits of technology

It realizes efficient, safe and precise movement of foot robots in complex environments, improves the real-time planning and trajectory quality, exerts the robot's motion potential, and is suitable for a variety of practical application scenarios.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN117572773B_ABST
    Figure CN117572773B_ABST
Patent Text Reader

Abstract

The invention belongs to the field of robot technology, and discloses a method, system, device and terminal for planning the motion trajectory of a legged robot, comprising: generating a global collision-free trajectory at an upper layer, optimizing a nonlinear dynamic trajectory at a middle layer, and tracking an expected state trajectory of a bottom layer model predictive control; the upper layer module quickly generates a rough polynomial trajectory of the robot's center of mass motion on a global obstacle map, comprising a plane position [x, y], a direction angle θ and a trajectory time T of the robot's motion; the middle layer module constructs a nonlinear optimization problem according to an initial trajectory and robot dynamics, wherein the optimization indicators include obstacle avoidance cost, state trajectory smoothness, robot dynamics restriction, omnidirectional motion constraint of the legged robot and trajectory time cost, and the optimization variables are trajectory polynomial coefficients and time expressed in segments; the bottom layer module uses the optimized trajectory as an expected center of mass spatiotemporal state trajectory of a nonlinear model predictive controller to perform specific motion control of the robot.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of robots, and in particular relates to a method, system, device and terminal for motion trajectory planning of a legged robot. Background Art

[0002] Legged robots are bionic robots that imitate the movements of legged mammals in nature, such as humans and cats. Legged robots have omnidirectional movement capabilities and the ability to pass through complex terrain. Nowadays, the body motion control of legged robots is at a high level, and they have certain actual animal movement capabilities, such as translation, turning, jumping, and walking on rough ground. However, most of the motion control involves a single motion mode. The flexible movement of animals in the real world is a combination of various motion modes. The current various motion controllers are still some distance away from achieving the agile movement of animals.

[0003] Model predictive controllers are widely used in the motion control of legged robots with collision-free constraints to achieve robot navigation planning. However, the prediction cycle of the model predictive controller greatly affects the control effect. A long prediction cycle leads to a long solution time, and a short prediction cycle easily causes the optimization solution to fall into a local optimum. Learning-based methods can achieve real-time navigation of robots, but they require pre-training or the use of data sets. These data sets are mainly obtained in simulated environments, and there will be differences when used in actual environments, and the production cost of real-world data sets is very high. The trajectory generated by traditional sampling or search-based path planning methods is difficult to conform to the complex dynamic model of legged robots, so large errors or slow tracking speeds are likely to occur during trajectory tracking.

[0004] Through the above analysis, the problems and defects of the prior art are as follows:

[0005] (1) The actual movement of the robot is slow: the potential speed of the legged robot is not fully utilized, and the robot moves slowly to ensure the trajectory tracking effect;

[0006] (2) The robot's motion trajectory is single: most of them are geometric trajectories in space, which only reflect the obstacle avoidance ability but not the flexibility of the legged robot's motion;

[0007] (3) The real-time performance of the planning algorithm is difficult to ensure: The dynamic model of the legged robot is very complex, the dimension of the planning problem is large, the planning solution time is long, and the real-time performance is difficult to ensure. Summary of the invention

[0008] In view of the problems existing in the prior art, the present invention provides a method, system, device and terminal for motion trajectory planning of a legged robot.

[0009] The present invention is implemented as follows: a motion trajectory planning method for a legged robot, wherein the motion trajectory planning method for the legged robot has three layers, namely: an upper-layer global collision-free trajectory generation module, a middle-layer nonlinear dynamic trajectory optimization module, and a bottom-layer model prediction controller state trajectory tracking module. The upper-layer module quickly generates a rough polynomial trajectory of the robot's center of mass motion on a global 2D obstacle map, including the plane position [x, y] of the robot's motion, the direction angle θ, and the trajectory duration T; the middle-layer module constructs a nonlinear optimization problem based on the searched initial trajectory and the robot's dynamics, and the optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics restrictions (speed, acceleration), omnidirectional motion constraints of the legged robot, and trajectory time cost, and the optimization variables are the coefficients of the polynomial trajectory expressed in segments and the duration of each segment of the trajectory; the bottom-layer module uses the optimized trajectory as the expected center of mass spatiotemporal state trajectory of the nonlinear model prediction controller, and uses the controller to track the motion trajectory of the robot's center of mass to perform motion control of the robot body.

[0010] Furthermore, in the generation of the upper-level global collision-free trajectory, the planned robot state is the plane position and orientation angle [x, y, θ], the given uniform space is discretized into g×g grids, and each grid is associated with the corresponding state P 2D (idx, idy) = [x, y, θ] association, the sampling strategy is:

[0011] x=idx·grid+rand(-1,1)·bias

[0012] y=idy·grid+rand(-1,1)·bias.

[0013] Where (idx, idy) is the index of the state point, grid is the grid size, g is the number of discrete grids, P 2D is a state point, stored in the state point set RoadMap, and the total number of state points is g×g=n.

[0014] Furthermore, the legged robot can move in all directions, so the state quantities [x, y, θ] are considered separately, and the connection between the two state points is constructed as an optimal boundary value problem. The initial state is given as s i =[s pi ,s vi ], is the state of the parent node, and the final state is s f =[s pf ,s vf ], the end position is the position of the child node, the end speed is obtained by solving, and the energy of the entire state trajectory is optimized J(T) = ∫ T s a (t) 2dt is minimum (i.e., the acceleration integral is minimum), and using the Pondriagin maximum principle, the explicit solution of the state estimation is obtained as follows:

[0015]

[0016]

[0017]

[0018] The numerical solution of this problem requires trajectory time T, given the reference line velocity v ref and angular velocity ω ref Set reference time T = T ref :=max(||[Δx, Δy]||2 / v ref , Δθ / ω ref ).

[0019] Furthermore, in nonlinear dynamic trajectory optimization, the optimization indicators include: the smoothness cost of the state trajectory, with a weight of λ s , the cost of the state trajectory distance obstacle, with weight λ c and the time cost of the trajectory segment, with weight λ t , limiting robot dynamics such as maximum velocity and maximum acceleration to The robot's omnidirectional motion is limited to The continuity restriction of the previous and next states at a state point is As the constraint of the optimization problem, the solution variables of the optimization problem are the polynomial coefficient c and time T of each state trajectory:

[0020]

[0021]

[0022]

[0023]

[0024]

[0025] Where s(t) is the nth-order polynomial trajectory of the state variables x, y, and θ, N is the number of polynomial segments, j represents the jth segment, R is the positive definite weight matrix of the state smoothness cost, which measures the cost ratio between the three state variables, T is the time vector of the trajectory segment, and T j is the duration of the jth polynomial, c ji Represents the nth-order coefficient vector of the jth-segment polynomial.

[0026] Furthermore, in the state trajectory tracking of the model predictive controller, a nonlinear model predictive control problem is constructed:

[0027]

[0028]

[0029]

[0030] g(x,u,t)=0

[0031] h(x,u,t)<0

[0032] where x(t) and u(t) are the state variables and state inputs, Φ(·) is the terminal state constraint cost function, L(·) is the quadratic cost function for trajectory tracking, is the current observation state. c (·), g(·) and h(·) are the system dynamic equations, equality constraints and inequality constraints respectively.

[0033] Speed ​​safety constraints are added to the model predictive controller to ensure stability during fast motion.

[0034]

[0035] where λ1 and λ θ Weigh the weights of translational velocity and angular velocity, is the safety threshold.

[0036] The model predictive controller solves and calculates the control instructions of the robot's joint motors based on the expected state trajectory and the robot's dynamic model to achieve autonomous movement of the robot.

[0037] Another object of the present invention is to provide a legged robot motion trajectory planning system using the legged robot motion trajectory planning method, comprising:

[0038] The upper module is used to quickly generate a rough polynomial trajectory of the robot's center of mass motion on the global obstacle map, including the robot's plane position [x, y], direction angle θ, and trajectory time T;

[0039] The middle-level module is used to construct a nonlinear optimization problem based on the initial trajectory and robot dynamics. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics constraints, legged robot omnidirectional motion constraints and trajectory time cost. The optimization variables are the trajectory polynomial coefficients expressed in pieces and time.

[0040] The bottom-level module is used to use the optimized trajectory as the expected center-of-mass spatiotemporal state trajectory of the nonlinear model predictive controller to perform specific motion control of the robot.

[0041] Another object of the present invention is to provide a computer device, which includes a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the legged robot motion trajectory planning method.

[0042] Another object of the present invention is to provide a computer-readable storage medium storing a computer program, which, when executed by a processor, enables the processor to execute the steps of the legged robot motion trajectory planning method.

[0043] Another object of the present invention is to provide an information data processing terminal, which is used to implement the legged robot motion trajectory planning system.

[0044] In combination with the above technical solutions and the technical problems solved, the advantages and positive effects of the technical solutions to be protected by the present invention are as follows:

[0045] First, as a new type of bionic robot, the dynamic model of the legged robot is complex, and the problems related to planning and control are less studied and difficult to solve. It is very difficult to extract the key motion problems with distinctive characteristics of the legged robot from the complex dynamic model, and to model and express and solve them. In addition, in order to ensure the real-time planning, it is necessary to balance the model complexity, problem complexity and solution efficiency to achieve online planning. The research and development of legged robots is for practical applications. Real-time navigation tasks in real unknown environments are the basis for the application of legged robots, and the autonomy of robots is the key issue for their wide application. Legged robots are used in real scenarios such as inspection, transportation, security, search and rescue, and pursuit. Autonomous movement is the basis for the implementation of various tasks. Flexible and agile real-time planning will greatly help the application of legged robots.

[0046] 1) Real-time motion trajectory and tracking of mobile robots Due to the limitation of onboard computing units, online planning takes a long time and efficiency is difficult to improve. The present invention uses a hierarchical planning method to balance the planning problem dimension and planning length at different planning levels, efficiently allocate computing resources, and achieve stable real-time planning.

[0047] 2) The hierarchical planning method is combined with trajectory search to find the global trajectory to avoid falling into the local optimal solution; trajectory optimization is used to improve the quality of the trajectory to make it more consistent with the actual motion characteristics of the legged robot; the model predictive controller tracks the trajectory of the predetermined period to ensure the motion trajectory tracking effect.

[0048] 3) Current planning methods for legged robots mostly consider motion stability, and the robot moves slowly. The present invention considers the anisotropy of the omnidirectional motion of the legged robot, plans the spatiotemporal trajectory of the robot motion (including the time series of position, velocity, and acceleration, not only the spatial geometric trajectory, but also the trajectory related to time in multiple dimensions), and uses multiple indicators to optimize the trajectory, giving full play to the advantages of the legged robot's rapid motion and flexibility.

[0049] Second, considering the technical solution as a whole or from the perspective of the product, the technical effects and advantages of the technical solution to be protected by the present invention are described in detail as follows:

[0050] 1) The present invention divides the planning task into three layers. The complexity of the problem increases from the top layer to the bottom layer, and the planning distance becomes shorter. Compared with a single planning layer, the computing resources are efficiently allocated to realize online planning.

[0051] 2) Compared with random sampling, the uniform sampling with random deviation in the present invention greatly speeds up the search efficiency while ensuring that the optimality of the generated trajectory remains almost unchanged.

[0052] 3) The trajectory cost calculation strategy used in the optimal trajectory search process takes into account the characteristics of legged robot movement. On the basis of the planar 2D position, the robot's orientation angle planning is additionally considered. While calculating the trajectory space geometric distance, the steering cost is also calculated, so that the initial trajectory is more in line with the robot dynamics and more targeted.

[0053] 4) The local trajectory optimization problem constructed by the present invention based on the polynomial trajectory in the search phase takes into account obstacle avoidance, dynamic constraints, and time optimization, which can further improve the overall trajectory quality. In particular, the anisotropic constraints of the omnidirectional motion of the legged robot are introduced, that is, the constraints on the omnidirectional motion speed and motion direction.

[0054] 5) The present invention uses a local optimization method of gradient descent for the nonlinear optimization problem and uses the search result as the initial solution, which can ensure the quality of solving the optimization problem, reduce the probability of abnormal results, and improve the solution speed of nonlinear optimization.

[0055] 6) The optimized trajectory is directly used as the desired state trajectory of the robot body nonlinear model prediction controller. Compared with the instruction tracking method using acceleration, velocity or position, it can achieve more complex motion behaviors and greatly improve the tracking effect.

[0056] Third, as auxiliary evidence of the inventiveness of the claims of the present invention, it is also reflected in the following important aspects:

[0057] (1) The expected benefits and commercial value of the technical solution of the present invention after transformation are:

[0058] The present invention is an invention of a motion planning method for a legged robot, which is the basis for the autonomous motion of a legged robot and has a wide range of application scenarios. At present, the motion trajectory planning algorithms for mobile robots are mostly drones and wheeled robots, whose configurations, dynamic models and motion modes are significantly different from those of legged robots. Related methods are difficult to play a good role on legged robots. This method is suitable for legged robots to perform inspection tasks or provide an autonomous navigation basis for complex tasks. The landing application of legged robots has a broad space and great market value. The motion trajectory planning method for legged robots is a very critical technology and is also the key to whether legged robots can play a role in various potential application scenarios. The invention and innovation of the planning method for legged robots has extremely high application value in the commercial field. In addition, the R&D costs are reflected in the development and testing stage. The cost of deploying algorithms on robots is very low, and the profit margin is huge. Whether it is in the form of technology authorization or stand-alone packaging of the algorithm core, there is a very considerable profit margin.

[0059] (2) The technical solution of the present invention fills the technical gap in the industry at home and abroad:

[0060] At present, the motion planning of legged robots is mostly focused on the stable motion level of the robot body, or the landing point planning of the legged robot on rugged terrain. The control goal is to expect the legged robot to be able to stably pass through various terrains. In these methods, stability is the most critical consideration, so most robots move slowly, which does not meet the use requirements in most scenarios for practical applications. The robot motion planning method proposed in the present invention pays more attention to the movement potential of the legged robot body, so that it can move quickly and flexibly in space, thereby realizing the autonomous movement of the legged robot in various general scenarios.

[0061] Fourth, the legged robot motion trajectory planning method provided by the present invention is a major innovation in robot path planning technology. The significant technological progress it brings is mainly reflected in the following aspects:

[0062] 1) Three-level planning framework: This method divides the robot’s motion trajectory planning into three levels: upper-level global collision-free trajectory generation, middle-level nonlinear dynamic trajectory optimization, and bottom-level model predictive control desired state trajectory tracking, thus achieving accurate and efficient motion planning.

[0063] 2) Omnidirectional motion planning: This method fully considers the omnidirectional motion characteristics of the legged robot, can optimize the generation of omnidirectional motion trajectories, and improves the robot's action efficiency in complex environments.

[0064] 3) Diversification of optimization indicators: In the nonlinear dynamic trajectory optimization, multiple optimization indicators such as obstacle avoidance cost, state trajectory smoothness, robot dynamics limitations, omnidirectional motion constraints and trajectory time cost are considered, balancing the needs of obstacle avoidance, smooth motion, rapid response and other aspects, and improving the robot's motion performance.

[0065] 4) Model predictive control desired state trajectory tracking: By using the optimized trajectory as the desired center of mass state trajectory of the model predictive controller, precise control of the robot's specific motion is achieved, improving the stability and accuracy of the robot's motion.

[0066] 5) Safety enhancement: The underlying model predictive controller adds speed safety constraints to ensure that the robot can maintain the safety of movement while meeting trajectory optimization requirements.

[0067] The legged robot motion trajectory planning method provided by the present invention can realize efficient, safe and precise motion of the robot in a complex environment, greatly improving the application value and practicality of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. 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.

[0069] Figure 1 is a schematic diagram of trajectory distance of a hierarchical planner provided by an embodiment of the present invention;

[0070] Figure 2 Schematic diagram of sampling search test provided by an embodiment of the present invention; wherein, (a) random sampling + Dijkstra's algorithm, (b) uniform sampling with random deviation + Dijkstra's algorithm, (c) uniform sampling with random deviation + LazyPRM* algorithm;

[0071] Figure 3 Schematic diagram of different results of trajectory calculation costs in the search phase provided by an embodiment of the present invention;

[0072] Figure 4 is a trajectory generation flow chart provided by an embodiment of the present invention;

[0073] Figure 5 is a schematic diagram of a robot body coordinate system provided by an embodiment of the present invention;

[0074] Figure 6 is a flowchart of a navigation framework provided by an embodiment of the present invention;

[0075] Figure 7 is a schematic diagram of a planning algorithm test provided by an embodiment of the present invention;

[0076] Figure 8 is a horizontal position x state quantity change trajectory diagram provided by an embodiment of the present invention;

[0077] Fig. 9 is a horizontal position y state quantity change trajectory diagram provided by an embodiment of the present invention;

[0078] Fig.10 is a trajectory diagram of the change of the direction angle θ state quantity provided by an embodiment of the present invention;

[0079] Fig.11 is a structural diagram of a motion trajectory planning system for a legged robot provided by an embodiment of the present invention;

[0080] Fig.12 This is the robot motion trajectory planning test tracking effect provided by the embodiment of the present invention;

[0081] Fig.13 It is a real-world test of a robot motion planning system at night provided by an embodiment of the present invention;

[0082] Fig.14 It is a daytime real-world test of the robot motion planning system provided by an embodiment of the present invention; DETAILED DESCRIPTION

[0083] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0084] In view of the problems existing in the prior art, the present invention provides a method, system, device and terminal for motion trajectory planning of a legged robot. The present invention is described in detail below with reference to the accompanying drawings.

[0085] Example 1: Service robot in indoor environment

[0086] In indoor environments, legged robots such as service robots need to avoid various obstacles, such as furniture, decorations, etc., and also need to reach the target location as quickly as possible. In this case, the above trajectory planning method can be used.

[0087] 1. The upper module generates a rough polynomial trajectory on the global indoor environment map (e.g., a 2D map generated by a lidar), which indicates the robot's preliminary running path.

[0088] 2. The middle-level module solves the nonlinear optimization problem based on the initial path and the dynamic model of the robot through an optimization algorithm (such as gradient descent or genetic algorithm) to generate an optimized trajectory that is smooth, obstacle-avoiding, and complies with omnidirectional motion constraints.

[0089] 3. The bottom-level module uses a model predictive controller to generate the robot’s motion control instructions, such as the motor’s speed and steering instructions, based on the optimized trajectory to achieve autonomous motion of the robot.

[0090] Example 2: Search and rescue robot in outdoor environment

[0091] In outdoor environments, such as mountainous areas, forests and other complex environments, search and rescue robots need to search for targets quickly while ensuring safety. In this case, the above trajectory planning method can also be used.

[0092] 1. The upper module generates a rough polynomial trajectory on a global outdoor environment map (e.g., a 3D map generated by a drone), which indicates the robot’s preliminary running path.

[0093] 2. The middle-level module solves the nonlinear optimization problem based on the initial path and the dynamic model of the robot through an optimization algorithm (such as particle swarm optimization algorithm or simulated annealing algorithm) to generate an optimized trajectory that is smooth, obstacle-avoiding, and complies with omnidirectional motion constraints.

[0094] 3. The bottom-level module uses a model predictive controller to generate the robot’s motion control instructions, such as motor speed and steering instructions, based on the optimized trajectory to achieve autonomous motion of the robot.

[0095] The hierarchical planning method of the present invention has three layers, namely: upper-level global collision-free trajectory generation, middle-level nonlinear dynamic trajectory optimization, and bottom-level model predictive control expected state trajectory tracking. The upper-level module quickly generates a rough polynomial trajectory of the robot's center of mass motion on the global obstacle map, which includes the plane position [x, y], direction angle [θ] and trajectory time T of the robot's motion. The middle-level module constructs a nonlinear optimization problem based on the initial trajectory obtained by the search and the robot dynamics model. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics restrictions, legged robot omnidirectional motion constraints and trajectory time cost. The optimization variables are the trajectory polynomial coefficients and time expressed in segments. The bottom-level module uses the optimized trajectory as the expected center of mass spatiotemporal state trajectory of the nonlinear model predictive controller to perform specific motion control of the robot. The planning trajectory distance of the hierarchical planner is as follows: Figure 1 shown.

[0096] Global collision-free trajectory generation: A legged robot has six degrees of freedom, but the main focus during movement is only the horizontal position and orientation angle, because the body height, pitch angle, and roll angle will change according to the terrain. Therefore, the planned robot state is the plane position and orientation angle [x, y, θ]. The given uniform space is discretized into g×g grids, and each grid is associated with the corresponding state P 2D (idx, idy) = [x, y, θ] association, the sampling strategy is

[0097] x=idx·grid+rand(-1,1)·bias

[0098] y=idy·grid+rand(-1,1)·bias.

[0099] Where (idx, idy) is the index of the state point, grid is the grid size, g is the number of discrete grids, P 2D is a state point, which is stored in the state point set RoadMap. The total number of state points is g×g=n. The present invention modifies the LazyPRM* algorithm. In the extended search phase, only the neighbor nodes around the parent node are considered, and the neighbor state points are indexed in the state point set. The time complexity is O(n). If the entire space is completely randomly sampled and the Dijkstra method is used to search for the optimal result, the time complexity is O(n). 2 ), the improved method of the present invention greatly improves the search efficiency without affecting the optimality of the final result. The comparison results show Figure 2 .

[0100] The robot's orientation angle is initialized to 0. During the search process, it is determined by the connection from the parent node to the child node, expressed as θ curr =arctan((y curr -y pare ) / (x curr -x pare )). Where x curr ,y curr is the current node coordinate, x pare ,y pare Parent node coordinates.

[0101] like Figure 2 As shown in the figure, 400 points are randomly sampled in a 8m*8m space, the grid size is set to 0.4m, and 100 sampling search tests are performed. Figure 2 (a) is random sampling + Dijkstra's algorithm, with an average time consumption of 39.8697s and an average path length of 9.6137m. Figure 2(b) is uniform sampling + Dijkstra's algorithm with random deviation, with an average time consumption of 40.4524s and an average path length of 9.6254m. Figure 2 (c) is the uniform sampling + LazyPRM* algorithm with random deviation, with an average time consumption of 0.6953s and an average path length of 9.6216m.

[0102] The legged robot can move in all directions, so the state quantity [x, y, θ] can be considered separately. The connection between the two state points is constructed as an optimal boundary value problem. The initial state is given as s i =[s pi ,s vi ], is the state of the parent node, and the final state is s f =[s pf ,s vf ], the end position is the position of the child node, the end speed is obtained by solving, and the energy of the entire state trajectory is optimized J(T) = f T s a (t) 2 dt is minimum (i.e., the acceleration integral is minimum), and using the Pondriagin maximum principle, the explicit solution of the state estimation is obtained as follows:

[0103]

[0104]

[0105]

[0106] The numerical solution of this problem requires the trajectory time T, given the reference line velocity v ref and angular velocity ω ref Set reference time T = T ref :=max(||[Δx, Δy]||2 / v ref , Δθ / ω ref ).

[0107] The direction angle and linear velocity direction have a significant impact on the stability of the robot's motion. Therefore, the present invention proposes the following trajectory cost

[0108]

[0109] The first term is the cost of the change in direction angle, and the weight coefficient is λ yaw , the second term is the arc length of the trajectory. Using the quadratic form to calculate the cost of the yaw angle can make the change of the direction angle between the state points smoother, see Figure 3 (Diagram of different results of trajectory calculation cost in the search phase)

[0110] The angle difference from P1 to P5 is consistent, but the quadratic cost of the angle change is larger in the left figure than in the right figure. Obviously, the trajectory change in the right figure is smoother. The quadratic cost of the angle change proposed in the present invention can obtain a smoother trajectory.

[0111] Through sampling and searching, the present invention obtains a rough robot mass center motion trajectory and a series of state nodes on the trajectory. The trajectory state quantity is [x, y, θ], which is expressed by polynomial coefficients and time. The position and speed at the state node are continuous. The trajectory generation flow chart is shown in Figure 4 shown.

[0112] Nonlinear dynamics trajectory optimization

[0113] The rough trajectory is to solve an unconstrained optimal problem. There will be some dynamically unreachable trajectory segments in the entire state trajectory. On the basis of considering the dynamic characteristics of the legged robot, we further make full use of the potential of the robot's movement speed and construct a nonlinear trajectory optimization problem based on piecewise polynomials.

[0114] like Figure 5 As shown, the forward and backward movement and translational movement capabilities of the legged robot are different. The present invention proposes a hypothesis to represent the anisotropy of the omnidirectional movement of the legged robot: the maximum forward and backward movement speed of the robot is v mx , the maximum speed of translational motion is v my , and v my <v mx , the maximum linear velocity of the robot is related to the robot's motion direction, and the two constitute an elliptical constraint, such as Figure 5 , which can be expressed as:

[0115]

[0116]

[0117] Where θ is the robot's orientation angle, and R(θ) is the rotation matrix from the world inertial coordinate system I to the robot's body coordinate system B.

[0118] The present invention constructs a nonlinear optimization problem based on piecewise polynomials, and the optimization indicators include: the smoothness cost of the state trajectory, the weight is λ s , the cost of the state trajectory distance obstacle, with weight λ c and the time cost of the trajectory segment, with weight λ t , limiting robot dynamics such as maximum velocity and maximum acceleration to The robot's omnidirectional motion is limited to The continuity restriction of the previous and next states at a state point is As the constraint of the optimization problem, the solution variables of the optimization problem are the polynomial coefficient c and time T of each state trajectory.

[0119]

[0120]

[0121]

[0122]

[0123]

[0124] Where s(t) is the nth-order polynomial trajectory of the state variables x, y, and θ, N is the number of polynomial segments, j represents the jth segment, R is the positive definite weight matrix of the state smoothness cost, which measures the cost ratio between the three state variables, T is the time vector of the trajectory segment, and T j is the duration of the jth polynomial, c ji Represents the nth-order coefficient vector of the jth-segment polynomial.

[0125] The above nonlinear optimization problem is solved by using continuous state discretization and numerical optimization method of gradient descent. The state trajectory coefficient and time in the search phase are used as the initial solution of the non-trust optimization solver to ensure the quality of the solution.

[0126] The present invention can obtain a state trajectory that conforms to the dynamic characteristics of a legged robot.

[0127] Model Predictive Control Desired State Trajectory Tracking

[0128] The optimized state trajectory is used as the expected center of mass trajectory of the model predictive controller to control the motion of the robot. In the state trajectory tracking of the model predictive controller, a nonlinear model predictive control problem is constructed:

[0129]

[0130]

[0131]

[0132] g(x,u,t)=0

[0133] h(x,u,t)<0

[0134] where x(t) and u(t) are the state variables and state inputs, Φ(·) is the terminal state constraint cost function, L(·) is the quadratic cost function for trajectory tracking, is the current observation state. c(·), g(·) and h(·) are the system dynamic equations, equality constraints and inequality constraints respectively.

[0135] During high-speed movement, if both the angular velocity and the linear velocity are large, the robot's movement will become very unstable and prone to falling. Therefore, the present invention adds a speed safety constraint to the model predictive controller.

[0136]

[0137] where λ1 and λ θ Weigh the weights of translational velocity and angular velocity, is the safety threshold.

[0138] The model predictive controller solves and calculates the control instructions of the robot's joint motors based on the expected state trajectory and the robot's dynamic model to achieve autonomous movement of the robot.

[0139] (3) Working principle:

[0140] 1. The present invention integrates the planning algorithm into the real-time navigation framework, combines the front-end environment perception with the back-end motion control, and tests the planning algorithm of the present invention. The navigation framework flow chart is as follows: Figure 6 shown.

[0141] 2. Set the target posture through the computer, use the radar to perceive the surrounding environment and locate the robot, the planner generates the motion trajectory to the end point based on the map information and the current posture, and transmits the state trajectory at a certain predicted time in the future to the motion controller, and the motion controller calculates the expected instructions for the joint motor.

[0142] 3. The present invention tests the planning algorithm in different scenarios, such as Figure 7 In the scene shown, the algorithm of the present invention generates a smooth continuous trajectory. The five-pointed star is the end point of the optimization trajectory, and the triangle is the end point of the search trajectory. NO-T is the optimization trajectory, KD-T is the search trajectory, bound is the dynamic limit, horizontal obstacle avoidance trajectory, black is the obstacle, gray is the distance field gradient, and the horizontal position [x] state quantity change trajectory is as follows Figure 8 As shown, the trajectory of the horizontal position [y] state quantity change is as follows Fig. 9 As shown, the trajectory of the direction angle [θ] state quantity change is as follows Fig.10 shown.

[0143] like Fig.11 As shown, the legged robot motion trajectory planning system provided by the embodiment of the present invention includes:

[0144] The upper module is used to quickly generate a rough polynomial trajectory of the robot's center of mass motion on the global obstacle map, including the robot's plane position [x, y], direction angle θ, and trajectory time T;

[0145] The middle-level module is used to construct a nonlinear optimization problem based on the initial trajectory and robot dynamics. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics constraints, legged robot omnidirectional motion constraints and trajectory time cost. The optimization variables are the trajectory polynomial coefficients expressed in pieces and time.

[0146] The bottom-level module is used to use the optimized trajectory as the expected center-of-mass spatiotemporal state trajectory of the nonlinear model predictive controller to perform specific motion control of the robot.

[0147] An application embodiment of the present invention provides a computer device, which includes a memory and a processor. The memory stores a computer program. When the computer program is executed by the processor, the processor executes the steps of the legged robot motion trajectory planning method.

[0148] An application embodiment of the present invention provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps of a legged robot motion trajectory planning method.

[0149] An application embodiment of the present invention provides an information data processing terminal, which is used to implement a motion trajectory planning system for a legged robot.

[0150] The present invention is an invention innovation oriented towards practical applications, and its purpose is to improve the autonomy of legged robots and promote their application in real scenarios. The application field is the basic field of autonomous movement of legged robots.

[0151] 1 Nowadays, large automatic machines are mostly used for coal mining. Autonomous mining has been widely used and has formed a standardized operation process. This greatly reduces the number of construction workers in the mine and increases the safety of coal mine control. However, it is still inevitable that inspectors are required to check whether the various equipment in the autonomous operation channel is complete, operating normally, or has safety hazards. The space in the mine is relatively closed, but the environment is complex and the working conditions are harsh. The legged robot has excellent terrain adaptability and can adapt to complex working conditions. The planning algorithm proposed in the present invention can be used in such an environment and applied to the motion trajectory planning of the legged robot for mine inspection.

[0152] 2 Highways require maintenance personnel to conduct regular inspections to check road conditions. The tunnel section of the highway is closed, and the inspection channel environment is complex. It is difficult for wheeled robots to pass normally, and manual inspections are inevitable. And because tunnel safety issues are of paramount importance, the inspection frequency is relatively high. my country's highway scale is the largest in the world, and the inspection investment is huge. Legged robots are very suitable for inspections of complex road conditions in tunnel environments. The robot can self-sufficiently identify the tunnel environment, dynamically construct feasible safety intervals and operating intervals according to the task and environment, and plan the robot's inspection path. The path planning method described in the present invention is expected to be applied in this field.

[0153] General mission scenarios of three-legged robots, such as picking up, handling and stacking of humanoid robots, and transportation and accompanying tasks of four-legged robots, all require motion trajectory planning as a mobile planning module, which provides a basis for the completion of upper-level tasks and is one of the basic conditions for the realization of complex tasks.

[0154] The present invention is verified on a physical platform of a legged robot. The experimental platform is a small quadruped robot equipped with sensor equipment and a computer unit.

[0155] First, the trajectory planning results of the given scenario in the example are tested in real time to verify the rationality of the trajectory obtained by the planning method proposed in this invention. The experimental results are as follows: Fig.12 The actual motion planning of the legged robot swings around the planned expected trajectory, and the motion effect is good. The algorithm proposed in the present invention is reasonable and effective.

[0156] Use LiDAR to sense the surrounding environment and update the obstacle map. Based on this, the robot motion planning is planned and controlled. Set the reference speed, dynamic limit and local map radius to 3m for real-time online planning. Test the real-world night scene, such as Fig.13 , and real-world daytime scenes, such as Fig.14 The planner searches for a globally collision-free reachable trajectory in real time and continuously optimizes the trajectory in real time during the movement process to make the trajectory more consistent with the robot dynamics. The robot can flexibly move between obstacles. The planning framework can adapt to uneven grass and rugged terrain. The real-time updated probabilistic obstacle map can also avoid the impact of moving obstacles on the planning system to a certain extent. The robot can flexibly and quickly reach the target position and posture. The motion parameters of the robot in the two scenarios are shown in Table 1.

[0157]

[0158] It should be noted that the embodiments of the present invention can be implemented by hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated design hardware. It can be understood by a person of ordinary skill in the art that the above-mentioned devices and methods can be implemented using computer executable instructions and / or contained in a processor control code, such as a carrier medium such as a disk, CD or DVD-ROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. Such code is provided on the carrier medium. The device and its modules of the present invention can be implemented by hardware circuits such as very large-scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, etc., or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, and can also be implemented by a combination of the above-mentioned hardware circuits and software, such as firmware.

[0159] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principle of the present invention should be covered by the protection scope of the present invention.

Claims

1. A method for planning the motion trajectory of a legged robot, characterized in that: The motion trajectory planning method of the legged robot has three layers, namely: The upper layer generates global collision-free trajectories, the middle layer optimizes nonlinear dynamic trajectories, and the bottom layer predicts the control of the desired state trajectory. The upper layer module quickly generates an initial trajectory of the robot's center of mass motion on the global obstacle map, including the robot's plane position [x, y], direction angle θ, and trajectory time T. The middle-level module constructs a nonlinear optimization problem based on the initial trajectory and robot dynamics. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics restrictions, legged robot omnidirectional motion constraints and trajectory time cost. The optimization variables are the trajectory polynomial coefficients and time expressed in pieces. The bottom-level module uses the optimized trajectory as the expected center-of-mass spatiotemporal state trajectory of the nonlinear model prediction controller to perform specific motion control of the robot. The legged robot can move in all directions, so the state quantities [x, y, θ] are considered separately, and the connection between the two state points is constructed as an optimal boundary value problem. The initial state is given as s i =[s pi ,s vi ], is the state of the parent node, and the final state is s f =[s pf ,s vf ], the end position is the position of the child node, the end speed is obtained by solving, and the energy of the entire state trajectory is optimized J(T) = ∫0 T s a (t) 2 dt is the smallest, and using the Pondriagin maximum principle, the explicit solution of the state estimation is: The numerical solution of this problem requires the trajectory time T, given the reference line velocity v ref and angular velocity ω ref Set reference time T = T ref :=max(||[Δx, Δy]||2 / v ref , Δθ / ω ref ); In nonlinear dynamic trajectory optimization, the optimization indicators include: the smoothness cost of the state trajectory, with a weight of λ s , the cost weight of the state trajectory distance obstacle is λ c and the time cost weight of the trajectory segment is λ t , limiting the robot dynamics to maximum speed and maximum acceleration Limitations of omnidirectional motion of robots Continuity of the previous and next states at a state point As the constraints of the optimization problem, the solution variables of the optimization problem are the polynomial coefficient c and time T of each state trajectory: Where s(t) is the nth-order polynomial trajectory of the state variables x, y, and θ, N is the number of polynomial segments, j represents the jth segment, R is the positive definite weight matrix of the state smoothness cost, which measures the cost ratio between the three state variables, T is the time vector of the trajectory segment, and c ji Represents the nth-order coefficient vector of the jth-segment polynomial.

2. The method for motion trajectory planning of a legged robot according to claim 1, characterized in that: In the upper-level global collision-free trajectory generation, the planned robot state is the plane position and direction angle [x, y, θ]. The given uniform space is discretized into g×g grids, and each grid is associated with the corresponding state P 2D (idx, idy) = [x, y, θ] association, the sampling strategy is: x=idx·grid+rand(-1,1)·bias y=idy·grid+rand(-1,1)·bias Where (idx, idy) is the index of the state point, grid is the grid size, g is the number of discrete grids, P 2D is a state point, stored in the state point set RoadMap, and the total number of state points is g×g=n.

3. The method for motion trajectory planning of a legged robot according to claim 1, characterized in that: In the model predictive control desired state trajectory tracking, a speed safety constraint is added to the model predictive controller. where λ1 and λ θ Weigh the weights of translational velocity and angular velocity, is the safety threshold; The model predictive controller calculates the control instructions of the robot's joint motors according to the desired state trajectory to achieve autonomous movement of the robot.

4. A legged robot motion trajectory planning system using the legged robot motion trajectory planning method according to any one of claims 1 to 3, characterized in that: include: The upper module is used to quickly generate an initial trajectory of the robot's center of mass motion on the global obstacle map, including the robot's plane position [x, y], direction angle θ, and trajectory time T; The middle-level module is used to construct a nonlinear optimization problem based on the initial trajectory and robot dynamics. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics constraints, legged robot omnidirectional motion constraints and trajectory time cost. The optimization variables are the trajectory polynomial coefficients expressed in pieces and time. The bottom-level module is used to use the optimized trajectory as the expected center-of-mass spatiotemporal state trajectory of the nonlinear model predictive controller to perform specific motion control of the robot.

5. A computer device, comprising a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the legged robot motion trajectory planning method as described in any one of claims 1 to 3.

6. A computer-readable storage medium storing a computer program, which, when executed by a processor, enables the processor to execute the steps of the legged robot motion trajectory planning method as described in any one of claims 1 to 3.

7. A motion trajectory planning system for a legged robot that implements the motion trajectory planning method for a legged robot as claimed in any one of claims 1 to 3, characterized in that: The system includes: a global collision-free trajectory generation module, configured to generate a rough polynomial trajectory on a global environment map, the trajectory indicating a preliminary running path of the robot; A nonlinear dynamic trajectory optimization module, which solves the nonlinear optimization problem through an optimization algorithm based on the initial path and the robot's dynamic model, and generates an optimized trajectory that is smooth, avoids obstacles, and complies with omnidirectional motion constraints; A model predictive control desired state trajectory tracking module generates the robot's motion control instructions according to the optimized trajectory to achieve autonomous motion of the robot.

8. The motion trajectory planning system of a legged robot as claimed in claim 7, characterized in that: The system includes: The nonlinear dynamic trajectory optimization module takes into account the obstacle avoidance cost, state trajectory smoothness, robot dynamics restrictions, omnidirectional motion constraints and trajectory time cost optimization indicators, balances the requirements of obstacle avoidance, smooth motion and rapid response, and improves the robot's motion performance; Furthermore, the model predictive control desired state trajectory tracking module adds a speed safety constraint.

Citation Information

Patent Citations

  • Walking motion planning method for biped walking robot

    CN108089578A

  • Motion planning method of quadruped robot for narrow environment

    CN114022824A

  • Autonomous exploration and mapping system based on quadruped robot

    CN114442621A

  • Motion track generation method combining long time domain and reactivity of foot type robot

    CN116185015A

  • Planning in mobile robots

    US20230089978A1