A local path planning method for mobile robots based on centrifugal force and jerk constraints

Abstract

The present invention discloses a local path planning method for a mobile robot based on centrifugal force and jerk constraints. The method comprises: obtaining the current laser radar scanning frame, the global cost map, the current position of the mobile robot, the current mass of the mobile robot and the global path, constructing a local cost map according to the current laser radar scanning frame and the global cost map, initializing a trajectory sequence according to the local cost map and the global path and constructing an original TEB hypergraph; then, introducing centrifugal force and jerk constraints into the original TEB hypergraph; finally, optimizing the improved TEB hypergraph, obtaining the best trajectory sequence and inputting it into a trajectory tracking controller for control signal calculation. The method can plan a trajectory with low curvature, small speed fluctuation and less motor impact, effectively improving the operating efficiency and safety of the mobile robot.

Description

Technical Field

[0001] The invention belongs to the technical field of local path planning of mobile robots, and in particular relates to a local path planning method of mobile robots based on centrifugal force and jerk constraints. Background Art

[0002] At present, mobile robots have been widely used in logistics, medical and other fields. The key technologies of mobile robots include environmental perception, global path planning, local path planning and motion control. Among them, the common algorithms for local path planning are artificial potential field method, dynamic window method and time elastic band method (TEB). The main idea of ​​TEB is to regard a continuous trajectory as an "elastic band" composed of several time points and positions, consider the kinematic constraints, obstacle constraints, maximum speed constraints and maximum acceleration constraints of the mobile robot, construct a hypergraph with trajectory points and time intervals as the variables to be optimized, use optimization tools to calculate the optimal trajectory sequence, and calculate the control signal of the mobile robot according to the optimal trajectory sequence.

[0003] Although the TEB algorithm performs well in trajectory planning in dynamic environments, it does not fully consider planning under different load conditions. In practical applications, changes in load can affect the dynamic characteristics of the mobile robot, especially when turning. When the load is large, the mobile robot needs to slow down the turning speed to maintain the stability of the movement and avoid excessive tilting or loss of control. Since the TEB algorithm mainly focuses on smooth trajectories and obstacle avoidance constraints, it does not explicitly consider the impact of the load on the turning behavior of the mobile robot. Therefore, when the load changes greatly, the algorithm may not be able to automatically adjust the turning speed of the mobile robot, making the planned trajectory less than ideal.

[0004] The introduction of jerk acceleration helps to reduce the speed mutation and motor impact of the mobile robot during the movement process, thereby improving the smoothness of the trajectory and the operation efficiency of the system. In the relevant technical field, the concept of jerk acceleration has been applied in some patents and literature. For example, in patent CN118170030B (transformation coefficient adaptive unwinding rotary axis ultra-flexible control method of operation control system), jerk acceleration is used to describe the high-order dynamic characteristics in high-precision motion control systems. In addition, in patent CN110775067B (driving evaluation system, driving evaluation method, and computer storage medium), jerk acceleration is also used to evaluate the dynamic behavior of the vehicle during driving.

[0005] In the field of mobile robot motion control, linear velocity and angular velocity are usually used to describe the motion state of the mobile robot. In this application, the acceleration is divided into two parts: the linear velocity and the angular velocity. The linear velocity represents the rate of change of the linear velocity, the linear velocity represents the rate of change of the linear velocity, and the linear velocity represents the rate of change of the linear velocity. The angular velocity represents the rate of change of the angular velocity, the angular velocity represents the rate of change of the angular velocity, and the angular velocity represents the rate of change of the angular velocity. Based on the TEB algorithm, this application introduces the linear velocity constraint and the angular velocity constraint to improve the smoothness of the local path trajectory, and introduces the centrifugal force constraint to reduce the fluctuation of the driving curve of the mobile robot under different load conditions.

[0006] In summary, providing a local path planning method for mobile robots based on centrifugal force and jerk constraints has strong scientific significance and engineering practice value. Under different load conditions, as well as in the driving curves and turning processes of mobile robots, compared with the original algorithm, this method can plan trajectories with lower curvature, less speed fluctuations, and fewer motor impacts, effectively improving the operating efficiency and safety of the mobile robot. Summary of the invention

[0007] The purpose of the present invention is to provide a local path planning method for a mobile robot based on centrifugal force and jerk constraints, which can effectively improve the motion stability of the mobile robot during operation due to load changes and turning.

[0008] To achieve the above object, the technical solution adopted by the present invention is as follows: a local path planning method for a mobile robot based on centrifugal force and jerk constraints, wherein the mobile robot is equipped with a laser radar and an industrial computer, and the method comprises the following steps:

