Dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm

By integrating Kalman filtering algorithm into the DWA algorithm to predict dynamic obstacle trajectory and design new evaluation functions, the problems of insufficient trajectory prediction and local optimal traps of DWA algorithm during dynamic obstacle avoidance are solved, and the obstacle avoidance of mobile robots is improved Accuracy and global planning capabilities ensure efficient obstacle avoidance of robots in complex environments.

CN118938924AInactive Publication Date: 2024-11-12SHAANXI UNIV OF SCI & TECH

Patent Information

Application Number
CN202411084008.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-08
Publication Date
2024-11-12
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

The existing DWA algorithm lacks prediction of dynamic obstacle trajectory during dynamic obstacle avoidance, resulting in slower robot obstacle avoidance response, low path planning efficiency, and easy to fall into dynamic local optimal traps, and unable to effectively deal with local optimal constructs of dynamic obstacles. trap.

Method used

Kalman filtering algorithm is used to predict dynamic obstacle trajectory, combined with the improved DWA algorithm, the obstacle position and velocity information is obtained through lidar, the obstacle trajectory prediction is used to predict obstacle trajectory, and dynamic obstacles are integrated into the DWA algorithm. Trajectory prediction information, increase the accuracy of obstacle avoidance and global planning capabilities, and design new evaluation functions to avoid falling into dynamic local optimal problems.

Benefits of technology

Improve the obstacle avoidance ability and path planning accuracy of mobile robots in complex environments, shorten obstacle avoidance time, enhance the prediction ability of dynamic obstacles, avoid falling into dynamic local optimal traps, and ensure that the robot can complete it safely and efficiently Task.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938924A_ABST
    Figure CN118938924A_ABST
Patent Text Reader

Abstract

The invention discloses a dynamic obstacle avoidance method based on fusion Kalman filtering and an improved DWA algorithm. The method comprises the following steps: 1, acquiring an initial position, a target position and dynamic and static obstacle information of a mobile robot by using a laser radar; 2, according to the dynamic obstacle position and speed observation information obtained by the laser radar, using a Kalman filtering algorithm to carry out obstacle trajectory prediction, and obtaining the optimal estimated values of the obstacle position and speed; 3, adding the obtained optimal estimation values of the positions and the speeds of the dynamic and static obstacles into a DWA algorithm; and 4, scoring speed pairs in a DWA algorithm speed window by using the improved evaluation function, and selecting the speed pair with the highest score until a target point is found, thereby solving the problem of easy falling into dynamic local optimum. According to the method, the obstacle avoidance capability and the path planning precision of the robot in a complex environment are improved, the robot is prevented from falling into a local optimal trap, and the global planning capability is improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of dynamic obstacle avoidance of mobile robots, and in particular relates to a dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm. Background Art

[0002] In the field of mobile robots, dynamic obstacle avoidance refers to the process of a mobile robot adjusting its path in real time to avoid obstacles and safely reach the target location in a constantly changing environment. With the rapid development of technology, mobile robots are widely used in various fields such as services, agriculture, and medical care. In practical applications, mobile robots need to face the interference of dynamic obstacles such as pedestrians, robots, and other moving objects. These obstacles may cause the mobile robot to reduce its speed, deviate from the predetermined trajectory, or even interrupt the mission. Therefore, the research on dynamic obstacle avoidance is of great significance to ensure that mobile robots can complete their tasks safely and efficiently in complex environments.

[0003] Dynamic Window Approach (DWA) is a local path planning algorithm for obstacle avoidance. It generates possible speed sampling points under the premise of considering speed and acceleration limits, and selects the optimal trajectory through an evaluation function.

[0004] When solving the dynamic obstacle avoidance problem, the DWA algorithm lacks trajectory prediction of dynamic obstacles and often treats dynamic obstacles as instantaneous static obstacles, ignoring the speed and direction of movement of the obstacles, resulting in slow obstacle avoidance response of the robot and low path planning efficiency.

[0005] In addition, Fan et al. (DOI:10.1109 / JSYST.2023.3274187.) proposed a low-speed turning strategy to help the robot escape from the U-shaped trap in response to the defect of local minimum in the DWA algorithm.

