A patrol car path planning method based on multi-sensor tracking and obstacle avoidance

Abstract

A patrol car path planning algorithm based on multi-sensor tracking and obstacle avoidance, the method comprises the following steps: step 1, using multi-sensors to collect patrol car operation data; step 2, simulating the environmental noise when the patrol car is driving; step 3, designing the patrol car tracking detection system; step 4, designing the patrol car obstacle avoidance auxiliary system; step 5, co-adjusting the patrol car tracking detection system and the patrol car obstacle avoidance auxiliary system; step 6, enabling the patrol car abnormal processing interrupt. The present invention simulates the harsh environment during driving, realizes the patrol car tracking function under the interference of the noise environment, enhances the stability, reliability and robustness of the patrol car tracking, and realizes the patrol car's obstacle avoidance function according to the millimeter wave radar data.

Description

Technical Field

[0001] The invention relates to the field of patrol car tracking, and in particular to a patrol car path planning method based on multi-sensor tracking and obstacle avoidance. Background Art

[0002] As life enters the information age and the intelligent manufacturing industry emerges, with the rapid development of basic science and information technology, humanoid robots have appeared in the fields of service industry, traditional manufacturing industry and agriculture for production. They can replace workers to complete boring and repetitive operations in industrial production, and greatly liberate manpower in agricultural industrialization. In service work, robots are widely used in sanitation, medical surgery, heavy object handling and other tasks, and complete autonomous care and monitoring in the family. Corresponding to the traditional reliance on manpower to complete tasks, it provides more convenient, efficient and safe services for our daily life. However, when the application site environment is complex, such as closed and narrow space, many obstacles, and no one on-site cooperation, special equipment is needed to complete the corresponding patrol tasks. As its scope of adaptation continues to expand, its development tends to be automated and intelligent, which makes mobile robots a research hotspot for current industrial applications.

[0003] Accurate tracking control is the prerequisite for mobile robots to complete various tasks. It requires the robot to follow the set ideal trajectory and finally reach the specified target point. However, due to the difficulty in determining the precise mathematical model of the mobile robot and the uncertainty of external interference, errors will occur in the actual trajectory tracking of the mobile robot. The present invention realizes the trajectory detection, tracking and obstacle avoidance functions under interference through multi-sensors and YOLOv5 algorithm. Summary of the invention

[0004] To solve the above problems, the present invention adds salt and pepper noise to simulate the harsh environment of vehicle driving on the basis of the road surface image collected by the real-time image acquisition system to improve the stability and robustness of the model. First, the lane lines are detected and segmented by the YOLOv5 network, and the lane lines of the patrol car are tracked by the algorithm. Finally, the patrol car's obstacle avoidance function is realized according to the data collected by the millimeter wave radar. The present invention provides a patrol car path planning method based on multi-sensor tracking and obstacle avoidance, the specific steps are as follows, and it is characterized by:

[0005] Step 1: Use multiple sensors to collect patrol car operation data: Use a high-definition bird's-eye view fisheye camera to collect road driving images, and use millimeter-wave radar to accurately obtain physical environment information around the car, including the relative distance, relative speed, angle, and movement direction between the patrol car and other objects;

[0006] Step 2, simulating the environmental noise when the patrol car is driving: adding 15% to 25% salt and pepper noise to the collected road driving image, and adding Gaussian white noise to the millimeter wave radar data to simulate the interference of the collected signal in the interference environment;

[0007] Step 3, design a patrol car tracking detection system: train the YOLOV5 target detection model based on the road driving image with environmental noise added, detect the lane lines of the patrol car through the YOLOV5 network, and track the lane lines;

[0008] Step 4, design the patrol car obstacle avoidance assistance system: determine the patrol car's safe speed and safe distance based on the relative distance, relative speed, angle, and movement direction data between the patrol car and other objects obtained by the millimeter-wave radar;

[0009] Step 5: Jointly debug the patrol car tracking detection system and the patrol car obstacle avoidance assistance system to determine the priority of the obstacle avoidance assistance system and the tracking detection system;

[0010] Step 6, turn on the patrol car exception processing interrupt. When an exception occurs within the safe distance, the system starts the interrupt processing mechanism. The log module records the abnormal situation of the vehicle terminal and transmits the abnormal data to the server.

[0011] Furthermore, the process of simulating the ambient noise of the patrol car in step 2 can be expressed as:

[0012] The salt and pepper noise function added to the road driving image is as follows:

[0013]

[0014] In the formula, i max and i min are the maximum and minimum values ​​of the pixels of the collected image data, p is the probability of noise, which is 20% to 30%, and i xy is the actual value of the pixel point (x, y) of the road driving image, and f(x, y) is the image pixel value after adding salt and pepper noise;

[0015] The model for adding Gaussian white noise to millimeter-wave radar data can be expressed as:

[0016] G s =G n +G

[0017] In the formula, G is the original voltage data, Gn is the Gaussian white noise data, Gs represents the data collected under the simulated noise environment, and the signal-to-noise ratio range of the signal is controlled within 25dB.

[0018] Furthermore, the process of designing the patrol car tracking detection system in step 3 can be expressed as:

[0019] Step 3.1, establish the YOLOV5 target detection algorithm, detect the lane lines of the patrol car, and divide the detected image into lane lines and other non-road objects. In the feature extraction network, YOLOV5 uses the Focus CSP2 structure to extract image features; the Neck structure uses FPN+PAN to enhance the ability of network feature fusion. The loss function at the output end is:

[0020]

[0021] Where IoU is the intersection over union loss, A c is the minimum circumscribed rectangular area of ​​the network detection box and the annotation, U is the union area of ​​the detection box and the annotation, and the weighted DIOU_NMS algorithm is selected to optimize the problem of detecting overlapping targets. The following formula is used to determine whether the detection anchor box needs to be deleted:

[0022]

[0023] In the formula, s i is the score of the corresponding predicted box, M and Bi represent two different boxes, RDIoU is the distance between the center points of the two boxes, and ε represents the non-maximum suppression threshold; after the training is completed, the trained model is pruned and quantized to obtain the patrol car road image lane line YOLOV5 detection model;

[0024] Step 3.2: Detect lane lines through YOLOV5 and establish a lane line model. The center of the lane line image detected by the YOLOV5 model is recorded as the origin, and the horizontal axis and vertical axis are set as the x-axis and y-axis respectively. Fit the lane line function through a linear function:

[0025] y 1 =k 1 x 1 +b 1

[0026] In the formula, x 1 is the horizontal coordinate of the lane line, y 1 is the ordinate of the lane line, k 1 is the slope of the lane line fitting function, b 1 is the intercept of the lane line fitting function;

[0027] Step 3.3, calculate the deviation angle of the patrol car according to the fitted lane line, take the vertical axis set in step 3.2 as the driving direction of the patrol car, calculate the deviation angle between the vertical axis and the driving direction through inverse trigonometric functions, and correct the yaw angle to achieve the patrol car tracking function.

[0028] Furthermore, the process of designing the patrol car obstacle avoidance assistance system in step 4 can be expressed as:

[0029] The patrol car's safe speed and safe distance are determined based on the relative distance, relative speed, angle, and movement direction data between the patrol car and other objects obtained by the millimeter-wave radar. When the patrol car detects an object in its forward direction, the relationship between its relative distance and relative speed is:

[0030] l≥v

[0031] Wherein, the unit of l is m, and the unit of v is km / h.

[0032] The invention discloses a patrol car path planning method based on multi-sensor tracking and obstacle avoidance, and has the following beneficial effects:

[0033] 1. The present invention simulates the harsh environment of patrol cars when driving, and realizes the tracking and obstacle avoidance functions of patrol cars under the interference of the noise environment, thereby enhancing the stability, reliability and robustness of patrol car tracking;

[0034] 2. The present invention uses the YOLOv5 algorithm to detect the edge line of the lane line, which can accurately detect the lane line and provide important lane line data for the tracking model;

[0035] 3. The present invention provides an important technical means for patrol multi-sensor tracking and obstacle avoidance. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0037] Figure 2 This is the deviation angle model diagram of the present invention DETAILED DESCRIPTION

[0038] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0039] The present invention proposes a patrol car path planning method based on multi-sensor tracking and obstacle avoidance, aiming to improve the patrol car tracking accuracy. At the same time, in order to improve the stability and accuracy of the patrol car tracking solution, salt and pepper noise is added to the collected image to simulate the harsh environment during driving, and the patrol car's obstacle avoidance function is realized according to the millimeter wave radar data. Figure 1 The steps of the present invention are described in detail below in conjunction with the flow chart.

[0040] Step 1: Use multiple sensors to collect patrol car operation data: Use a high-definition bird's-eye view fisheye camera to collect road driving images, and use millimeter-wave radar to accurately obtain physical environment information around the car, including the relative distance, relative speed, angle, and movement direction between the patrol car and other objects;

