CN117572773A - A method, system, equipment and terminal for motion trajectory planning of a footed robot - Google Patents

Abstract

The invention belongs to the field of robot technology and discloses a method, system, equipment and terminal for planning a motion trajectory of a footed robot, including: upper-level global collision-free trajectory generation, middle-level nonlinear dynamic trajectory optimization, and bottom-level model predictive control desired state trajectory tracking. ; The upper module quickly generates a rough polynomial trajectory of the robot's center of mass motion on the global obstacle map, including the plane position [x, y], direction angle θ and trajectory time T of the robot's motion; the middle module constructs a Nonlinear optimization problem, the optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics limitations, footed robot omnidirectional motion constraints and trajectory time cost, the optimization variables are the trajectory polynomial coefficients expressed in pieces and time; the underlying module The optimized trajectory is used as the desired center-of-mass spatio-temporal state trajectory of the nonlinear model prediction controller to perform specific motion control of the robot.

Description

A method, system, equipment and terminal for motion trajectory planning of a footed robot

Technical field

The invention belongs to the field of robot technology, and in particular relates to a method, system, equipment and terminal for planning a motion trajectory of a footed robot.

Background technique

Footed robots are bionic robots that imitate the movements of footed mammals in nature, such as humans and cats. Footed robots have omnidirectional movement capabilities and the ability to pass through complex terrain. Nowadays, the body motion control of footed robots is at a high level and has certain actual animal motion capabilities, such as translational motion, steering motion, 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 movement modes. The current various motion controllers are still some distance away from realizing the agile movement of animals.

Model predictive controllers are widely used in motion control of footed robots with collision-free constraints to realize robot navigation planning. However, the prediction period of the model predictive controller greatly affects the control effect. A long prediction period causes the solution to take a long time, and a short prediction period can easily cause the optimization solution to fall into a local optimum. Learning-based methods can achieve real-time navigation of robots, but require pre-training or the use of data sets. These datasets are primarily acquired in simulated environments and will show differences when used in real-world environments, and real-world datasets are very expensive to produce. Trajectories generated using traditional path planning methods based on sampling or search are difficult to conform to the complex dynamic model of footed robots, so large errors are prone to occur during trajectory tracking or the tracking speed is slow.

Through the above analysis, the problems and defects existing in the existing technology are:

(1) The actual movement of the robot is slow: the potential of the movement speed of the footed robot is not used, and the movement is slow to ensure the trajectory tracking effect;

(2) The robot's movement trajectory is single: most of it is a geometric trajectory in space, which only reflects the obstacle avoidance ability and does not reflect the flexibility of the footed robot's movement;

(3) The real-time performance of the planning algorithm is difficult to guarantee: the dynamic model of the legged robot is very complex, the dimensions of the planning problem are large, the planning solution takes a long time, and the real-time performance is difficult to guarantee.

Contents of the invention

In view of the problems existing in the existing technology, the present invention provides a method, system, equipment and terminal for planning the motion trajectory of a footed robot.

The present invention is implemented as follows: a method for planning the movement trajectory of a footed robot. The method for planning the movement trajectory of a footed robot has three layers, namely: an upper-layer global collision-free trajectory generation module, and a middle-layer nonlinear dynamics trajectory optimization module. The underlying model predicts the controller state trajectory tracking module. The upper module quickly generates a rough polynomial trajectory of the robot's center of mass motion on the global 2D obstacle map, including the plane position [x, y], direction angle θ and trajectory duration T of the robot's motion; the middle module uses the initial trajectory and robot dynamics obtained from the search Learn to construct a nonlinear optimization problem. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics limitations (speed, acceleration), footed robot omnidirectional motion constraints and trajectory time cost. The optimization variables are expressed in pieces. The coefficients of the polynomial trajectory and the duration of each trajectory; the underlying module uses the optimized trajectory as a nonlinear model to predict the spatio-temporal state trajectory of the desired center of mass of the controller, and uses the controller to track the motion trajectory of the robot's center of mass to control the motion of the robot body. .