[0006] However, these technologies mainly solve the local optimal traps formed by static obstacles, but lack the processing of the local optimal traps formed by dynamic obstacles, that is, the dynamic obstacles and the mobile robot are prone to fall into a continuous parallel state. Summary of the invention

[0007] In order to overcome the problems existing in the above-mentioned prior art, the present invention provides a dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm, which improves the robot's obstacle avoidance ability and path planning accuracy in complex environments, avoids falling into local optimal traps, and improves the global planning ability.

[0008] In order to achieve the above object, the technical solution adopted by the present invention is:

[0009] A dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm includes the following steps:

[0010] Step 1: Use the laser radar to obtain the mobile robot's starting position, target position, and dynamic and static obstacle information;

[0011] Step 2: Based on the dynamic obstacle position and speed observation information obtained by the lidar, the Kalman filter algorithm is used to predict the obstacle trajectory and obtain the optimal estimated values ​​of the obstacle position and speed;

[0012] Step 3: Add the optimal estimated values ​​of the positions and velocities of the moving and static obstacles obtained above into the DWA algorithm to increase its prediction function for dynamic trajectories, greatly improving the accuracy of obstacle avoidance and shortening the obstacle avoidance time.

[0013] Step 4: Use the improved evaluation function to score the speed pairs in the speed window of the DWA algorithm, select the speed pair with the highest score until the target point is found, and solve the problem of being easily trapped in dynamic local optimality.

[0014] The step 1 specifically includes:

[0015] The laser radar is used to obtain the starting position, target position, and dynamic and static obstacle information of the mobile robot; the starting coordinate is (x q ,y q ), the target coordinates are (x z ,y z ), the obstacle coordinates are (ob xi ,ob yi ), i is the number of obstacles.

[0016] The step 2 specifically includes:

[0017] Record the position and speed of dynamic obstacles collected by LiDAR in real time;

[0018] Establish a system model based on the spatial state equation and observation equation;

[0019] Space state equation: X k =AX k-1 +Bu k-1 +w k-1

[0020] Observation equation: Z k =HX k +v k

[0021] Where: X k , X k-1 is the real state of the system at time k and k-1, Z kis the state observed at time k, A is the state transfer matrix, B is the control input matrix, H is the state observation matrix, and w k-1 is the random interference noise of the system state, v k is the observation noise;

[0022] Use the Kalman filter algorithm for prediction and correction, and output the optimal estimated value of the system state at time k and the predicted value of the system state at time k+dt;

[0023] The Kalman filter algorithm performs prediction and correction, and the process is as follows:

[0024] Prediction process: Based on the optimal estimated value of the system state at time k-1, the system state at time k is predicted through the following equation, and the corresponding system estimation error covariance matrix at this time is obtained:

[0025]

[0026] in: and They represent the predicted value of the system state at time k and the optimal estimated value of the system state at time k-1, respectively. represents the system estimation error covariance matrix corresponding to time k, Q represents the covariance matrix of the system working process;

[0027] Correction process: Through the Kalman filter gain matrix K k Combined with the system state prediction value at time k And the state Z observed by the sensor at time k k Get the optimal estimate of the system state at time k and the optimal estimate of the covariance of the filtering error at time k P k ;

[0028]

[0029] Where: R represents the measurement noise covariance matrix, and I is the identity matrix.

[0030] The step 3 specifically includes:

[0031] (1) Selecting windows: Obtain the dynamic window set V according to the speed constraint, acceleration constraint, and safety constraint. m =V n ∩V a ∩V d ;

[0032] (2) Simulate the predicted trajectory: Sample the velocity in the window and simulate the predicted trajectory;

[0033] (3) Obtaining dynamic obstacle prediction information: Define the speed window prediction time t, and use step 2 to obtain the optimal estimated values ​​of the position and speed of the dynamic obstacle within the prediction time;

[0034] (4) Information fusion: The heading angle deviation, minimum distance to the obstacle, braking distance, and speed are calculated through the state at the end of the trajectory. The minimum distance to the obstacle and braking distance are calculated based on the state at the end of the trajectory of the mobile robot and the optimal estimated value of the obstacle after t seconds in (3). The trajectory prediction information of dynamic obstacles is integrated into the DWA algorithm.