[0009] S1. Obtain the current LiDAR scan frame, global cost map, current position of the mobile robot, current mass of the mobile robot and global path, build a local cost map based on the current LiDAR scan frame and global cost map, initialize the trajectory sequence B based on the local cost map and global path and build the original TEB hypergraph , the specific mathematical expression is:

[0010] ,

[0011] in The trajectory point pose of the mobile robot contains the position coordinates With heading angle , is the time interval between two trajectory points;

[0012] S2. In the original TEB hypergraph, in order to solve the problem that the mobile robot may roll over due to sudden changes in speed and excessive trajectory curvature when walking on a curve under different weight load conditions, a centrifugal force constraint penalty function is designed. The specific mathematical expression is:

[0013] ,

[0014] in is the centrifugal force, is the deviation tolerance, For the maximum centrifugal force, introduce centrifugal force constraint The specific mathematical expression is:

[0015] ,

[0016] in is the centrifugal force constraint weight coefficient;

[0017] S3. In the original TEB hypergraph part, in order to solve the problem of speed mutation of the mobile robot in the local path planning process and reduce the motor impact, the linear velocity constraint penalty function is designed. And add angular velocity constraint penalty function The specific mathematical expression is:

[0018] ,

[0019] ,

[0020] in is the linear velocity of the mobile robot, that is, the third-order derivative of the linear velocity, is the angular velocity of the mobile robot, that is, the third-order derivative of the angular velocity, is the deviation tolerance, is the maximum acceleration line speed, For the maximum angular velocity, introduce the linear velocity constraint And add angular velocity constraints The specific mathematical expression is:

[0021] ,

[0022] ,

[0023] in and are the linear velocity constraint weight coefficient of Jiajiajia and the angular velocity constraint weight coefficient of Jiajiajia respectively;

[0024] S4. Improved TEB hypergraph for introducing centrifugal force and jerk constraints Use g2o tools to optimize and obtain the best trajectory sequence , the best trajectory sequence Input to the trajectory tracking controller to calculate the control signal of the mobile robot. The specific mathematical expression is:

[0025] ,

[0026] .

[0027] Furthermore, in step S1, the method for obtaining the current mass of the mobile robot can be obtained through a force sensor. When there is no force sensor, the mass can be obtained according to the current working mode of the mobile robot.

[0028] Further, in step S2, the centrifugal force The specific mathematical expression is:

[0029] ,

[0030] in is the current mass of the mobile robot, and Respectively represent and The heading angle of the mobile robot at each trajectory point, and Respectively represent and The position coordinates of the mobile robot at each trajectory point, For the and The time interval between trajectory points.

[0031] Further, in step S3, the specific mathematical expressions of the Jiajiajia linear velocity and the Jiajiajia angular velocity are:

[0032] ,

[0033] ,

[0034] ,

[0035] ,

[0036] in , , , and are the position coordinates of the mobile robot at five adjacent trajectory points, , , , and is the heading angle of the mobile robot at five adjacent trajectory points, , , and is the time interval between five adjacent trajectory points.

[0037] Compared with the prior art, the advantages of the present invention are:

[0038] The present invention introduces centrifugal force constraints into the original TEB algorithm to reduce the average curvature of the trajectory, improve the operating stability of the mobile robot under different load conditions, and automatically adjust the turning speed of the mobile robot under different load conditions; at the same time, the jerk constraint is introduced to improve the trajectory smoothness and the operating efficiency of the mobile robot, thereby reducing the number of speed mutations and motor impacts. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 is a flow chart of the steps of the present invention;

[0040] Figure 2 is the original TEB hypergraph in the specific implementation of the present invention;

[0041] Figure 3 It is an improved TEB hypergraph in the specific implementation of the present invention. DETAILED DESCRIPTION

[0042] In order to make the purpose and advantages of the present invention more clearly understood, the present invention is specifically described below in conjunction with embodiments. It should be understood that the following text is only used to describe one or several specific embodiments of the present invention, and does not strictly limit the scope of protection of the specific claims of the present invention.

[0043] A local path planning method for a mobile robot based on centrifugal force and jerk constraints, wherein the mobile robot is equipped with a laser radar and an industrial computer, characterized in that the method comprises the following steps:

[0044] S1. Obtain the current LiDAR scan frame, global cost map, current position of the mobile robot, current mass of the mobile robot and global path, build a local cost map based on the current LiDAR scan frame and global cost map, initialize the trajectory sequence B based on the local cost map and global path and build the original TEB hypergraph , the specific mathematical expression is:

[0045] ,