Furthermore, 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 The state P _2D (idx, idy) = [x, y, θ] is associated, and the sampling strategy is:

x＝idx·grid+rand(-1,1)·bias

y＝idy·grid+rand(-1,1)·bias.

Among them (idx, idy) is the index of the state point, grid is the grid size, g is the number of discrete grids, P _2D is the state point, which is stored in the state point set RoadMap. The total number of state points is g×g=n indivual.

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, the terminal state is s _f = [s _pf , s _vf ], the terminal position is the position of the child node, the terminal speed is obtained by solving, the energy of optimizing the entire state trajectory J(T) = ∫ ^T s _a (t) ² dt is the minimum (that is, the acceleration integral is the minimum). Using Pontryagin's maximum principle, the displayed solution of the state estimation is:

The numerical solution to this problem requires trajectory time T. The reference time T=T ref is set by giving the reference linear velocity v _ref and angular velocity ω _{ref :} =max(||[Δx, Δy]|| ₂ /v _ref ,Δθ /ω _ref ).

Furthermore, in the nonlinear dynamics trajectory optimization, the optimization indicators include: the smoothness cost of the state trajectory, the weight is λ _s , the cost of the state trajectory distance from the obstacle, the weight is λ _c and the time cost of the trajectory segment, the weight is λ _t , Limit robot dynamics such as maximum speed and maximum acceleration to The limit of the robot’s omnidirectional motion is The continuity limit of the state before and after the state point is/> as constraints in optimization problems. The solution variables of the optimization problem are the polynomial coefficient c and time T of each state trajectory:

where s(t) is the n-order polynomial trajectory of the state variables x, y, θ, N is the number of polynomial segments, j represents the jth segment, and R is the positive definite weight matrix of the state smoothness cost, measuring the cost between the three state variables. Specific gravity, T is the time vector of the trajectory segment, T _j is the duration of the j-th segment polynomial, and c _ji represents the n-th order coefficient vector of the j-th segment polynomial.

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

g(x,u,t)=0

h(x,u,t)<0

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

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

where λ ₁ and λ _θ measure the weight of translational velocity and angular velocity, is the safety threshold.

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

Another object of the present invention is to provide a footed robot motion trajectory planning system that applies the footed robot motion trajectory planning method, including:

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 plane position [x, y], direction angle θ and trajectory time T of the robot's motion;

The middle-level module is used to construct nonlinear optimization problems based on the initial trajectory and robot dynamics. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics limitations, footed robot omnidirectional motion constraints and trajectory time cost. Optimization The variables are the trajectory polynomial coefficients expressed piecewise and time;

The underlying module is used to use the optimized trajectory as the desired center-of-mass spatio-temporal state trajectory of the nonlinear model prediction controller to perform specific motion control of the robot.

Another object of the present invention is to provide a computer device. The computer device includes a memory and a processor. The memory stores a computer program. When the computer program is executed by the processor, it causes the processor to execute the method for planning the motion trajectory of a footed robot. step.

Another object of the present invention is to provide a computer-readable storage medium that stores a computer program. When the computer program is executed by a processor, it causes the processor to execute the steps of the footed robot motion trajectory planning method.

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

Combined 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:

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

1) Due to the limitations of the onboard computing unit, the real-time motion trajectory and tracking of mobile robots takes a long time to plan online and it is difficult to improve the efficiency. The present invention uses a hierarchical planning method to balance planning problem dimensions and planning lengths at different planning layers, efficiently allocate computing resources, and achieve stable real-time planning.

2) Use the hierarchical planning method and combine the trajectory search to find the global trajectory to avoid falling into the local optimal solution; use trajectory optimization to improve the quality of the trajectory to make it more consistent with the actual motion characteristics of the footed robot; the model prediction controller tracks the predetermined cycle trajectory to ensure the motion trajectory tracking effect.