[0035] The step (1) is specifically:

[0036] Speed ​​Constraints:

[0037] V n ={v∈[v min ,v max ],ω∈[ω min ,ω max ]}

[0038] where v min and v max Indicates the minimum and maximum linear speed of the robot, w min and w max Indicates the minimum and maximum angular velocity of the robot;

[0039] Acceleration constraint: The set of motion velocities that can be achieved within a dynamic window period is defined as:

[0040]

[0041] Among them, v r and ω r are the linear velocity and angular velocity of the robot at the current moment, and are the maximum linear acceleration and maximum angular acceleration, respectively, which are mainly determined by the motor performance; Δt is the sampling time interval;

[0042] Safety constraints: The speed space that satisfies the robot's motion safety is defined as:

[0043]

[0044] Among them, dist(v,ω) is the distance to the nearest obstacle on the current motion trajectory;

[0045] According to the above three constraints, the intersection of the three speed constraints is taken to obtain the most suitable dynamic window v for the robot. m .

[0046] The step 4 specifically includes:

[0047] When selecting the optimal trajectory, the Kalman filter fusion DWA algorithm takes into account the robot's motion constraints and the influence of dynamic obstacles to ensure that the robot can safely reach the target point; in addition, a direction angle evaluation function is designed to ensure the path quality and avoid falling into the dynamic local optimal trap. The improved evaluation function is as follows:

[0048] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)+δ·bearing(v,ω))

[0049] Where, heading(v,ω) is the heading angle evaluation subfunction, which is the deviation angle between the predicted trajectory of the mobile robot and the target point. The smaller the deviation angle, the higher the score; dist(v,ω) is the obstacle distance evaluation subfunction, which is the minimum distance between the predicted trajectory and the obstacle at the current speed. The longer the distance, the higher the score; velocity(v,ω) is the speed evaluation subfunction. While ensuring safe obstacle avoidance, the faster the speed, the higher the score; bearing(v,ω) is the direction angle evaluation subfunction, which is the angle difference between the predicted trajectory of the mobile robot and the nearest dynamic obstacle. The larger the angle difference, the higher the score;

[0050] α, β, γ, and δ are weight coefficients of the evaluation function, and σ represents the normalization of the evaluation function.

[0051] The formula of the direction angle evaluation subfunction is Δθ=|θ robot -θ obstacle |, where θ robot is the angle of each predicted trajectory of the mobile robot, θ obstacle It is the current angle of the dynamic obstacle closest to the mobile robot. The specific usage steps are:

[0052] Analyze and record the heading angles of each obstacle that appears, and calculate the distance between each obstacle and the mobile robot;

[0053] Find the obstacle closest to the mobile robot and its corresponding heading angle;

[0054] Calculate the distance change of the obstacle within Δt=0.1s. If the distance change is 0, it is judged as a static obstacle and the evaluation sub-function is not used; if the distance is not 0, it is judged as a dynamic obstacle and the evaluation sub-function is used.

[0055] Through the above steps, a path with the highest score, taking into account the movement trend of dynamic obstacles, avoiding obstacles smoothly, and avoiding falling into the dynamic local optimum can be obtained, so that the mobile robot can complete the optimal planning of the obstacle avoidance path.

[0056] Beneficial effects of the present invention:

[0057] The present invention firstly integrates Kalman filtering into the DWA algorithm and uses Kalman filtering to estimate the speed and position of dynamic obstacles, so that the DWA algorithm can take more comprehensive considerations when planning obstacle avoidance trajectories. Then, the evaluation function of the fusion algorithm is optimized and the problem of easily falling into dynamic local optimal solutions is solved by adding new evaluation sub-functions.

[0058] The Kalman filter algorithm is combined with the DWA algorithm. By adding the future motion information of dynamic obstacles predicted by the Kalman filter algorithm to the DWA evaluation function, the prediction ability of dynamic obstacles is enhanced while retaining the real-time performance of DWA, and the accuracy and reliability of the mobile robot's obstacle avoidance are improved. The DWA algorithm is improved by adding a new direction angle evaluation sub-function to consider the angle difference between each predicted trajectory of the mobile robot and the obstacle in real time. The larger the angle difference, the higher the score. The introduction of the angle difference can avoid falling into the problem of continuous parallel dynamic local optimality, and improve the timeliness and globality of the mobile robot's obstacle avoidance. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 It is a flowchart of the overall process of the present invention.

