A dynamic obstacle avoidance method for unmanned vehicles based on particle filtering

Abstract

The invention discloses a dynamic obstacle avoidance method for an unmanned vehicle based on particle filtering. The unmanned vehicle is provided with a general control system, and a motion control system, an inertial navigation system, a laser radar module and an IMU module respectively connected to the general control system; the general control system is loaded with a map and an unmanned vehicle operating system ROS control software; the unmanned vehicle determines the speed and position of the unmanned vehicle through the IMU module, scans the surrounding environment of the unmanned vehicle through the laser radar, compares the scanned picture with the map information, and first removes the known obstacles on the map; then the remaining obstacles are sampled by particle filtering, and the movement speed and movement trend of the obstacles relative to the unmanned vehicle are predicted; a DWA algorithm is combined with an obstacle particle filtering method to avoid moving obstacles; when the unmanned vehicle reaches a safe distance from the moving obstacle, the unmanned vehicle continues to move along the global optimal path.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicles, and in particular to a dynamic obstacle avoidance method for unmanned vehicles based on particle filtering. Background Art

[0002] With the rapid development of unmanned vehicle related technologies, people have an increasing demand for unmanned vehicles, especially the positioning technology of unmanned vehicles. The positioning technology of unmanned vehicles usually provides the position (including position and heading angle) of unmanned vehicles in the world coordinate system of the environment. At the same time, the positioning technology of unmanned vehicles is the key technology for unmanned vehicles to achieve autonomous navigation.

[0003] For the global positioning of unmanned vehicles in a preset environment map, the existing method is to use particle filter positioning technology to locate mobile unmanned vehicles. The adaptive Monte Carlo algorithm for particle filter positioning uses probability grid maps, laser radar measurement data and mileage displacement data, and combines the motion model with random noise to realize the motion update of random particles. The adaptive Monte Carlo algorithm for particle filter positioning adjusts the particle weight according to the matching degree between the current observation data and the preset static local environment, realizes observation update, uses adaptive resampling to maintain the number of valid particles, and uses particle clusters with higher weights to output estimated poses.

[0004] However, the particle filter pose estimation of the existing adaptive Monte Carlo positioning algorithm has the following shortcomings: 1. When the local environment in which the unmanned vehicle is currently operating has changed significantly compared with the local area of ​​the preset static map, there is a difference between the current observation data and the preset static local environment. Matching the existing preset static local environment and then adjusting the particle weights will lead to pose estimation errors; 2. When there are obvious errors in the mileage displacement data, the estimated pose will also have obvious errors accordingly. Summary of the invention

[0005] The present invention proposes a particle filtering-based unmanned vehicle dynamic obstacle avoidance method, which can effectively solve the above technical problems.

[0006] The present invention is achieved through the following technical solutions:

[0007] A particle filtering-based unmanned vehicle dynamic obstacle avoidance method, the unmanned vehicle is provided with a general control system, and a motion control system, an inertial navigation system, a laser radar module and an IMU module respectively connected to the general control system; the general control system is loaded with a map and an unmanned vehicle operating system ROS control software; the unmanned vehicle determines the speed and position of the unmanned vehicle through the IMU module, scans the surrounding environment of the unmanned vehicle through the laser radar, compares the scanned image with the map information, and first removes the known obstacles on the map; then the remaining obstacles are sampled by particle filtering, and the movement speed and movement trend of the obstacles relative to the unmanned vehicle are predicted; the DWA algorithm is combined with the obstacle particle filtering method to avoid moving obstacles; when the unmanned vehicle reaches a safe distance from the moving obstacle, it continues to move along the global optimal path.

[0008] Furthermore, the unmanned vehicle samples the remaining obstacles by particle filtering, wherein the laser radar scans around the unmanned vehicle to identify obstacles different from those on the map. After the steps of initialization, weight calculation, resampling, and state transfer, the particles slowly gather at the real position of the moving obstacle, and focus on scattering points on obstacles different from those on the map. When the obstacle deviates from the center position of the scattering point, the scattering range is adjusted, and the position of the next scattering point is predicted accordingly. The formula for obtaining the position of the obstacle is as follows:

[0009]

[0010]

[0011]

[0012] In the formula is the rotation angle of the laser radar at the kth second, is the distance from the unmanned vehicle to the obstacle at the kth second, is the average rotation angle of the obstacle, is the initial rotation angle of the obstacle, is the obstacle termination rotation angle, K is the angle ratio of the obstacle to the laser radar, is the initial rotation angle of the unmanned vehicle, The termination rotation angle of the unmanned vehicle; the direction and amplitude of the scatter point adjustment are determined by comparing the size of the K value.