3) The current planning methods of legged robots mostly consider motion stability, and the robot moves slowly. The present invention considers the anisotropy of the omnidirectional motion of the footed robot, and the planned spatio-temporal trajectory of the robot movement (including the time sequence of position, speed, and acceleration) is not only the spatial geometric trajectory, but also includes multiple-dimensional time-related trajectories. ), and uses multiple indicators to optimize the trajectory to take advantage of the rapid movement and flexibility of the footed robot.

Second, considering the technical solution as a whole or from a product perspective, the technical effects and advantages possessed by the technical solution to be protected by the present invention are specifically described as follows:

1) The present invention divides planning tasks into three layers. The complexity of problems from the top to the bottom increases and the planning distance becomes shorter. Compared with a single planning layer, computing resources are efficiently allocated and online planning is achieved.

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 is almost unchanged.

3) The trajectory cost calculation strategy used in the optimal trajectory search process takes into account the motion characteristics of the legged robot and additionally considers the planning of the robot's direction angle based on the plane 2D position. It calculates the trajectory space geometric distance and also calculates the steering cost, so that The initial trajectory is more in line with the robot dynamics and is more targeted.

4) The present invention constructs a local trajectory optimization problem based on the polynomial trajectory in the search stage, taking into account obstacle avoidance, dynamic constraints, and time optimization, and can further improve the overall trajectory quality. In particular, the anisotropic constraints on the omnidirectional motion of the footed robot are introduced, that is, the constraints on the omnidirectional motion speed and motion direction.

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

6) The optimized trajectory is directly used as the desired state trajectory of the robot body nonlinear model prediction controller. Compared with the command tracking method using acceleration, speed or position, more complex motion behaviors can be achieved, and the tracking effect is greatly improved.

Third, as auxiliary evidence of inventive step for the claims of the present invention, it is also reflected in the following important aspects:

(1) The expected income and commercial value after the transformation of the technical solution of the present invention are:

The invention is an invention of a motion planning method for a footed robot. It is the basis for the autonomous movement of a footed robot and has a wide range of application scenarios. At present, the motion trajectory planning algorithms of mobile robots are mostly drones and wheeled robots, and their configuration, dynamic model and movement mode are significantly different from those of footed robots. Related methods are difficult to work well on legged robots. This method is suitable for legged robots to perform inspection tasks or provide an autonomous navigation basis for complex tasks. There is a broad space for the application of footed robots, and the market value is huge. The motion trajectory planning method of footed robots is a very critical technology, and it is also related to whether the footed robots can play a role in various potential application scenarios. key. Inventions and innovations related to legged robot planning methods have extremely high application value in the commercial field. In addition, R&D costs are reflected in the development and testing stage. The cost of deploying algorithms on robots is very low, and the profit margins are huge, whether it is technology licensing or algorithm The form of core stand-alone packaging has very considerable profit margins.

(2) The technical solution of the present invention fills the technical gaps in the industry at home and abroad:

At present, the motion planning of footed robots is mostly focused on the stable movement of the robot body, or the foothold planning of the footed robot on rugged terrain. The control objectives are to expect the footed robot to be able to pass through various terrains stably. Among these methods, stability is the most critical consideration, so most robots move slowly, which does not meet the needs of most scenarios for practical applications. The robot motion planning method proposed by the present invention pays more attention to the movement potential of the footed robot body, so that it can move quickly and dexterously in space, thereby realizing the autonomous movement of the footed robot in various general scenarios.

Fourth, the footed 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:

1) Three-layer 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, achieving accurate and Efficient motion planning.

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

3) Diversification of optimization indicators: In nonlinear dynamics 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 to balance obstacle avoidance. , smooth motion, fast response and other various needs, which improve the motion performance of the robot.

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 movement is achieved, and the stability and accuracy of the robot's movement are improved.