[0060] Figure 2 This is an environmental information diagram in an embodiment of the present invention.

[0061] Figure 3 Schematic diagram of the predicted trajectory of the dynamic window of the DWA algorithm.

[0062] Figure 4 1 is a comparison diagram of DWA obstacle avoidance with and without trajectory prediction information in an embodiment of the present invention, wherein (a) is a simulation diagram of obstacle avoidance trajectory without prediction, and (b) is a simulation diagram of obstacle avoidance trajectory with prediction.

[0063] Figure 5 Schematic diagram of the dynamic local optimal trap that often appears when the DWA algorithm avoids obstacles.

[0064] Figure 6 Schematic diagram of the obstacle avoidance effect after adding a new evaluation sub-function. DETAILED DESCRIPTION

[0065] The present invention will be further described in detail below in conjunction with the accompanying drawings.

[0066] Combination Figure 1 , a dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm, the specific steps of the invention are as follows:

[0067] Step 1: Use LiDAR to obtain the mobile robot's starting position, target position, and dynamic and static obstacle information.

[0068] Specifically:

[0069] Record the mobile robot information obtained by the laser radar and set the corresponding simulation environment information, such as Figure 2 As shown in the figure, the purple hollow circles represent the mobile robot, the blue solid line is the motion trajectory of the mobile robot, and the green solid line is the predicted trajectory under the dynamic window of the DWA algorithm; the red solid circles represent static obstacles, and 4 static obstacles are set with coordinates of (3, 1), (2, 6), (5, 9), and (6, 5) respectively; the blue solid circles represent dynamic obstacles, and 3 dynamic obstacles are set with starting coordinates of (-3, 8), (-1, 4), and (7, 0) respectively. The blue dotted line is the motion trajectory of the dynamic obstacles; the starting point is set to (0, 0) and the end point is set to (10, 10).

[0070] Step 2: Based on the dynamic obstacle position and speed observation information obtained by the lidar, the Kalman filter algorithm is used to predict the obstacle trajectory and obtain the optimal estimated values ​​of the obstacle position and speed.

[0071] Specifically:

[0072] The position and speed of dynamic obstacles collected by the laser radar in real time are recorded. In this implementation example, it is assumed that the dynamic obstacles move in a straight line at a uniform speed of 0.55 m / s.

[0073] The system model is established based on the spatial state equation and observation equation.

[0074] Space state equation: X k =AX k-1 +Bu k-1 +w k-1

[0075] Observation equation: Z k =HX k +v k

[0076] Where: X k , X k-1 is the real state of the system at time k and k-1, Z k is the state observed at time k, is the state transfer matrix, B=0 is the control input matrix, is the state observation matrix, w k-1 is the random interference noise of the system state, v k To observe the noise, Δt is taken as 0.1.

[0077] The Kalman filter algorithm is used for prediction and correction, and the optimal estimated value of the system state at time k and the predicted value of the system state at time k+Δt are output, where Δt is 0.1.

[0078] In combination with this implementation example, the Kalman filter algorithm performs prediction and correction, and the specific process is as follows:

[0079] Prediction process: According to the simulation, the optimal estimated value of the system state at a certain previous moment is selected as The following equation is used to predict the state of the system at the current moment, and the corresponding system estimation error covariance matrix at this moment is obtained:

[0080]

[0081]

[0082] in: and They represent the predicted value of the system state at the current moment and the optimal estimated value of the system state at the previous moment, respectively. represents the system estimation error covariance matrix corresponding to the current moment, Q represents the covariance matrix of the system working process,

[0083] Correction process: Through the Kalman filter gain matrix K k Combined with the current system status prediction value And the state Z observed by the sensor at the current moment k Get the optimal estimate of the system state at the current moment The optimal estimate of the covariance of the filtering error at the current moment P k .

[0084]

[0085] Where: R represents the measurement noise covariance matrix, I is the unit matrix, and the above process is repeated at other times.