[0046] in The trajectory point pose of the mobile robot contains the position coordinates With heading angle , is the time interval between two trajectory points;

[0047] The global cost map is constructed using the Cartographer algorithm. The current lidar scan frame is aligned with the global cost map using the ICP algorithm to obtain the current position of the mobile robot. A 5m*5m rectangle is set with the mobile robot as the center. Obstacle information is obtained based on the current lidar scan frame to construct a local cost map. The global path from the start point to the end point is calculated using the A-star algorithm. The construction is done to minimize the running time and include kinematic constraints. , obstacle constraints , robot performance constraints , Waypoint Constraints The original TEB hypergraph , the original TEB super map is attached Figure 2 As shown, the specific mathematical expression is:

[0048] ,

[0049] ,

[0050] in is the distance between the trajectory and the global path, is the maximum distance between the trajectory and the global path, the value is set to 1, is the distance between the trajectory and the obstacle, is the minimum distance between the trajectory and the obstacle, and the value is set to 0.6. and are the linear velocity and angular velocity of a point on the trajectory, and are the maximum linear velocity and maximum angular velocity of the mobile robot, respectively, and the value is set to 0.6. , , and They are kinematic constraint weight coefficient, robot performance constraint weight coefficient, path point constraint weight coefficient, and obstacle constraint weight coefficient. The values ​​are set to 3000, 1, 1, and 100 respectively. The offset tolerance Set to 0.1.

[0051] S2. In the original TEB hypergraph, in order to solve the problem that the mobile robot may roll over due to sudden speed changes and excessive trajectory curvature when walking on a curve under different weight load conditions, a centrifugal force constraint penalty function is designed. The specific mathematical expression is:

[0052] ,

[0053] in is the centrifugal force, is the deviation tolerance, For the maximum centrifugal force, introduce centrifugal force constraint The specific mathematical expression is:

[0054] ,

[0055] in is the centrifugal force constraint weight coefficient, its value is set to 1, the maximum centrifugal force Set to 0.3;

[0056] S3. In the original TEB hypergraph part, in order to solve the problem of speed mutation of the mobile robot in the local path planning process and reduce the motor impact, the linear velocity constraint penalty function is designed. And add angular velocity constraint penalty function The specific mathematical expression is:

[0057] ,

[0058] ,

[0059] in is the linear velocity of the mobile robot, that is, the third-order derivative of the linear velocity, is the angular velocity of the mobile robot, that is, the third-order derivative of the angular velocity, is the deviation tolerance, is the maximum acceleration line speed, For the maximum angular velocity, introduce the linear velocity constraint And add angular velocity constraints The specific mathematical expression is:

[0060] ,

[0061] ,

[0062] in and They are the linear velocity constraint weight coefficient of Jiajiajia and the angular velocity constraint weight coefficient of Jiajiajia, and their values ​​are set to 1. The maximum linear velocity of Jiajiajia Set to 1, maximum angular velocity Set to 5;

[0063] S4. Improved TEB hypergraph for introducing centrifugal force and jerk constraints Use g2o tools to optimize and obtain the best trajectory sequence , the best trajectory sequence Input to the trajectory tracking controller to calculate the control signal of the mobile robot. The specific mathematical expression is:

[0064] ,

[0065] ,

[0066] Improved TEB super diagram as attached Figure 3 As shown, according to the optimal trajectory sequence Computing control signals for mobile robots The specific mathematical expression is:

[0067] .

[0068] In step S1, the method for obtaining the current mass of the mobile robot can be obtained through a force sensor. When there is no force sensor, the mass can be obtained according to the current working mode of the mobile robot.

[0069] In step S2, the centrifugal force The specific mathematical expression is:

[0070] ,

[0071] in is the current mass of the mobile robot, and Respectively represent and The heading angle of the mobile robot at each trajectory point, and Respectively represent and The position coordinates of the mobile robot at each trajectory point, For the and The time interval between trajectory points.

[0072] In step S3, the specific mathematical expressions of the linear velocity and angular velocity are:

[0073] ,

[0074] ,

[0075] ,

[0076] ,

[0077] in , , , and are the position coordinates of the mobile robot at five adjacent trajectory points, , , , and is the heading angle of the mobile robot at five adjacent trajectory points, , , and is the time interval between five adjacent trajectory points.