5) Safety enhancement: The underlying model predictive controller adds speed safety constraints to ensure that the robot can meet the trajectory optimization while also ensuring the safety of movement.

Through the motion trajectory planning method of the legged robot provided by the present invention, efficient, safe and precise movement of the robot in a complex environment can be realized, which greatly improves the application value and practicability of the robot.

Description of the drawings

In order to explain the technical solutions of the embodiments of the present invention more clearly, the drawings required to be used in the embodiments of the present invention will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without exerting creative efforts.

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

Figure 2 is a schematic diagram of the sampling search test provided by the 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;

Figure 3 is a schematic diagram of the results of different trajectory calculation costs in the search stage provided by the embodiment of the present invention;

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

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

Figure 6 is a flow chart of the navigation framework provided by the embodiment of the present invention;

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

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

Figure 9 is a change trajectory diagram of the horizontal position y state quantity provided by the embodiment of the present invention;

Figure 10 is a change trajectory diagram of the direction angle θ state quantity provided by the embodiment of the present invention;

Figure 11 is a structural diagram of a footed robot motion trajectory planning system provided by an embodiment of the present invention;

Figure 12 is the tracking effect of the robot motion trajectory planning test provided by the embodiment of the present invention;

Figure 13 is a real-world test at night of the robot motion planning system provided by the embodiment of the present invention;

Figure 14 is a daytime real-world test of the robot motion planning system provided by the embodiment of the present invention;

Detailed ways

In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with examples. It should be understood that the specific embodiments described here are only used to explain the present invention and are not intended to limit the present invention.

In order to solve the problems existing in the prior art, the present invention provides a method, system, equipment and terminal for planning the motion trajectory of a footed robot. The present invention will be described in detail below with reference to the accompanying drawings.

Example 1: Service robot in indoor environment

In an indoor environment, 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 trajectory planning method described above can be used.

1. The upper module generates a rough polynomial trajectory on the global indoor environment map (such as a 2D map generated by lidar), which indicates the robot's preliminary operating path.

2. The middle-level module solves the nonlinear optimization problem through an optimization algorithm (such as gradient descent method or genetic algorithm) based on the initial path and the robot's dynamic model, and generates an optimized trajectory that is smooth, obstacle-avoidable, and conforms to omnidirectional motion constraints.

3. The underlying 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 realize the robot's autonomous movement.

Example 2: Search and rescue robot in outdoor environment

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

1. The upper module generates a rough polynomial trajectory on the global outdoor environment map (such as a 3D map generated by a drone), which indicates the robot's preliminary operating path.

2. The middle-level module solves the nonlinear optimization problem through optimization algorithms (such as particle swarm optimization algorithm or simulated annealing algorithm) based on the initial path and the robot's dynamic model, and generates an optimized trajectory that is smooth, obstacle-avoidable, and conforms to omnidirectional motion constraints.

3. The underlying 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 realize the robot's autonomous movement.

The hierarchical planning method of the present invention has three layers, namely: upper-layer global collision-free trajectory generation, middle-layer nonlinear dynamic trajectory optimization, and bottom-layer model predictive control desired state trajectory tracking. The upper module quickly generates a rough polynomial trajectory of the robot's center of mass motion on the global obstacle map, including 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 from the search and the robot dynamics model. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics limitations, footed robot omnidirectional motion constraints and trajectory time cost. Optimization The variables are the coefficients of the trajectory polynomial expressed piecewise and time. The underlying module uses the optimized trajectory as the desired center-of-mass spatio-temporal state trajectory of the nonlinear model prediction controller to perform specific motion control of the robot. The planned trajectory distance of the hierarchical planner is shown in Figure 1.

Global collision-free trajectory generation: The legged robot has six degrees of freedom, but the main concerns during movement are only the horizontal position and direction 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 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.