[0086] Step 3: Add the optimal estimated values ​​of the obstacle position and speed obtained above to the DWA algorithm to increase its prediction function for dynamic trajectories. Due to the introduction of Kalman filtering, the mobile robot can switch from avoiding the current position of dynamic obstacles to avoiding the predicted position of dynamic obstacles, avoiding obstacles in advance, and eliminate the noise caused by the laser radar when measuring the obstacle position in actual applications. Therefore, the accuracy of obstacle avoidance is greatly improved and the obstacle avoidance time is shortened.

[0087] Specifically:

[0088] (1) Determine the speed window: Determine the search space, i.e., the dynamic window set, based on the speed constraint, acceleration constraint, and safety constraint.

[0089] Speed ​​constraint: Since the speed of the robot is limited by the distance between itself and the obstacle, the speed of the robot is limited to a certain range.

[0090] V n ={v∈[v min ,v max ],ω∈[ω min ,ω max ]}

[0091] where v min and v max Indicates the minimum and maximum linear speed of the robot, w min and w max Indicates the minimum and maximum angular velocity of the robot.

[0092] Acceleration constraint: Considering that the robot is limited by the acceleration and deceleration performance of the motor, the set of motion speeds that can be achieved within a dynamic window period is defined as:

[0093]

[0094] Among them, v r and ω r are the linear velocity and angular velocity of the robot at the current moment, and They are the maximum linear acceleration and maximum angular acceleration, respectively, which are mainly determined by the motor performance; Δt is the sampling time interval.

[0095] Safety constraints: When an obstacle appears, in order to ensure that the mobile robot can stop safely before hitting the obstacle, the speed space that satisfies the robot's motion safety is defined as:

[0096]

[0097] Among them, dist(v,ω) is the distance to the nearest obstacle on the current motion trajectory.

[0098] According to the above three constraints, the intersection of the three speed constraints is taken to obtain the most suitable dynamic window v for the robot. m :

[0099] V m =V n ∩V a ∩V d

[0100] (2) Simulate the predicted trajectory: define the speed window prediction time t = 3s, sample the speed in the window, and simulate the predicted trajectory. The predicted trajectory of the dynamic window is as follows: Figure 3As shown in the figure, among the several predicted trajectories, some will collide with obstacles, while others will successfully avoid obstacles. The optimal trajectory for obstacle avoidance is selected through the subsequent evaluation function scoring.

[0101] (3) Obtaining dynamic obstacle prediction information: Use step 2 to obtain the optimal estimated values ​​of the dynamic obstacle position and speed within the prediction time of 3 s, and estimate at intervals of Δt = 0.1 s.

[0102] (4) Information fusion: The heading angle deviation, minimum distance to the obstacle, braking distance, speed, etc. are calculated by predicting the state at the end of the trajectory. The minimum distance to the obstacle and the braking distance are calculated based on the state of the mobile robot predicted at the end of the trajectory 3 seconds later in (2) and the optimal estimated value of the obstacle 3 seconds later in (3). The trajectory prediction information of dynamic obstacles is integrated into the DWA algorithm.

[0103] In order to make the comparison results clear, the obstacles in this example are simplified in this section, and only one dynamic obstacle (-1, 4) is retained, and other static obstacles and parameters remain unchanged. Figure 4 Comparison of DWA obstacle avoidance with and without trajectory prediction information.

[0104] exist Figure 4 a. In the DWA algorithm without prediction, although there is no collision problem, the existence of observation noise causes the obstacle measurement position to oscillate back and forth, hindering the search of the dynamic window. The DWA algorithm stops running many times, waits in place, and re-searches the window, which increases the obstacle avoidance time.

[0105] exist Figure 4 In b, the complete obstacle avoidance trajectory with and without prediction is given at the same time. The DWA algorithm with added prediction information can avoid obstacles smoothly, avoid stagnation, shorten the obstacle avoidance time, and make the obstacle avoidance trajectory smoother.

[0106] Step 4: Use the evaluation function in (3) that incorporates the dynamic obstacle trajectory prediction information to score the predicted trajectory of the mobile robot in the speed window, and add a new evaluation sub-function to avoid falling into the dynamic local optimal trap.