[0078] The present invention aims at the problem that the existing TEB algorithm does not fully consider the path planning under different load conditions in a dynamic environment, and proposes a local path planning method for a mobile robot based on centrifugal force and jerk constraints. The existing TEB algorithm mainly focuses on smoothness and obstacle avoidance constraints when planning the trajectory, but does not consider the impact of load changes on the turning behavior of the mobile robot. Especially when the load is large, the mobile robot needs to slow down the turning speed to maintain stability and avoid excessive tilting or loss of control. However, the TEB algorithm may not be able to automatically adjust the turning speed when the load changes greatly, resulting in a less than ideal planned trajectory. To address this problem, the present invention can adjust the turning speed and trajectory of the mobile robot in real time by introducing centrifugal force and jerk constraints, thereby improving the stability of the mobile robot's movement during load changes and turning, thereby optimizing the local path planning effect.

[0079] The above is only a preferred embodiment of the present invention. It should be noted that, for those skilled in the art, several improvements and modifications can be made without departing from the principles of the present invention, and these improvements and modifications should also be considered as the protection scope of the present invention. The structures, devices and operating methods not specifically described and explained in the present invention shall be implemented according to the conventional means in the art unless otherwise specified and limited.

Claims

1. A local path planning method for a mobile robot based on centrifugal force and jerk constraints, wherein the mobile robot is equipped with a laser radar and an industrial computer, characterized in that: The method comprises the following steps: S1. Obtain the current lidar scan frame, global cost map, current position of the mobile robot, current mass of the mobile robot and global path, build a local cost map based on the current lidar scan frame and global cost map, initialize the trajectory sequence B based on the local cost map and the global path and build the original TEB hypergraph f(B). The specific mathematical expression is: B={s1,ΔT1,s2,…,ΔT n-1 ,s n }, Where s is the trajectory point pose of the mobile robot, including the position coordinates p and the heading angle θ, and ΔT is the time interval between two trajectory points; S2. In the original TEB hypergraph, in order to solve the problem that the mobile robot may roll over due to sudden speed changes and excessive trajectory curvature when walking on a curve under different weight load conditions, a centrifugal force constraint penalty function is designed. The specific mathematical expression is: Among them, F c is the centrifugal force, ε is the allowable deviation, F cmax For the maximum centrifugal force, introduce centrifugal force constraint The specific mathematical expression is: Where α is the centrifugal force constraint weight coefficient; S3. In the original TEB hypergraph, in order to solve the problem of speed mutation of the mobile robot in the local path planning process and reduce the motor impact, the linear velocity constraint penalty function f is designed. snap_v And add the angular velocity constraint penalty function f snap_w The specific mathematical expression is: in is the linear velocity of the mobile robot, that is, the third-order derivative of the linear velocity, is the angular velocity of the mobile robot, that is, the third-order derivative of the angular velocity, ε is the allowable deviation, is the maximum acceleration line speed, is the maximum angular velocity of Jiajiajia, and the linear velocity constraint E is introduced snap_v and add the angular velocity constraint E snap_w The specific mathematical expression is: Where β and γ are the weight coefficients of the linear velocity constraint and the angular velocity constraint, respectively; S4. The improved TEB hypergraph f′(B) with centrifugal force and jerk constraints is optimized using the g2o tool to obtain the optimal trajectory sequence B * , the best trajectory sequence B * Input to the trajectory tracking controller to calculate the control signal of the mobile robot. The specific mathematical expression is: B * =arg min f′(B)={s1 * ,ΔT1 * ,s2 * ,…,ΔT k-1 * ,s k * }。 2. The method for local path planning of a mobile robot based on centrifugal force and jerk constraints according to claim 1, characterized in that: In step S1, the method for obtaining the current mass of the mobile robot is to obtain it through a force sensor. When there is no force sensor, the mass is obtained according to the current working mode of the mobile robot.

3. The method for local path planning of a mobile robot based on centrifugal force and jerk constraints according to claim 1, characterized in that: In step S2, the centrifugal force F c The specific mathematical expression is: Where m is the current mass of the mobile robot, θ i+1 With θ i denote the heading angle of the mobile robot at the i+1th and ith trajectory points, respectively, and p i+1 With p i Respectively represent the position coordinates of the mobile robot at the i+1th and ith trajectory points, ΔT i is the time interval between the i+1th and ith trajectory points.

4. The method for local path planning of a mobile robot based on centrifugal force and jerk constraints according to claim 1, characterized in that: In step S3, the specific mathematical expressions of the Jiajiajia linear velocity and the Jiajiajia angular velocity are: where p i+4 、p i+3 、p i+2 、p i+1 and p i are the position coordinates of the mobile robot at five adjacent trajectory points, θ i+4 ,θ i+3 ,θ i+2 ,θ i+1 and θ i is the heading angle of the mobile robot at five adjacent trajectory points, ΔT i+3 , ΔT i+2 , ΔT i+1 and ΔT i is the time interval between five adjacent trajectory points.

Images