Among them (idx, idy) is the index of the state point, grid is the grid size, g is the number of discrete grids, P _2D is the state point, which is stored in the state point set RoadMap. The total number of state points is g×g=n indivual. This 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 centrally in the state points. The time complexity is O(n). If the entire space is completely randomly sampled, If the Dijkstra method is used to search to obtain the optimal result, the time complexity is O(n ² ). The improved method of the present invention greatly improves the search efficiency without affecting the optimality of the final result. The comparison results are shown in Figure 2.

The direction angle of the robot 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 )). Among them, x _{curr and} y _curr are the coordinates of the current node, and are the coordinates of the parent node of x _pare and y _pare .

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

The legged robot can move in all directions, so the state quantities [x, y, θ] can be considered separately. The connection of 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, the terminal state is s _f = [s _pf , s _vf ], the terminal position is the position of the child node, the terminal speed is obtained by solving, and the energy of the entire state trajectory is optimized J(T) = f ^T s _a (t) ² dt is the minimum (that is, the acceleration integral is the minimum). Using Pontryagin's maximum principle, the displayed solution of the state estimation is:

The numerical solution to this problem requires trajectory time T. The reference time T=T ref is set by giving the reference linear velocity v _ref and angular velocity ω _{ref :} =max(||[Δx, Δy]||2/v _ref , Δθ /ω _ref ).

Direction angle and linear velocity direction have a significant impact on the stability of robot motion. Therefore, the present invention proposes the following trajectory cost

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

The angle difference from P ₁ to P ₅ is the same, but the quadratic cost of angle change in the left picture is larger than that in the right picture. It is obvious that the trajectory change in the right picture is smoother. The quadratic cost of angle change proposed by the present invention can obtain a smoother trajectory. .

Through sampling and searching, the present invention obtains a rough robot center of mass 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 velocity of the state node are continuous. The trajectory generation flow chart is shown in Figure 4.

Nonlinear dynamics trajectory optimization

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 to construct Nonlinear trajectory optimization problem based on piecewise polynomials.

As shown in Figure 5, the forward and backward movement and translational movement capabilities of the footed robot are different. The present invention proposes a hypothesis to represent the anisotropy of the omnidirectional movement of the footed robot: the maximum speed of the forward and backward movement of the robot is v _mx , and the translational movement The maximum speed of is v _my , and v _my < v _mx . The maximum linear speed of the robot movement is related to the direction of the robot movement. The two constitute an elliptical constraint, as shown in Figure 5, which can be expressed as:

where θ is the orientation angle of the robot, and R(θ) is the rotation matrix from the world inertial coordinate system I to the robot body coordinate system B.

The present invention constructs a nonlinear optimization problem based on piecewise polynomials. The optimization indicators include: the smoothness cost of the state trajectory, the weight is λ _s , the cost of the state trajectory distance from the obstacle, the weight is λ _c and the time cost of the trajectory segment, the weight is λ _t , limiting robot dynamics such as maximum speed and maximum acceleration to The limit of the robot’s omnidirectional motion is/> The continuity limit of the state before and after the state point is/> as constraints in optimization problems. The solution variables of the optimization problem are the polynomial coefficient c and time T of each state trajectory.

where s(t) is the n-order polynomial trajectory of the state variables x, y, θ, N is the number of polynomial segments, j represents the jth segment, and R is the positive definite weight matrix of the state smoothness cost, measuring the cost between the three state variables. Specific gravity, T is the time vector of the trajectory segment, T _j is the duration of the j-th segment polynomial, and c _ji represents the n-th order coefficient vector of the j-th segment polynomial.

The numerical optimization method of continuous state discretization and gradient descent is used to solve the above nonlinear optimization problem. The state trajectory coefficients and time in the search phase are used as the initial solution of the non-believing optimization solver to ensure the solution quality.

The present invention can obtain a state trajectory that conforms to the dynamic characteristics of a footed robot.