[0107] Dynamic local optimum traps such as Figure 5 As shown in the figure, when the mobile robot travels to (6, 6.8), the dynamic obstacle travels roughly to the vicinity of (5, 8). In order to successfully avoid the obstacle and reach the target point, the mobile robot chooses to accelerate and detour, resulting in the mobile robot and the dynamic obstacle having the same direction and similar speed, thus falling into the dynamic local optimum and continuing to travel in parallel, unable to successfully reach the target point.

[0108] The improvement effect after adding the new evaluation sub-function is as follows Figure 6As shown in the figure, based on the comprehensive consideration of the other three evaluation sub-functions, it is more inclined to select the predicted trajectory with a larger angle difference between the mobile robot and the dynamic obstacle to achieve deceleration and detour, so as to avoid the state of continuous parallelism between the two. The improved evaluation function is as follows:

[0109] G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)+δ·bearing(v,ω))

[0110] heading(v,ω): heading angle evaluation subfunction, whose formula is 180°-θ, where θ is the angle between the direction of the predicted trajectory and the target point. The smaller θ is, the smaller the deviation angle between the trajectory and the target is, and the higher the score is.

[0111] dist(v,ω): Obstacle distance evaluation subfunction, which measures the shortest distance between the corresponding simulated trajectory and the obstacle at the current speed. The farther away from the obstacle, the higher the score. If there is no obstacle or the closest distance is greater than the set threshold, the evaluation function score is set to the threshold. The purpose of this function is to exclude sampling paths that may collide with obstacles or are too close to obstacles to achieve safe obstacle avoidance of the robot.

[0112] velocity(v,ω): velocity evaluation subfunction, which indicates the current velocity. It can be directly expressed by the current linear velocity. The faster the speed, the higher the evaluation function score. While ensuring safe obstacle avoidance, it can select the fastest navigation trajectory.

[0113] bearing(v,ω): direction angle evaluation subfunction, its formula is Δθ=|θ robot -θ obstacle |, where θ robot is the angle of each predicted trajectory of the mobile robot, θ obstacle It is the current angle of the dynamic obstacle closest to the mobile robot. The larger Δθ is, the greater the angle difference between the mobile robot and the obstacle is, and the higher the score is. The purpose of adding this direction angle evaluation subfunction is to avoid falling into the dynamic local optimal trap, where the mobile robot and the obstacle are in a parallel state with the same direction and similar speed.

[0114] Among them, the specific steps of using the direction angle evaluation subfunction are:

[0115] Analyze and record the heading angles of each obstacle that appears, and calculate the distance between each obstacle and the mobile robot;

[0116] Find the obstacle closest to the mobile robot and its corresponding heading angle;

[0117] Calculate the distance change of the obstacle within Δt=0.1s. If the distance change is 0, it is judged as a static obstacle and the evaluation sub-function is not used; if the distance is not 0, it is judged as a dynamic obstacle and the evaluation sub-function is used.

[0118] α, β, γ, and δ are weight coefficients of the evaluation function. σ indicates that in order to avoid a certain factor from being overweighted in the evaluation function, heading(v,ω), dist(v,ω), velocity(v,ω), and bearing(v,ω) are normalized to make the trajectory smoother. In this embodiment, the weight parameters of the evaluation function are α=0.4, β=0.2, γ=0.8, and δ=0.05. In this embodiment, combined with Figure 6 , when the mobile robot travels to the vicinity of (6, 7.8), the specific scores of each evaluation function are shown in Table 1:

[0119] Table 1 Specific score table of each evaluation function

[0120] Predicted trajectory Heading Angle Score Distance score Speed ​​score Direction Angle Score Overall 1 173.4964 0.6364 0.4800 39.1000 0.0926 2 170.0580 0.6519 0.4800 42.1000 0.0927 3 166.6242 0.6693 0.4800 45.1000 0.0928 4 163.1958 0.6883 0.4800 48.1000 0.0930 5 159.7736 0.7089 0.4800 51.1000 0.0932 6 156.3587 0.6753 0.4800 0 0.0865 7 152.9518 0.6409 0.4800 0 0.0853 8 149.5537 0.6072 0.4800 0 0.0841 9 169.9747 0.6255 0.4900 42.1000 0.0932 10 166.5292 0.6435 0.4900 45.1000 0.0934 11 163.0893 0.6632 0.4900 48.1000 0.0935 12 159.6561 0.6846 0.4900 51.1000 0.0937 13 156.2303 0.6666 0.4900 0 0.0873 14 152.8129 0.6313 0.4900 0 0.0861 15 159.5371 0.6604 0.5000 51.1000 0.0943 16 156.1003 0.6584 0.5000 0 0.0882

