CN112082555A - Curvature consistency path planning algorithm based on self-adaptive dynamic window method under narrow channel environment - Google Patents

Abstract

The invention discloses a curvature consistency path planning algorithm based on a self-adaptive dynamic window method in a narrow passage environment, and belongs to the technical field of robot navigation. Calculating the distance between the target function and the nearest barrier, and if the distance D is smaller than a threshold Dt, calculating a dynamic weight gamma of a velocity term vel in the target function; calculating the curvature k of the track corresponding to each sampling speed, thereby calculating a curvature similarity factor C_sim(ii) a Calculating values of leading items, dis items and vel items in an objective function of a standard dynamic window method, and substituting the values into a curvature similarity factor C_simAnd the dynamic weight gamma of the vel item is summed to obtain the optimal motion track and the corresponding execution speed at the next moment. The method solves the problems that when the robot carries out local path planning in a narrow passage, the robot is easy to fall into local optimum, so that the robot cannot reach a terminal point and the smoothness of the movement planning track is poor, so as to improve the rationality and consistency of the track planning in the narrow passage environment.

Description

Curvature consistency path planning algorithm based on self-adaptive dynamic window method under narrow channel environment

Technical Field

The invention belongs to the technical field of mobile robot navigation, and particularly relates to a curvature consistency path planning algorithm based on a self-adaptive dynamic window method in a narrow channel environment.

Background

The mobile robot motion planning is divided into global path planning and local motion planning according to different tasks required to be completed. The local motion planning is mainly responsible for generating motion behaviors, dynamic obstacle avoidance is carried out according to local environment information, and the local motion planning is a key factor for reflecting the intelligent level of the mobile robot.

The local motion planning algorithm of the mobile robot, which is widely applied at present, is a Dynamic Window Approach (DWA). Firstly, sampling speeds meeting constraints in a speed space; then simulating the motion tracks of the sampling speeds in a certain period according to a kinematic model of the robot; and finally, scoring the motion tracks according to the target function, selecting the motion track with the highest score as the motion track of the next moment, and sending the corresponding speed to the chassis control module. However, the local motion planning of the mobile robot based on the dynamic window method has the following problems: 1) robots in narrow passages tend to get stuck in local optima and fail to reach the end point. 2) The motion track of the robot in the narrow passage is not smooth enough.

Disclosure of Invention

The invention aims to: aiming at the problem that the existing mobile robot local path planning algorithm cannot smoothly pass through a narrow channel, the curvature consistency path planning algorithm based on the self-adaptive dynamic window method is provided under the environment of the narrow channel.

The technical scheme of the invention is as follows:

s1, sampling in a speed space, calculating the distance D between each simulated track and the nearest barrier, judging the relation between D and a threshold Dt, if D is more than Dt, performing the second step, otherwise, calculating the dynamic weight gamma of the linear velocity v;

s2, calculating corresponding curvature for each sampled speed

And comparing the curvature with the curvature of the track at the previous moment to calculate a curvature similarity factor C_sim. Wherein the normalization is performed to prevent an item from affecting the result too much;

s3, according to the three inputs, the included angle heading between the robot and the terminal point, and the distance dis and the speed between the nearest barriersCalculating the level and normalizing to obtain an objective function, substituting the objective function into gamma and C obtained by calculation in S1 and S2_simObtaining a new objective function;

s4, scoring all the sampled tracks by executing S3, selecting the track with the highest score as an optimal track, and taking the corresponding linear velocity and angular velocity as the motion command at the current moment;

s5, calculating the distance Dg between the current position and the target point, judging the relation between the Dg and Dgt, and if the Dg is less than Dgt, ending the operation; otherwise, execution continues with S1.

In conclusion, the beneficial effects of the invention are as follows: the mobile robot can advance at a low speed through a parameter self-adaptive mechanism under the environment of a narrow passage, but the track is unsmooth due to the low speed, and the curvature constraint factor can prevent the robot from steering at a large angle when the robot passes through the narrow passage at the low speed, so that the curvature consistency of the track is ensured. When the vehicle is far away from the narrow passage, the vehicle can resume high-speed movement, and the driving efficiency is increased. Therefore, the method and the device combine the dynamic adjustment of the weight and the curvature similarity factor mechanism to improve the reasonability and consistency of the trajectory planning in the narrow channel environment.

Drawings

FIG. 1 is a flow chart of the present invention;

FIG. 2 is an experimental graph of the application of the original DWA algorithm through a narrow channel;

FIG. 3 is an experimental diagram of the application of the adaptive DWA algorithm through a narrow channel;