[0041] Step 2, simulating the environmental noise when the patrol car is driving: adding 15% to 25% salt and pepper noise to the collected road driving image, and adding Gaussian white noise to the millimeter wave radar data to simulate the interference of the collected signal in the interference environment;

[0042] The process of simulating the ambient noise of the patrol car in step 2 can be expressed as:

[0043] The salt and pepper noise function added to the road driving image is as follows:

[0044]

[0045] In the formula, i max and i min are the maximum and minimum values ​​of the pixels of the collected image data, p is the probability of noise, which is 20% to 30%, and i xy is the actual value of the pixel point (x, y) of the road driving image, and f(x, y) is the image pixel value after adding salt and pepper noise;

[0046] The model for adding Gaussian white noise to millimeter-wave radar data can be expressed as:

[0047] G s =G n +G

[0048] In the formula, G is the original voltage data, Gn is the Gaussian white noise data, Gs represents the data collected under the simulated noise environment, and the signal-to-noise ratio range of the signal is controlled within 25dB.

[0049] Step 3, design a patrol car tracking detection system: train the YOLOV5 target detection model based on the road driving image with environmental noise added, detect the lane lines of the patrol car through the YOLOV5 network, and track the lane lines;

[0050] The process of designing the patrol car tracking detection system in step 3 can be expressed as:

[0051] Step 3.1, establish the YOLOV5 target detection algorithm, detect the lane lines of the patrol car, and divide the detected image into lane lines and other non-road objects. In the feature extraction network, YOLOV5 uses the Focus CSP2 structure to extract image features; the Neck structure uses FPN+PAN to enhance the ability of network feature fusion. The loss function at the output end is:

[0052]

[0053] Where IoU is the intersection over union loss, A cis the minimum circumscribed rectangular area of ​​the network detection box and the annotation, U is the union area of ​​the detection box and the annotation, and the weighted DIOU_NMS algorithm is selected to optimize the problem of detecting overlapping targets. The following formula is used to determine whether the detection anchor box needs to be deleted:

[0054]

[0055] In the formula, s i is the score of the corresponding predicted box, M and Bi represent two different boxes, RDIoU is the distance between the center points of the two boxes, and ε represents the non-maximum suppression threshold; after the training is completed, the trained model is pruned and quantized to obtain the patrol car road image lane line YOLOV5 detection model;

[0056] Step 3.2: Detect lane lines through YOLOV5 and establish a lane line model. The center of the lane line image detected by the YOLOV5 model is recorded as the origin, and the horizontal axis and vertical axis are set as the x-axis and y-axis respectively. Fit the lane line function through a linear function:

[0057] y 1 =k 1 x 1 +b 1

[0058] In the formula, x 1 is the horizontal coordinate of the lane line, y 1 is the ordinate of the lane line, k 1 is the slope of the lane line fitting function, b 1 is the intercept of the lane line fitting function;

[0059] Step 3.3, calculate the deviation angle of the patrol car according to the fitted lane line, take the vertical axis set in step 3.2 as the driving direction of the patrol car, and calculate the deviation angle between the vertical axis and the driving direction through inverse trigonometric functions, and correct the yaw angle. The deviation angle model diagram is shown in Figure 2 As shown, the patrol car tracking function is achieved.

[0060] Step 4, design the patrol car obstacle avoidance assistance system: determine the patrol car's safe speed and safe distance based on the relative distance, relative speed, angle, and movement direction data between the patrol car and other objects obtained by the millimeter-wave radar;

[0061] The process of designing the patrol car obstacle avoidance assistance system in step 4 can be expressed as:

[0062] The patrol car's safe speed and safe distance are determined based on the relative distance, relative speed, angle, and movement direction data between the patrol car and other objects obtained by the millimeter-wave radar. When the patrol car detects an object in its forward direction, the relationship between its relative distance and relative speed is:

[0063] l≥v

[0064] Wherein, the unit of l is m, and the unit of v is km / h.

[0065] Step 5: Jointly debug the patrol car tracking detection system and the patrol car obstacle avoidance assistance system to determine the priority of the obstacle avoidance assistance system and the tracking detection system;

[0066] Step 6, turn on the patrol car exception processing interrupt. When an exception occurs within the safe distance, the system starts the interrupt processing mechanism. The log module records the abnormal situation of the vehicle terminal and transmits the abnormal data to the server.