[0013] Furthermore, the specific operation steps of initialization, weight calculation, resampling, and state transfer are as follows:

[0014] S1. After the system is initialized, the laser radar scatters points around the unmanned vehicle in a random distribution. The particles represent the assumed positions. After the initialization is completed, the results of the scattering are compared with the map loaded in the system to lock the newly appeared obstacles; the laser radar is concentrated to scatter points on the newly appeared obstacles to generate quantitative position hypotheses, and the particle filter estimates the position of the obstacles in space;

[0015] S2. Calculate weights: Generate quantitative particle hypotheses, record the probability of particles based on the location of the unmanned vehicle and the distance to the obstacle, and make the same evaluation for all particles, attaching a larger weight to particles with a higher probability and a smaller weight to particles with a lower probability;

[0016] S3. Resampling: When the obstacle deviates from the center of the scattering point, the scattering point range is adjusted, the old particles are discarded, and new particles are generated. The generated new particles appear in the positions of the old particles with large weights, and a large number of new particles are superimposed at the same position;

[0017] S4. State transfer: The unmanned vehicle and the obstacle continue to move forward, and the particles also move together according to the motion model, knowing the direction and speed range of the unmanned vehicle based on the sensors.

[0018] Furthermore, by scattering points on obstacles that are different from those on the map, it is possible to determine whether the obstacle is a dynamic obstacle. If it is not a dynamic obstacle, it is directly avoided. If it is a dynamic obstacle, the moving speed of the dynamic obstacle is calculated at the same time, and the movement trend of the obstacle is predicted based on the adjustment of the scattering points. The formula for solving the speed of the moving obstacle is as follows:

[0019]

[0020]

[0021] Where L t is the distance from the obstacle to the unmanned vehicle at the current moment, They are the angles of the laser radar 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment. are the distances from the unmanned vehicle to the obstacle 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment, v is the current obstacle moving speed, and L t-1 is the distance from the obstacle to the unmanned vehicle t seconds before the current moment.

[0022] Furthermore, by sampling the remaining obstacles using the particle filter method, the movement speed and movement trend of the obstacles relative to the unmanned vehicle are predicted, so that the movement direction of the unmanned vehicle can be determined. The formula for solving the movement direction of the unmanned vehicle is as follows:

[0023] G(υ,ω)=σ(α·h(υ,ω)+β·d(υ,ω)+γ·v(υ,ω))

[0024] h(υ,ω) is the azimuth evaluation function, d(υ,ω) is the distance between the unmanned vehicle and the nearest obstacle, and v(υ,ω) is the speed corresponding to the trajectory;

[0025] Divide each item by the total of each item, calculate the proportion of each in its own category, then add them up, normalize them, calculate the proportion of their scores in their own category, and then add them up to make the car avoid obstacles and drive towards the target at the set speed.

[0026] Furthermore, when the direction and speed of the unmanned vehicle are determined, the position and speed of the unmanned vehicle can be obtained. According to the position and speed of the unmanned vehicle, obstacles are sampled in a particle filtering manner. When the average score of particles is found to be suddenly reduced, some particles are re-scattered globally, and the probability of increasing particles is evaluated by the IMU module, that is:

[0027] p(z t |z 1:t-1 ,u 1:t ,m)

[0028] And relate it to the average measurement probability. In particle filtering, the approximation of this quantity is easy to obtain based on the importance factor, because the importance weight is a random estimate of this probability, and its average value is:

[0029]

[0030] p is the actual measurement z that can be generated in a certain position t The probability of z 1:t-1 The distance measured by the sensor, u 1:t is the motion information, m is a number between 1 and M and is meaningless, and ω is the measurement probability.

[0031] A motion model of the unmanned vehicle is set in the motion control system. In the motion model, the unmanned vehicle can move forward and rotate. The motion trajectory between two adjacent points is regarded as a straight line. The distance traveled by the unmanned vehicle can be calculated by the following formula. The specific calculation formula is as follows:

[0032] ΔS = ν * Δt;

[0033] In the above formula, ΔS is the distance the unmanned vehicle moves in Δt time, ν is the moving speed of the unmanned vehicle, and Δt is a very small period of time and has no meaning;

[0034] Transformed into the x,y coordinates of the unmanned vehicle:

[0035] Δx=νΔtsinθ;