fig. 4 is an experimental diagram of the improved DWA algorithm of the present invention as it passes through a narrow channel.

Detailed Description

In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail with reference to the following embodiments and accompanying drawings.

As shown in fig. 2, the original dynamic window path planning algorithm falls into a local optimum value when passing through a narrow channel, and thus cannot pass through the narrow channel smoothly and safely.

The method comprises the following concrete implementation steps:

s1, sampling in a speed space, calculating the track simulated by each sampled speed and the distance D of the nearest obstacle, judging the relation between D and a threshold Dt, if D is more than Dt, performing the second step, otherwise, calculating the dynamic weight gamma of the linear speed v:

wherein γ ∈ [ γ ]_min,γ_max]。

As shown in fig. 3, the speed term parameter in the objective function can be dynamically adjusted to smoothly pass through a narrow passage, but an uneven track occurs at a low speed, so that the track consistency is poor at this time, and the running efficiency of the mobile robot is affected.

S2, calculating corresponding curvature for each sampled speed

And comparing the curvature with the curvature of the track at the previous moment to calculate a curvature similarity factor C_sim：

Where normalization is performed to prevent an item from affecting the result too much.

As shown in fig. 4, the smoothness and consistency of the trajectory are improved by adding the curvature similarity evaluation factor.

S3, calculating and normalizing according to three input headers, dis and vel of the original dynamic window method, and substituting the linear velocity dynamic weight and the curvature similarity factor calculated in the steps 1 and 2 to obtain a new objective function as follows:

G(v,w)＝α*heading(v,w)+β*dis(v,w)+γ*vel(v,w)+*C_sim，

wherein, gamma is the dynamic weight calculated in the step 1, C_simAnd (4) obtaining the curvature similarity factor after normalization in the step (2).

And S4, scoring all the sampled tracks by executing S3, selecting the track with the highest score as the optimal track, wherein the corresponding linear velocity and angular velocity are the motion commands at the current moment.

S5, calculating the distance Dg between the current position and the target point, judging the relation between the Dg and Dgt, and if the Dg is less than Dgt, ending the operation; otherwise, execution continues with S1.

Comparing fig. 2, 3 and 4, it can be seen that: the original dynamic window method is easy to fall into a local optimal value in a narrow channel, so that safe crossing cannot be realized; the robot can pass through a narrow channel at a lower speed by introducing a weight dynamic adjustment mechanism, but the track curvature consistency is poor; and the reasonability and curvature consistency of the trajectory planning under the narrow channel environment can be ensured simultaneously by combining the dynamic weight adjustment and curvature similarity factor mechanism to optimize the objective function.

For example, in the moving process of the intelligent wheelchair in an indoor narrow corridor, the speed can be reduced near the corner of the corridor, the intelligent wheelchair turns at a lower speed and safely passes through the corridor, the planning result is more reasonable, and the safety is facilitated. And the curvature similarity factor can reduce the large-range and unnecessary steering in the process that the mobile robot moves forward in the corridor, increase the curvature consistency of the track, make the running more smooth and stable, and is favorable for the comfort.

Claims (2)

1. A curvature consistency path planning algorithm based on a self-adaptive dynamic window method in a narrow channel environment is characterized in that an optimal track is selected based on weight dynamic adjustment and a curvature similarity factor mechanism, and the reasonability and curvature consistency of track planning in the narrow channel environment are improved.

2. The trajectory planning method based on the weight dynamic adjustment and curvature similarity factor mechanism according to claim 1, comprising the following steps:

s1, sampling in a speed space, calculating the distance D between each simulated track and the nearest barrier, judging the relation between D and a threshold Dt, if D is more than Dt, performing the second step, otherwise, calculating the dynamic weight gamma of the linear velocity v;

s2, calculating corresponding curvature for each sampled speed

And comparing the curvature with the curvature of the track at the previous moment to calculate a curvature similarity factor C_sim. Wherein the normalization is performed to prevent an item from affecting the result too much;

s3, calculating and normalizing the distance dis and the speed vel between the robot and the terminal point and the nearest obstacle according to the three inputs to obtain an objective function, and substituting the objective function into the linear velocity dynamic weight gamma and the curvature similarity factor C obtained by calculation in S1 and S2_simObtaining a new objective function;

s4, scoring all the sampled tracks by executing S3, selecting the track with the highest score as an optimal track, and taking the corresponding linear velocity and angular velocity as the motion command at the current moment;

s5, calculating the distance Dg between the current position and the target point, judging the relation between the Dg and Dgt, and if the Dg is less than Dgt, ending the operation; otherwise, execution continues with S1.

Images