Model predictive control desired state trajectory tracking

The optimized state trajectory is used as the desired center-of-mass trajectory of the model prediction controller to perform motion control of the robot body. In model predictive controller state trajectory tracking, nonlinear model predictive control problems are constructed:

g(x,u,t)=0

h(x,u,t)<0

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

During high-speed movement, if the angular velocity and linear velocity are both very large, the robot's movement will become very unstable and it will easily fall. Therefore, this invention adds speed safety constraints to the model predictive controller,

where λ ₁ and λ _θ measure the weight of translational velocity and angular velocity, is the safety threshold.

The model predictive controller solves and calculates the control instructions for the robot's joint motors based on the desired state trajectory and the robot's dynamic model to realize the robot's autonomous movement.

(3) Working principle part:

1. The present invention integrates the planning algorithm into the real-time navigation framework, combines the front-end environment sensing and back-end motion control, and tests the planning algorithm of the present invention. The navigation framework flow chart is shown in Figure 6.

2. Set the target pose through the computer, use radar to sense the surrounding environment and position the robot. The planner generates a motion trajectory to the end point based on the map information and the current pose, and transmits the state trajectory for a certain predicted time in the future to the motion control. The motion controller calculates the desired command of the joint motor.

3. The present invention tests the planning algorithm in different scenarios. In the scenario shown in Figure 7, the algorithm of the present invention generates a smooth continuous trajectory. The five-pointed star is the end point of the optimized trajectory and the triangle is the end point of the search trajectory. NO-T is the optimized 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 shown in Figure 8 As shown, the change trajectory of the horizontal position [y] state quantity is shown in Figure 9, and the change trajectory of the direction angle [θ] state quantity is shown in Figure 10.

As shown in Figure 11, the motion trajectory planning system for a footed robot provided by an embodiment of the present invention includes:

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 plane position [x, y], direction angle θ and trajectory time T of the robot's motion;

The middle-level module is used to construct nonlinear optimization problems based on the initial trajectory and robot dynamics. The optimization indicators include: obstacle avoidance cost, state trajectory smoothness, robot dynamics limitations, footed robot omnidirectional motion constraints and trajectory time cost. Optimization The variables are the trajectory polynomial coefficients expressed piecewise and time;

The underlying module is used to use the optimized trajectory as the desired center-of-mass spatio-temporal state trajectory of the nonlinear model prediction controller to perform specific motion control of the robot.

An application embodiment of the present invention provides a computer device. The computer device includes a memory and a processor. The memory stores a computer program. When the computer program is executed by the processor, it causes the processor to execute the steps of the footed robot motion trajectory planning method.

Application embodiments of the present invention provide a computer-readable storage medium that stores a computer program. When the computer program is executed by a processor, it causes the processor to execute the steps of the footed robot motion trajectory planning method.

Application embodiments of the present invention provide an information data processing terminal, which is used to implement a footed robot motion trajectory planning system.

The present invention is a practical application-oriented invention and innovation, aiming to improve the autonomy of the footed robot and promote its application in real scenarios. The application field is the basic field of autonomous movement of footed robots.

1 Nowadays, large-scale automatic machinery is mostly used in coal mining. Independent mining has been widely used and standardized operating procedures have been formed. This greatly reduces the number of construction workers in the mine and increases the safety of coal mine control, but it is still inevitable that inspection personnel will be required to check whether the various equipment in the autonomous working channels are complete, operating normally, or have 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 by the present invention can be used in such an environment and applied to the motion trajectory planning of mine inspection footed robots.

2Highways require regular inspections by maintenance personnel to check road conditions. The tunnel section of the expressway has a closed environment and the inspection channel environment is complex. It is difficult for wheeled robots to pass normally, and manual inspection is inevitably required. And because tunnel safety is a top priority, the frequency of inspections is relatively high. my country's highways are the largest in the world and investment in inspections is huge. Legged robots are very suitable for inspection work in complex road conditions in tunnel environments. The robots can self-identify the tunnel environment, dynamically construct feasible safety zones and operating zones based on tasks and environments, and plan robot inspection paths. The path planning method of the present invention is expected to be applied in this field.