[0036] Δy=υΔtsinθ;

[0037] In the above formula, Δx is the displacement of the unmanned vehicle relative to the x-axis within Δt, and Δy is the displacement of the unmanned vehicle relative to the y-axis within Δt;

[0038] The next state of the driverless car:

[0039] x=x'+υΔtcosθ;

[0040] y=y'+υΔtsinθ;

[0041] θ=θ+ωΔt;

[0042] In the above formula, x' is the horizontal coordinate of the position of the unmanned vehicle at the previous moment, y' is the vertical coordinate of the position of the unmanned vehicle at the previous moment, θ is the overall moving direction of the unmanned vehicle, and ω is the moving angle of the unmanned vehicle at time Δt.

[0043] Furthermore, the overall control system is based on an STM32 controller, the STM32 controller is connected to a Raspberry Pi platform via a bus, and the Raspberry Pi platform communicates with a host computer via a Wi-Fi wireless network to complete data transmission.

[0044] Furthermore, the inertial navigation device includes an odometer and a gyroscope. The odometer, gyroscope and lidar ranging device are connected to the overall control system via a signal line to provide the overall control system with raw data of the unmanned vehicle's own positioning.

[0045] Beneficial Effects

[0046] The present invention proposes a particle filtering-based unmanned vehicle dynamic obstacle avoidance method, which has the following beneficial effects compared with the traditional prior art:

[0047] (1) This technical solution integrates the data of the inertial navigation system, the lidar module and the IMU module. The unmanned vehicle determines the speed and real-time position of the unmanned vehicle through the IMU module, and scans the surrounding environment of the unmanned vehicle through the lidar to obtain a map of the unmanned vehicle's operating environment. The scanned image is compared with the map information in the original system to remove known obstacles on the map first; the obstacle positioning is achieved through the particle filter algorithm, and the DWA algorithm is combined to control the action of the unmanned vehicle.

[0048] (2) This technical solution can accurately move obstacles through the particle filtering step, identify dynamic obstacles, and take them into account in path planning, effectively solving the problems of low efficiency and long path planning caused by other methods that treat dynamic obstacles as static obstacles, so that the unmanned vehicle can achieve accurate positioning in the entire working environment and effectively complete the task.

[0049] (3) This technical solution uses the motion model of the unmanned vehicle set in the unmanned vehicle motion control system to regard the motion trajectory between two adjacent points as a straight line, thereby calculating the distance traveled by the unmanned vehicle, calculating the mileage and displacement data of the unmanned vehicle and sending it to the main controller for comparison, and verifying the mileage and displacement data of the unmanned vehicle to avoid errors in the mileage and displacement data of the unmanned vehicle during operation, which would lead to obvious errors in the posture. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 It is a schematic diagram of the positioning process based on particle filtering in the present invention.

[0051] Figure 2 It is a schematic diagram of the sampling process based on particle filtering in the present invention. DETAILED DESCRIPTION

[0052] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments.

[0053] Example:

[0054] A particle filtering-based unmanned vehicle dynamic obstacle avoidance method, wherein the unmanned vehicle is provided with a general control system, and a motion control system, an inertial navigation system, a laser radar module and an IMU module respectively connected to the general control system; the general control system is loaded with a map and the unmanned vehicle operating system ROS control software.

[0055] The overall control system is based on the STM32 controller. The STM32 controller is connected to the Raspberry Pi platform through a bus. The Raspberry Pi platform communicates with the host computer through the Wi-Fi wireless network to complete data transmission.

[0056] The inertial navigation equipment includes an odometer and a gyroscope. The odometer, gyroscope and lidar ranging device are connected to the overall control system through signal lines, providing the overall control system with the original data of the unmanned vehicle's own positioning.

[0057] The unmanned vehicle determines the speed and position of the unmanned vehicle through the IMU module, scans the surrounding environment of the unmanned vehicle through the lidar, compares the scanned image with the map information, and first removes the known obstacles on the map; then the remaining obstacles are sampled by particle filtering to generate quantitative particle hypotheses. There is already a static map and the distance from the unmanned vehicle to the obstacle. According to the position of the unmanned vehicle and the distance to the obstacle, the possibility of the particles is recorded, and all particles are evaluated in the same way. A larger weight is attached to particles with a higher probability, and a smaller weight is attached to particles with a lower probability; the movement speed and movement trend of the obstacle relative to the unmanned vehicle are predicted; the DWA algorithm is combined with the obstacle particle filtering method to avoid moving obstacles; when the unmanned vehicle reaches a safe distance from the moving obstacle, it continues to move along the global optimal path.

