A motion planning method based on convex optimization smoothing DWA

Abstract

The application discloses a kind of motion planning methods based on convex optimization smooth DWA, first based on DWA algorithm in velocity space collection speed, simulate the robot motion trajectory within future 3 seconds through these speeds;Then the heading angle deviation of motion trajectory, the distance of trajectory end and terminal, the distance between trajectory and obstacle is evaluated in several aspects, select the optimal one group trajectory;Output this group optimal trajectory angular velocity and linear velocity, construct objective function and bring in angular velocity linear velocity and solve by convex optimization, obtain the angular velocity and linear velocity after smoothing;Finally, by calculating the average absolute error and root mean square error of angular velocity linear velocity sequence before and after optimization, verify that the path after optimization does not change obviously, finally realize robot motion planning.

Description

Technical Field

[0001] This invention relates to the field of robot motion planning, and in particular, to a motion planning method based on convex optimization smooth DWA. Background Technology

[0002] With the rapid advancement of technology, artificial intelligence is gradually being applied to various aspects of people's lives, such as smart homes and intelligent robots. Among these, intelligent robots, due to their comprehensive functions, have attracted increasing attention from scholars. However, the wide range of applications also presents more significant challenges for mobile robots. Therefore, assisting mobile robots in completing demanding tasks and developing effective motion planning mechanisms is of paramount importance.

[0003] To address this issue, this invention proposes a motion planning method based on the Dynamic Window Approach (DWA). This method addresses the problem that traditional DWA algorithms generate velocity sequences with frequent fluctuations, making it difficult to control robot movement in complex environments such as rugged terrain. By applying convex optimization smoothing to the velocity sequences generated by the DWA algorithm, the method achieves better robot motion control. It also provides a new approach to robot motion planning. Summary of the Invention

[0004] The purpose of this invention is to provide a motion planning method based on convex optimization smooth DWA for robot motion planning, which can further improve the stability of robot motion planning.

[0005] To achieve the above objectives, the solution of the present invention is:

[0006] The motion planning method based on convex optimization smooth DWA includes the following steps (1) to (3):

[0007] Figure 1 As shown, a motion planning method based on convex optimization smooth DWA includes the following steps:

[0008] Step (1): Use the DWA algorithm to collect angular velocity and linear velocity in the robot velocity space. Simulate the robot's motion trajectory in the next 3 seconds using these velocities. The velocity space is composed of the intersection of the hardware's limitation on the robot's speed, the motor's limitation on the robot's acceleration, and the obstacle's limitation on the robot's speed.

[0009] Step (2): By evaluating the trajectory in terms of heading angle deviation, distance between the end of the trajectory and the destination, and distance between the trajectory and the obstacle, the optimal trajectory is selected.

[0010] Step (3): Output the angular velocity and linear velocity of this optimal trajectory, construct the objective function, substitute the angular velocity and linear velocity into the convex optimization solution, and obtain the smoothed angular velocity and linear velocity to realize the motion planning of the robot.

[0011] Specifically, the steps for constructing the objective function in step (3) by substituting angular velocity and linear velocity for convex optimization are as follows:

[0012] Step 3.1: The set of angular velocities and linear velocities s of the output optimal trajectory is contaminated with noise e. The observed values ​​of s are:

[0013] s cor =s+e;

[0014] Step 3.2: Construct the optimized set of angular velocities and linear velocities The process is called signal reconstruction or noise removal, and it uses a quadratic smoothing function:

[0015] ;

[0016] in, It is a double diagonal matrix. This is the established objective function;

[0017]

[0018] Step 3.3: Minimize:

[0019] ;

[0020] Among them, we obtained and The optimal trade-off between them, where The parameter >0 parameterizes the optimal trade-off curve, controlling its smoothness, and yields the optimized set of angular and linear velocities. :

[0021] ;

[0022] Given a sparse identity matrix with 1s on its main diagonal, take... =3. Attached Figure Description

[0023] Figure 1 This is a flowchart of a motion planning method based on convex optimization smooth DWA. Detailed Implementation

[0024] The technical solution of the present invention will be further described below with reference to the accompanying drawings:

[0025] This invention provides a motion planning method based on convex optimization smooth DWA, the overall idea of ​​which is as follows:

[0026] First, multiple sets of velocities are collected in the velocity space based on the DWA algorithm. These velocities are used to simulate multiple sets of robot motion trajectories over the next time period. Then, the motion trajectories are evaluated using an evaluation function, and the optimal set of trajectories is selected to drive the robot's movement. The angular velocity and linear velocity sequence of this optimal trajectory is output. An objective function is constructed, and the angular velocity and linear velocity are substituted into it for convex optimization to obtain the smoothed angular velocity and linear velocity. Finally, the mean absolute error and root mean square error of the angular velocity and linear velocity sequences before and after optimization are calculated to verify that the path has not changed significantly after optimization.

[0027] like Figure 1 As shown, the motion planning method based on convex optimization smooth DWA of the present invention specifically includes the following steps (1) to (3):

[0028] Step (1): Use the DWA algorithm to collect angular velocity and linear velocity in the robot's velocity space, and use these velocity models

[0029] The robot's trajectory is planned for the next 3 seconds. The speed space is composed of the intersection of three factors: the hardware limit on the robot's speed, the motor limit on the robot's acceleration, and the obstacle limit on the robot's speed.

[0030] Step (2): By evaluating the trajectory in terms of heading angle deviation, distance between the end of the trajectory and the destination, and distance between the trajectory and the obstacle, the optimal trajectory is selected.

[0031] Step (3): Output the angular velocity and linear velocity of this optimal trajectory, construct the objective function, substitute the angular velocity and linear velocity into the convex optimization solution, and obtain the smoothed angular velocity and linear velocity to realize the motion planning of the robot.

[0032] The specific steps for constructing the objective function are as follows:

[0033] Step 3.1: The set of angular velocities and linear velocities s of the output optimal trajectory is contaminated with noise e. The observed values ​​of s are:

[0034] s cor =s+e;

[0035]

[0036] Step 3.2: Construct the optimized set of angular velocities and linear velocities The process is called signal reconstruction or noise removal, and it uses a quadratic smoothing function:

[0037] ;

[0038] in, It is a double diagonal matrix. This is the established objective function;

[0039] Step 3.3: Minimize:

[0040] ;

[0041] Among them, we obtained and The optimal trade-off between them, where The parameter >0 parameterizes the optimal trade-off curve, controlling its smoothness, and yields the optimized set of angular and linear velocities. :

[0042] ;

[0043] Given a sparse identity matrix with 1s on its main diagonal, take... =3.

[0044] By calculating the mean absolute error and root mean square error of the angular velocity and linear velocity sequences before and after optimization, it was verified that the path did not change significantly after optimization, thus verifying the feasibility of the method.

[0045] The above embodiments are merely illustrative of the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. Any modifications made to the technical solutions based on the technical concept proposed in this invention shall fall within the scope of protection of this invention.

Claims

1. A motion planning method based on convex optimization smooth DWA, characterized in that, Includes the following steps: Step (1): Use the DWA (Dynamic Window Approach) algorithm to collect angular velocity and linear velocity in the robot velocity space. Simulate the robot's motion trajectory in the next 3 seconds using these velocities. The velocity space is composed of the intersection of the hardware's limitation on the robot's velocity, the motor's limitation on the robot's acceleration, and the obstacle's limitation on the robot's velocity. Step (2): By evaluating the trajectory in terms of heading angle deviation, distance between the end of the trajectory and the destination, and distance between the trajectory and the obstacle, the optimal trajectory is selected. Step (3): Output the angular velocity and linear velocity of this optimal trajectory, and construct the objective function by substituting the angular velocity and linear velocity into it. By performing convex optimization, smoothed angular and linear velocities are obtained, enabling motion planning for the robot. The specific steps for constructing the objective function by substituting angular velocity and linear velocity into the convex optimization solution in step (3) are as follows: Step 3.1: The set of angular velocities and linear velocities s of the output optimal trajectory is contaminated with noise e. The observed values ​​of s are: ; Step 3.2: Construct the optimized set of angular velocities and linear velocities The process is called signal reconstruction or noise removal, and it uses a quadratic smoothing function: ; in, It is a double diagonal matrix. This is the established objective function; ； Step 3.3: Minimize: ; Among them, we obtained and The optimal trade-off between them, where The parameter >0 parameterizes the optimal trade-off curve, controlling its smoothness, and yields the optimized set of angular and linear velocities. : ; Given a sparse identity matrix with 1s on its main diagonal, take... =3.

Images