General task scenarios for 3-legged robots, such as picking up, transporting and palletizing goods for humanoid robots, and transporting and accompanying quadruped robots. All require motion trajectory planning as a mobile planning module to provide a basis for the completion of upper-level tasks and is one of the basic conditions for the realization of complex tasks.

The invention was 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.

First, the trajectory planning results of the given scene in the example are tested in real time to verify the rationality of the trajectory obtained by the planning method proposed by the present invention. The experimental results are shown in Figure 12. The actual motion planning of the legged robot swings near the planned desired trajectory, and the motion effect is good. The algorithm proposed in the present invention is reasonable and effective.

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 limits and local map radius to 3m for real-time online planning. Test the real-world night scene, as shown in Figure 13, and the real-world daytime scene, as shown in Figure 14. The planner quickly searches the global collision-free reachable trajectory in real time, and continuously optimizes the trajectory in real time during the movement to make the trajectory more satisfactory for robot dynamics. The robot can flexibly travel 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 reach the target location flexibly and quickly. and posture. The motion parameters of the robot in the two scenes are shown in Table 1.

It should be noted that embodiments of the present invention may 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 memory and executed by an appropriate instruction execution system, such as a microprocessor or specially designed hardware. Those of ordinary skill in the art will understand that the above-described apparatus and methods may be implemented using computer-executable instructions and/or included in processor control code, for example on a carrier medium such as a disk, CD or DVD-ROM, such as a read-only memory. Such code is provided on a programmable memory (firmware) or on a data carrier such as an optical or electronic signal carrier. The device and its modules of the present invention may 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., It can also be implemented by software executed by various types of processors, or by a combination of the above-mentioned hardware circuits and software, such as firmware.

The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any person familiar with the technical field shall, within the technical scope disclosed in the present invention, be within the spirit and principles of the present invention. Any modifications, equivalent substitutions and improvements made within the above shall be included in the protection scope of the present invention.

Claims (10)

1. The motion track planning method of the foot robot is characterized by comprising three layers, namely:

generating an upper global collision-free track, optimizing a middle nonlinear dynamic track, and predicting and controlling a desired state track by a bottom model; the upper module rapidly generates a rough polynomial track of robot mass heart movement on a global obstacle map, wherein the rough polynomial track comprises plane positions [ x, y ], direction angles theta and track time T of the robot movement; the middle layer module constructs a nonlinear optimization problem according to the initial track and the robot dynamics, and the optimization indexes comprise: obstacle avoidance cost, state track smoothness, robot dynamics limit, foot robot omnidirectional motion constraint and track time cost, wherein the optimization variables are track polynomial coefficients and time expressed in a segmented manner; and the bottom layer module takes the optimized track as a desired centroid space-time state track of the nonlinear model predictive controller to carry out specific motion control of the robot.

2. The method for planning motion trajectory of foot robot according to claim 1, wherein in the generation of upper global collision-free trajectory, the planned robot state is plane position and direction angle [ x, y, θ]Dispersing a given uniform space into g×g grids, each grid being associated with a corresponding state P _2D (idx，idy)＝[x，y，θ]In 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 planning motion trajectory of foot robot according to claim 1, wherein the foot robot can move omnidirectionally so that the state quantity [ x, y, θ ] is calculated]Separately considering the problem of constructing the connection of two state points as an optimal boundary value, the initial state is given as s _i ＝[s _pi ，s _vi ]Is the state of the father node, and the termination state is s _f ＝[s _pf ，s _vf ]The termination position is the position of the child node, the termination speed is obtained by solving, and the energy J (T) = [ pi ] of the whole state track is optimized ₀ ^T s _a (t) ² dt is minimum (i.e., acceleration integral is minimum), and using the poincare gold maximum principle, the display solution for the state estimation is:

wherein a numerical solution of the problem requires a trajectory time T by giving a reference line speed v _ref And angular velocity omega _ref Setting a reference time t=t _ref ：＝max(||[Δx，Δy]|| ₂ /v _ref ，Δθ/ω _ref )。

4. The motion trajectory planning method of a foot robot according to claim 1, which is characterized in thatThe method is characterized in that in nonlinear dynamics track optimization, optimization indexes comprise: the smoothness cost of the state track is lambda _s The cost weight of the state track from the obstacle is lambda _c And the time cost weight of the track segment is lambda _t Limiting robot dynamics such as maximum speed, maximum accelerationRestriction of robot omnidirectional movement ∈>Continuity of front and back states at a state pointAs a constraint on the optimization problem. The solving variables of the optimization problem are polynomial coefficients c and time T of each segment of state track:

n-th order polynomial rails where s (t) is the state variable x, y, θTrace, N is the polynomial segment number, j represents the j-th segment, R is a positive weighting matrix of state smoothness costs, measure the cost specific gravity between three state variables, T is the time vector of the trace segment, c _ji An nth order coefficient vector representing a j-th stage polynomial.

5. The method for planning a motion trajectory of a legged robot according to claim 1, wherein a speed safety constraint is added to the model predictive controller in tracking a trajectory of a desired state of the model predictive control,

wherein lambda is ₁ And lambda (lambda) _θ The weights of the translation speed and the angular speed are measured,is a safety threshold.

And the model prediction controller calculates a control instruction of a robot joint motor according to the expected state track to realize autonomous movement of the robot.

6. A foot robot motion trajectory planning system applying the foot robot motion trajectory planning method according to any one of claims 1 to 5, comprising:

the upper layer module is used for quickly generating a rough polynomial track of robot mass heart movement on the global obstacle map, and comprises plane positions [ x, y ], direction angles theta and track time T of the robot movement;

the middle layer module is used for constructing a nonlinear optimization problem according to the initial track and the robot dynamics, and the optimization indexes comprise: obstacle avoidance cost, state track smoothness, robot dynamics limit, foot robot omnidirectional motion constraint and track time cost, wherein the optimization variables are track polynomial coefficients and time expressed in a segmented manner;

and the bottom layer module is used for taking the optimized track as a desired centroid space-time state track of the nonlinear model predictive controller and carrying out specific motion control on the robot.

7. A computer device comprising a memory and a processor, the memory storing a computer program which, when executed by the processor, causes the processor to perform the steps of the foot robot motion trajectory planning method of any one of claims 1 to 5.

8. A computer readable storage medium storing a computer program which, when executed by a processor, causes the processor to perform the steps of the foot robot motion trajectory planning method of any one of claims 1 to 5.

9. A motion trajectory planning system for a foot robot, the system comprising:

a global collision-free trajectory generation module configured to generate a rough polynomial trajectory on a global environment map, the trajectory indicating a preliminary path of travel of the robot;

the nonlinear dynamic track optimization module is used for solving nonlinear optimization problems through an optimization algorithm according to the initial path and a dynamic model of the robot to generate an optimized track which is smooth, obstacle-avoiding and accords with the omnidirectional motion constraint;

and a model predictive control expected state track tracking module generates a motion control instruction of the robot according to the optimized track to realize autonomous motion of the robot.

10. The motion trajectory planning system of a foot robot of claim 9, wherein the system comprises:

the nonlinear dynamic track optimization module considers the obstacle avoidance cost, the state track smoothness, the robot dynamic limit, the omnidirectional movement constraint and the track time cost optimization index, balances the obstacle avoidance, smooth movement and quick response requirements, and improves the movement performance of the robot;

and, the model predictive control desired state trajectory tracking module adds a speed safety constraint.