[0058] The unmanned vehicle samples the remaining obstacles by particle filtering, wherein the laser radar scans around the unmanned vehicle to identify obstacles different from those on the map. After the following steps, the particles gather at the actual position of the moving obstacles.

[0059] S1. After the system is initialized, the laser radar scatters points around the unmanned vehicle in a random distribution. The particles represent the assumed positions. After the initialization is completed, the results of the scattering are compared with the map loaded in the system to lock the newly appeared obstacles; the laser radar is concentrated to scatter points on the newly appeared obstacles to generate quantitative position hypotheses, and the particle filter estimates the position of the obstacles in space;

[0060] S2. Calculate weights: Generate quantitative particle hypotheses, record the probability of particles based on the location of the unmanned vehicle and the distance to the obstacle, and make the same evaluation for all particles, attaching a larger weight to particles with a higher probability and a smaller weight to particles with a lower probability;

[0061] S3. Resampling: When the obstacle deviates from the center of the scattering point, the scattering point range is adjusted, the old particles are discarded, and new particles are generated. The generated new particles appear in the positions of the old particles with large weights, and a large number of new particles are superimposed at the same position;

[0062] S4. State transfer: The unmanned vehicle and the obstacle continue to move forward, and the particles also move together according to the motion model, knowing the direction and speed range of the unmanned vehicle based on the sensors.

[0063] After the above steps of initialization, weight calculation, resampling, and state transfer, the particles slowly gather at the real position of the moving obstacle, and focus on scattering points on obstacles that are different from those on the map. When the obstacle deviates from the center of the scattering point, the scattering range is adjusted, and the position of the next scattering point is predicted accordingly. The formula for obtaining the obstacle position is as follows:

[0064]

[0065]

[0066]

[0067] In the formula is the rotation angle of the laser radar at the kth second, is the distance from the unmanned vehicle to the obstacle at the kth second, is the average rotation angle of the obstacle, is the initial rotation angle of the obstacle, is the obstacle termination rotation angle, K is the angle ratio of the obstacle to the laser radar, is the initial rotation angle of the unmanned vehicle, The termination rotation angle of the unmanned vehicle; the direction and amplitude of the scatter point adjustment are determined by comparing the size of the K value.

[0068] By scattering points on obstacles that are different from those on the map, it is possible to determine whether the obstacle is a dynamic obstacle. If it is not a dynamic obstacle, it is directly avoided. If it is a dynamic obstacle, the moving speed of the dynamic obstacle is calculated at the same time, and the movement trend of the obstacle is predicted based on the adjustment of the scattering points. The formula for solving the speed of the moving obstacle is as follows:

[0069]

[0070]

[0071] Where L t is the distance from the obstacle to the unmanned vehicle at the current moment, They are the angles of the laser radar 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment. are the distances from the unmanned vehicle to the obstacle 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment, v is the current obstacle moving speed, and L is the distance from the unmanned vehicle to the obstacle 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment, respectively. t-1 is the distance from the obstacle to the unmanned vehicle t seconds before the current moment.

[0072] By sampling the remaining obstacles using particle filtering, the speed and trend of the obstacles relative to the unmanned vehicle are predicted, so that the direction of movement of the unmanned vehicle can be determined. The formula for solving the direction of movement of the unmanned vehicle is as follows:

[0073] G(υ,ω)=σ(α·h(υ,ω)+β·d(υ,ω)+γ·v(υ,ω))

[0074] h(υ,ω) is the azimuth evaluation function, d(υ,ω) is the distance between the unmanned vehicle and the nearest obstacle, and v(υ,ω) is the speed corresponding to the trajectory;

[0075] Divide each item by the total of each item, calculate the proportion of each in its own category, then add them up, normalize them, calculate the proportion of their scores in their own category, and then add them up to make the car avoid obstacles and drive towards the target at the set speed.

[0076] After determining the direction and speed of the unmanned vehicle, the vehicle's own position and running speed can be obtained. According to the vehicle's own position and running speed, obstacles are sampled in a particle filtering manner. When the average score of particles is found to be suddenly reduced, some particles are re-scattered globally, and the probability of increasing particles is evaluated by the IMU module, that is:

[0077] p(z t |z 1:t-1 ,u 1:t ,m)

[0078] And relate it to the average measurement probability. In particle filtering, the approximation of this quantity is easy to obtain based on the importance factor, because the importance weight is a random estimate of this probability, and its average value is:

[0079]

[0080] p is the actual measurement z that can be generated in a certain position t The probability of z 1:t-1 The distance measured by the sensor, u 1:t is the motion information, m is a number between 1 and M and is meaningless, and ω is the measurement probability.

[0081] A motion model of the unmanned vehicle is set in the motion control system. In the motion model, the unmanned vehicle can move forward and rotate. The motion trajectory between two adjacent points is regarded as a straight line. The distance traveled by the unmanned vehicle can be calculated by the following formula. The specific calculation formula is as follows:

[0082] ΔS = ν * Δt;

[0083] In the above formula, ΔS is the distance the unmanned vehicle moves in Δt time, ν is the moving speed of the unmanned vehicle, and Δt is a very small period of time and has no meaning;

[0084] Transformed into the x,y coordinates of the unmanned vehicle:

[0085] Δx=νΔtsinθ;

[0086] Δy=υΔtsinθ;

[0087] In the above formula, Δx is the displacement of the unmanned vehicle relative to the x-axis within Δt, and Δy is the displacement of the unmanned vehicle relative to the y-axis within Δt;

[0088] The next state of the driverless car:

[0089] x=x'+υΔtcosθ;

[0090] y=y'+υΔtsinθ;

[0091] θ=θ+ωΔt;

[0092] In the above formula, x' is the horizontal coordinate of the position of the unmanned vehicle at the previous moment, y' is the vertical coordinate of the position of the unmanned vehicle at the previous moment, θ is the overall moving direction of the unmanned vehicle, and ω is the moving angle of the unmanned vehicle at time Δt.

[0093] After obtaining the distance traveled by the unmanned vehicle through the above formula, the calculated distance is sent to the overall control system for verification.

Claims

1. A dynamic obstacle avoidance method for an unmanned vehicle based on particle filtering, characterized in that: The unmanned vehicle is provided with a general control system, and a motion control system, an inertial navigation system, a laser radar module and an IMU module respectively connected to the general control system; the general control system is loaded with a map and the unmanned vehicle operating system ROS control software; the unmanned vehicle determines the speed and position of the unmanned vehicle through the IMU module, scans the surrounding environment of the unmanned vehicle through the laser radar, compares the scanned image with the map information, and first removes the known obstacles on the map; then samples the remaining obstacles by particle filtering to predict the movement speed and movement trend of the obstacles relative to the unmanned vehicle; adopts the DWA algorithm combined with the obstacle particle filtering method to avoid moving obstacles; when the unmanned vehicle reaches a safe distance from the moving obstacle, it continues to move along the global optimal path; The unmanned vehicle samples the remaining obstacles by particle filtering. The laser radar scans around the unmanned vehicle to identify obstacles different from those on the map. After initialization, weight calculation, resampling, and state transfer, the particles slowly gather at the real position of the moving obstacle, and focus on scattering points on obstacles different from those on the map. When the obstacle deviates from the center of the scattering point, the scattering range is adjusted, and the position of the next scattering point is predicted accordingly. The formula for obtaining the obstacle position is as follows: In the formula is the rotation angle of the laser radar at the kth second, is the distance from the unmanned vehicle to the obstacle at the kth second, is the average rotation angle of the obstacle, is the initial rotation angle of the obstacle, is the obstacle termination rotation angle, K is the angle ratio of the obstacle to the laser radar, is the initial rotation angle of the unmanned vehicle, The end rotation angle of the unmanned vehicle; the direction and amplitude of the point adjustment are determined by comparing the size of the K value.

2. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 1, characterized in that: The specific steps of initialization, weight calculation, resampling, and state transfer are as follows: S1. After initialization, the laser radar scatters points around the unmanned vehicle in a random distribution. The particles represent the assumed positions. After initialization is completed, the results of the scattering are compared with the map loaded in the system to lock the newly appeared obstacles; Sprinkle the laser radar points towards the newly appeared obstacles to generate quantitative position assumptions, and record the particle filter to estimate the obstacle position in space; S2. Calculate weights: Generate quantitative particle hypotheses, record the probability of particles based on the location of the unmanned vehicle and the distance to the obstacle, and make the same evaluation for all particles, attaching a larger weight to particles with a higher probability and a smaller weight to particles with a lower probability; S3. Resampling: When the obstacle deviates from the center of the scattering point, the scattering point range is adjusted, the old particles are discarded, and new particles are generated. The generated new particles appear in the positions of the old particles with large weights, and a large number of new particles are superimposed at the same position; S4. State transfer: The unmanned vehicle and the obstacle continue to move forward, and the particles also move together according to the motion model, knowing the direction and speed range of the unmanned vehicle based on the sensors.

3. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 1, characterized in that: By scattering points on obstacles that are different from those on the map, it is possible to determine whether the obstacle is a dynamic obstacle. If it is not a dynamic obstacle, it is directly avoided. If it is a dynamic obstacle, the moving speed of the dynamic obstacle is calculated at the same time, and the movement trend of the obstacle is predicted based on the adjustment of the scattering points. The formula for solving the speed of the moving obstacle is as follows: Where L t is the distance from the obstacle to the unmanned vehicle at the current moment, They are the angles of the laser radar 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment. are the distances from the unmanned vehicle to the obstacle 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment, v is the current obstacle moving speed, and L is the distance from the unmanned vehicle to the obstacle 1 second, 2 seconds, 3 seconds, 4 seconds, and 5 seconds before the current moment, respectively. t-1 is the distance from the obstacle to the unmanned vehicle t seconds before the current moment.

4. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 1, characterized in that: By sampling the remaining obstacles using the particle filter method, the movement speed and movement trend of the obstacles relative to the unmanned vehicle are predicted, so that the movement direction of the unmanned vehicle can be determined. The formula for solving the movement direction of the unmanned vehicle is as follows: G(υ,ω)=σ(α·h(υ,ω)+β·d(υ,ω)+γ·v(υ,ω)) h(υ,ω) is the azimuth evaluation function, d(υ,ω) is the distance between the unmanned vehicle and the nearest obstacle, and v(υ,ω) is the speed corresponding to the trajectory; Divide each item by the total of each item, calculate the proportion of each in its own category, then add them up, normalize them, calculate the proportion of their scores in their own category, and then add them up to make the car avoid obstacles and drive towards the target at the set speed.

5. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 4, characterized in that: In the case of determining the direction and speed of the unmanned vehicle, the position and speed of the unmanned vehicle can be obtained. According to the position and speed of the unmanned vehicle, obstacles are sampled in a particle filtering manner. When the average score of particles is found to be suddenly reduced, some particles are re-scattered globally, and the probability of increasing particles is evaluated by the IMU module, that is: p(z t |z 1:t-1 ,u 1:t ,m) And relate it to the average measurement probability. In particle filtering, the approximation of this quantity is easy to obtain based on the importance factor, because the importance weight is a random estimate of this probability, and its average value is: p is the actual measurement z that can be generated in a certain position t The probability of z 1:t-1 The distance measured by the sensor, u 1:t is the motion information, m is a number between 1 and M and has no specific meaning, and ω is the measurement probability.

6. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 1, characterized in that: The motion control system is provided with a motion model of an unmanned vehicle. The unmanned vehicle can move forward and rotate. The motion trajectory between two adjacent points is regarded as a straight line. The distance traveled by the unmanned vehicle is: ΔS = ν * Δt; In the above formula, ΔS is the distance the unmanned vehicle moves in Δt time, ν is the moving speed of the unmanned vehicle, and Δt is a very small period of time and has no specific meaning; Transformed into the x,y coordinates of the unmanned vehicle: Δx=νΔtsinθ; Δy=υΔtsinθ; In the above formula, Δx is the displacement of the unmanned vehicle relative to the x-axis within Δt, and Δy is the displacement of the unmanned vehicle relative to the y-axis within Δt; The next state of the driverless car: x=x'+υΔtcosθ; y=y'+υΔtsinθ; θ=θ+ωΔt; In the above formula, x' is the horizontal coordinate of the position of the unmanned vehicle at the previous moment, y' is the vertical coordinate of the position of the unmanned vehicle at the previous moment, θ is the overall moving direction of the unmanned vehicle, and ω is the moving angle of the unmanned vehicle at time Δt.

7. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 1, characterized in that: The overall control system is based on an STM32 controller, which is connected to a Raspberry Pi platform via a bus, and the Raspberry Pi platform communicates with a host computer via a Wi-Fi wireless network to complete data transmission.

8. The unmanned vehicle dynamic obstacle avoidance method based on particle filtering according to claim 1, characterized in that: The inertial navigation system includes an odometer and a gyroscope. The odometer, gyroscope and laser radar distance measuring device are connected to the overall control system through a signal line to provide the overall control system with raw data of the unmanned vehicle's own positioning.

Images