[0067] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A patrol car path planning method based on multi-sensor tracking and obstacle avoidance, the specific steps are as follows, characterized in that: Step 1: Use multiple sensors to collect patrol car operation data: Use a high-definition bird's-eye view fisheye camera to collect road driving images, and use millimeter-wave radar to accurately obtain physical environment information around the car, including the relative distance, relative speed, angle, and movement direction between the patrol car and other objects; Step 2, simulating the environmental noise when the patrol car is driving: adding 15% to 25% salt and pepper noise to the collected road driving image, and adding Gaussian white noise to the millimeter wave radar data to simulate the interference of the collected signal in the interference environment; The process of simulating the ambient noise of the patrol car in step 2 can be expressed as: The salt and pepper noise function added to the road driving image is as follows: In the formula, i max and i min are the maximum and minimum values ​​of the pixels of the collected image data, p is the probability of noise, which is 20% to 30%, and i xy is the actual value of the pixel point (x, y) of the road driving image, and f(x, y) is the image pixel value after adding salt and pepper noise; The model for adding Gaussian white noise to millimeter-wave radar data can be expressed as: G s =G n +G In the formula, G is the original voltage data, Gn is the Gaussian white noise data, Gs represents the data collected in the simulated noise environment, and the signal-to-noise ratio range of the signal is controlled within 25dB; Step 3, design a patrol car tracking detection system: train the YOLOV5 target detection model based on the road driving image with environmental noise added, detect the lane lines of the patrol car through the YOLOV5 network, and track the lane lines; The process of designing the patrol car tracking detection system in step 3 can be expressed as: Step 3.1, establish the YOLOV5 target detection algorithm, detect the lane lines of the patrol car, and divide the detected image into lane lines and other non-road objects. In the feature extraction network, YOLOV5 uses the Focus CSP2 structure to extract image features; the Neck structure uses FPN+PAN to enhance the ability of network feature fusion. The loss function at the output end is: Where IoU is the intersection over union loss, A c is the minimum circumscribed rectangular area of ​​the network detection box and the annotation, U is the union area of ​​the detection box and the annotation, and the weighted DIOU_NMS algorithm is selected to optimize the problem of detecting overlapping targets. The following formula is used to determine whether the detection anchor box needs to be deleted: In the formula, s i is the score of the corresponding predicted box, M and Bi represent two different boxes, RDIoU is the distance between the center points of the two boxes, and ε represents the non-maximum suppression threshold; After the training is completed, the trained model is pruned and quantized to obtain the patrol car road image lane line YOLOV5 detection model; Step 3.2: Detect lane lines through YOLOV5 and establish a lane line model. The center of the lane line image detected by the YOLOV5 model is recorded as the origin, and the horizontal axis and vertical axis are set as the x-axis and y-axis respectively. Fit the lane line function through a linear function: y1=k1x1+b1 Where x1 is the horizontal coordinate of the lane line, y1 is the vertical coordinate of the lane line, k1 is the slope of the lane line fitting function, and b1 is the intercept of the lane line fitting function; Step 3.3, calculate the deviation angle of the patrol car according to the fitted lane line, take the vertical axis set in step 3.2 as the driving direction of the patrol car, and calculate the deviation angle between the vertical axis and the driving direction through inverse trigonometric functions, and correct the yaw angle, so as to achieve the function of patrol car tracking; Step 4, design the patrol car obstacle avoidance assistance system: determine the patrol car's safe speed and safe distance based on the relative distance, relative speed, angle, and movement direction data between the patrol car and other objects obtained by the millimeter-wave radar; The process of designing the patrol car obstacle avoidance assistance system in step 4 can be expressed as: The patrol car's safe speed and safe distance are determined based on the relative distance, relative speed, angle, and movement direction data between the patrol car and other objects obtained by the millimeter-wave radar. When the patrol car detects an object in its forward direction, the relationship between its relative distance and relative speed is: l≥v Where, the unit of l is m, and the unit of v is km / h; Step 5: Jointly debug the patrol car tracking detection system and the patrol car obstacle avoidance assistance system to determine the priority of the obstacle avoidance assistance system and the tracking detection system; Step 6, turn on the patrol car exception processing interrupt. When an exception occurs within the safe distance, the system starts the interrupt processing mechanism. The log module records the abnormal situation of the vehicle terminal and transmits the abnormal data to the server.

Images