[0121] In the current speed window, there are 16 predicted trajectories, which are scored using the evaluation function. When the direction angle score is 0, it means that the obstacle closest to this predicted trajectory is a static obstacle, so the evaluation subfunction is not considered; when the score is not 0, it means that the obstacle closest to this predicted trajectory is a dynamic obstacle, and the score is the angle difference between the predicted trajectory of the mobile robot and the nearest dynamic obstacle. Finally, considering the four evaluation functions comprehensively, it is concluded that the 15th trajectory has the highest score, and its corresponding angular velocity and linear velocity are selected as the state of the mobile robot at the next moment. The above process is followed for other moments.

Claims

1. A dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm, characterized in that: The steps include: Step 1: Use the laser radar to obtain the mobile robot's starting position, target position, and dynamic and static obstacle information; Step 2: Based on the dynamic obstacle position and speed observation information obtained by the lidar, the Kalman filter algorithm is used to predict the obstacle trajectory and obtain the optimal estimated values ​​of the obstacle position and speed; Step 3: Add the optimal estimated values ​​of the positions and velocities of the moving and static obstacles obtained above into the DWA algorithm; Step 4: Use the improved evaluation function to score the speed pairs in the speed window of the DWA algorithm, select the speed pair with the highest score until the target point is found, and solve the problem of being easily trapped in dynamic local optimality.

2. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 1, characterized in that: The step 1 specifically includes: The laser radar is used to obtain the starting position, target position, and dynamic and static obstacle information of the mobile robot; the starting coordinate is (x q ,y q ), the target coordinates are (x z ,y z ), the obstacle coordinates are (ob xi ,ob yi ), i is the number of obstacles.

3. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 1, characterized in that: The step 2 specifically includes: Record the position and speed of dynamic obstacles collected by LiDAR in real time; Establish a system model based on the spatial state equation and observation equation; Space state equation: X k =AX k-1 +Bu k-1 +w k-1 Observation equation: Z k =HX k +v k Where: X k , X k-1 is the real state of the system at time k and k-1, Z k is the state observed at time k, A is the state transfer matrix, B is the control input matrix, H is the state observation matrix, and w k-1 is the random interference noise of the system state, v k is the observation noise; The Kalman filter algorithm is used for prediction and correction, and the optimal estimated value of the system state at time k and the predicted value of the system state at time k+dt are output.

4. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 3 is characterized in that: The Kalman filter algorithm performs prediction and correction, and the process is as follows: Prediction process: Based on the optimal estimated value of the system state at time k-1, the system state at time k is predicted through the following equation, and the corresponding system estimation error covariance matrix at this time is obtained: in: and They represent the predicted value of the system state at time k and the optimal estimated value of the system state at time k-1, respectively. represents the system estimation error covariance matrix corresponding to time k, Q represents the covariance matrix of the system working process; Correction process: Through the Kalman filter gain matrix K k Combined with the system state prediction value at time k And the state Z observed by the sensor at time k k Get the optimal estimate of the system state at time k and the optimal estimate of the covariance of the filtering error at time k P k ; Where: R represents the measurement noise covariance matrix, and I is the identity matrix.

5. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 1, characterized in that: The step 3 specifically includes: (1) Selecting windows: Obtain the dynamic window set V according to the speed constraint, acceleration constraint, and safety constraint. m =V n ∩V a ∩V d ; (2) Simulate the predicted trajectory: Sample the velocity in the window and simulate the predicted trajectory; (3) Obtaining dynamic obstacle prediction information: Define the speed window prediction time t, and use step 2 to obtain the optimal estimated values ​​of the position and speed of the dynamic obstacle within the prediction time; (4) Information fusion: The heading angle deviation, minimum distance to the obstacle, braking distance, and speed are calculated through the state at the end of the trajectory. The minimum distance to the obstacle and braking distance are calculated based on the state at the end of the trajectory of the mobile robot and the optimal estimated value of the obstacle after t seconds in (3). The trajectory prediction information of dynamic obstacles is integrated into the DWA algorithm.

6. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 5, characterized in that: The step (1) is specifically: Speed ​​Constraints: V n ={v∈[v min ,v max ],ω∈[ω min ,ω max ]} where v min and v max Indicates the minimum and maximum linear speed of the robot, w min and w max Indicates the minimum and maximum angular velocity of the robot; Acceleration constraint: The set of motion velocities that can be achieved within a dynamic window period is defined as: Among them, v r and ω r are the linear velocity and angular velocity of the robot at the current moment, and are the maximum linear acceleration and maximum angular acceleration, respectively, which are mainly determined by the motor performance; Δt is the sampling time interval; Safety constraints: The speed space that satisfies the robot's motion safety is defined as: Among them, dist(v,ω) is the distance to the nearest obstacle on the current motion trajectory; According to the above three constraints, the intersection of the three speed constraints is taken to obtain the most suitable dynamic window v for the robot. m .

7. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 1, characterized in that: The step 4 specifically includes: When selecting the optimal trajectory, the Kalman filter fusion DWA algorithm takes into account the robot's motion constraints and the influence of dynamic obstacles to ensure that the robot can safely reach the target point; in addition, a direction angle evaluation function is designed to ensure the path quality and avoid falling into the dynamic local optimal trap. The improved evaluation function is as follows: G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)+δ·bearing(v,ω)) Where, heading(v,ω) is the heading angle evaluation subfunction, which is the deviation angle between the predicted trajectory of the mobile robot and the target point. The smaller the deviation angle, the higher the score; dist(v,ω) is the obstacle distance evaluation subfunction, which is the minimum distance between the predicted trajectory and the obstacle at the current speed. The longer the distance, the higher the score; velocity(v,ω) is the speed evaluation subfunction. While ensuring safe obstacle avoidance, the faster the speed, the higher the score; bearing(v,ω) is the direction angle evaluation subfunction, which is the angle difference between the predicted trajectory of the mobile robot and the nearest dynamic obstacle. The larger the angle difference, the higher the score; α, β, γ, and δ are weight coefficients of the evaluation function, and σ represents the normalization of the evaluation function.

8. The dynamic obstacle avoidance method based on fusion Kalman filtering and improved DWA algorithm according to claim 7, characterized in that: The formula of the direction angle evaluation subfunction is Δθ=|θ robot -θ obstacle |, where θ robot is the angle of each predicted trajectory of the mobile robot, θ obstacle It is the current angle of the dynamic obstacle closest to the mobile robot. The specific usage steps are: Analyze and record the heading angles of each obstacle that appears, and calculate the distance between each obstacle and the mobile robot; Find the obstacle closest to the mobile robot and its corresponding heading angle; Calculate the distance change of the obstacle within Δt=0.1s. If the distance change is 0, it is judged as a static obstacle and the evaluation sub-function is not used; if the distance is not 0, it is judged as a dynamic obstacle and the evaluation sub-function is used.

Citation Information

Patent Citations

  • ROS-based mobile robot autonomous navigation method in indoor environment

    CN114510057A

  • Mobile robot obstacle avoidance method based on improved dynamic window method

    CN115586774A

  • Local path planning method, storage medium and equipment

    CN117232511A

  • ROS robot path planning method and device based on improved dynamic window method

    CN118424259A

Cited By

  • Obstacle state prediction method and device, equipment and storage medium

    CN119668253A

  • Path planning method combining small-scale traffic sign recognition and DWA algorithm

    CN119693923A

  • Path Planning Method Combining Small-Scale Traffic Sign Recognition and DWA Algorithm

    CN119693923B

  • Improved dynamic window collision avoidance method, program and equipment for ocean robot to quickly approach target point and storage medium

    CN119717810A

  • Robot path navigation method based on improved DWA algorithm

    CN120063293A