Navigation and vehicle searching method for automatic inspection robot in outdoor parking lot environment

Abstract

The invention discloses a navigation and vehicle searching method of an automatic inspection robot in an outdoor parking lot environment. The method comprises the following steps of: 1, performing SLAM mapping on a parking lot environment by using an NDT algorithm; 2, in the SLAM mapping process, GNSS inspection track points are recorded, and sampling storage is carried out; 3, performing global fusion positioning and local fusion positioning; step 4, based on a combined positioning result of global fusion positioning and local fusion positioning, realizing navigation of the outdoor mobile robot by using a global pure tracking algorithm; step 5, carrying out clustering identification on the obstacles by using a laser radar at the top of the robot; 6, performing local path planning on the basis of known combined positioning, global path and obstacle; step 7, in the robot inspection process, identifying a license plate by using an image identification algorithm, so that a subsequent user can conveniently and autonomously find a vehicle based on the license plate; and step 8, code scanning and vehicle searching based on the WeChat applet. The method has the characteristics of wide application prospect, high stability and high practicability.

Description

Technical Field

[0001] The present invention relates to the technical fields of autonomous driving, multi-sensor fusion positioning and image recognition, and in particular to a navigation and vehicle finding method for an automatic inspection robot in an outdoor parking environment. Background Art

[0002] Traditional manual parking management and vehicle search are inefficient, and new solutions are urgently needed to improve parking lot management and operational efficiency.

[0003] Given the rapid development of sensor technology, autonomous driving, artificial intelligence and other technologies, automated and intelligent systems have been realized. These technologies enable outdoor inspection robots to work efficiently in complex environments.

[0004] Existing outdoor robot inspection solutions, such as patent technologies with publication numbers CN118518124A and CN118362122A, have the following shortcomings:

[0005] (1) The existing positioning input sources are mainly composed of the Global Navigation Satellite System (GNSS) and the Inertial Navigation System (INS). However, GNSS signals are easily affected by the environment, and the positioning performance of robots based on the GNSS / INS combination will drop rapidly.

[0006] (2) In parking lots, due to the large number of vehicles and pedestrians, it is crucial to ensure that the robot can effectively avoid obstacles during inspection. This is not only to prevent potential safety hazards, but also to avoid traffic obstruction.

[0007] (3) During the inspection process, how to accurately identify all parked vehicles and send the location information to users in real time so that users can find their vehicles independently and efficiently. Summary of the invention

[0008] In order to overcome the defects of the above-mentioned prior art, the present invention proposes a navigation and vehicle-finding method for an automatic patrol robot in an outdoor parking environment. The method can effectively solve the problem of decreased positioning accuracy of the robot in a complex environment, and can avoid obstacles while effectively completing the patrol task, thereby greatly increasing the safety of the system.

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

[0010] A navigation and vehicle-finding method for an automatic inspection robot in an outdoor parking environment comprises the following steps;

[0011] Step 1: Control the vehicle remotely according to the pre-set inspection route and use the NDT algorithm to perform SLAM mapping of the parking lot environment;

[0012] Step 2: Record GNSS inspection track points during SLAM mapping, and sample and save the recorded inspection track points according to the preset distance threshold;

[0013] Step 3: When GNSS has a signal, use the GNSS position information, IMU attitude information, and wheel odometer as the input of the Extended State Kalman Filter (ESKF) for global fusion positioning, and use the combined positioning result as the initial pose transformation (R, t) of the radar odometer to eliminate the accumulated error of NDT as the radar odometer;

[0014] When GNSS is blocked and has no signal, the radar odometer is used to replace the GNSS position information, and the radar odometer, IMU attitude information, and wheel odometer are used to achieve local fusion positioning;

[0015] Step 4: Based on the combined positioning results of global fusion positioning and local fusion positioning, the global pure tracking algorithm is used to realize the navigation of the outdoor mobile robot;

[0016] Step 5: Use the laser radar on the top of the robot to cluster and identify obstacles;

[0017] Step 6: Based on the known combined positioning, global path, and obstacles, perform local path planning;

[0018] Step 7: During the robot inspection process, the image recognition algorithm is used to identify the license plate, so that subsequent users can find the car based on the license plate;

[0019] Step 8: Scan the QR code to find the car based on the WeChat applet.

[0020] The formula of the NDT registration algorithm in step 1 is:

[0021]

[0022] The above formula is used to obtain the robot's position in the map (R * ,t * ), which makes the sum of the distances between the target point cloud and all paired points of the source point cloud f(R * ,t * ) is the smallest, where f(R * ,t * ) is the sum of the distances of all paired points; (R * ,t * ) is the NDT estimated robot's position in the map; R *is the rotation of the robot relative to the starting point of the map; t * is the translation of the robot relative to the starting point of the map; MIN is to minimize the sum of the distances of all paired points; N P is the number of paired points, p i With q i is a pair of matching points between the target point cloud and the source point cloud; || 2 is the Euclidean distance between paired points.

[0023] The step 3 is specifically as follows:

[0024] The ESKF fusion process between GNSS position information, IMU attitude information, wheel odometer, and laser odometer is divided into the following five steps:

[0025] 3.1) Predict the current vehicle state based on the optimal estimated state at the previous moment Predict the current state

[0026]

[0027] in is the state transfer matrix, which indicates the current state is inferred from the previous state; is the control matrix, which represents the control quantity u t-1 the impact on the current state, such as acceleration; The ^ in the value indicates that it is an estimate, not the true value. The - in the figure indicates that it is not the best estimate;

[0028] 3.2) Based on the optimal prediction state at the previous moment The covariance matrix P of the noise (error) t-1 , calculate the current prediction state The noise (error) covariance matrix P t - ;

[0029] P t - =FP t-1 F T +Q

[0030] Where P t-1 represents the noise (error) covariance moment of the previous moment; Q represents the noise (error) covariance matrix;

[0031] 3.3) Calculate the Kalman coefficient;

[0032]

[0033] Where K t represents the Kalman coefficient; H represents the predicted state To the sensor observation state Z t The dimensional transformation relationship between them; R represents the noise (error) covariance matrix of the observed state;

[0034] 3.4) Optimize prediction status Get the best prediction state As the final output of the filter, it also serves as the input for the next iteration;

[0035]

[0036] in Represents the actual sensor observation state Z t Sensor observation state and prediction The residual between

[0037] 3.5) Update the optimal prediction state The noise (error) covariance matrix is ​​used in the next iteration;

[0038] P t =(IK t H)P t -

[0039] Where P t The optimal prediction state The noise (error) covariance matrix of ; I is the identity matrix.

[0040] The step 4 is specifically as follows:

[0041] The specific tracking process is divided into the following four steps:

[0042] 4.1) In order for the rear wheel of the vehicle to track the global trajectory Path to reach the preview point P, the sine theorem relationship needs to be satisfied in the triangle AOP. The vehicle can track the preview point to achieve tracking.

[0043]

[0044] Simplified

[0045]

[0046] Where R represents the turning radius; l d Indicates preview distance;

[0047] 4.2) In the Ackermann Turn model, the rear wheel performs circular motion and the turning radius R is calculated in the triangle AOB;

[0048]

[0049] where δ f represents the front wheel steering angle; L represents the vehicle distance axis; A represents the center of the vehicle's rear wheels; B represents the center of the vehicle's front wheels; O represents the vehicle's steering center; P represents the vehicle's preview point,

[0050] 4.3) Combine 4.1) and 4.2) to get the steering angle of the pure tracking algorithm;

[0051]

[0052] 4.4) Calculate the angle between the preview point and the vehicle body;

[0053]

[0054] Where a represents the angle between the preview point and the vehicle body; β represents the angle between the preview point and the X-axis; represents the vehicle heading angle; (x1 y1) represents the center coordinate of the vehicle's rear wheel A; (x2 y2) represents the center coordinate of the vehicle's rear wheel B; (x3 y3) represents the coordinate of the preview point P.

[0055] The step 5 is divided into the following two steps:

[0056] 5.1) Downsampling of point cloud voxels, point cloud height clipping, and clipping of point clouds above the top of the vehicle; these two methods greatly reduce the number of points involved in subsequent calculations and reduce the amount of calculation;

[0057] 5.2) Separate the ground point cloud from the non-ground point cloud, and use the Euclidean clustering algorithm to cluster the obstacles of the non-ground point cloud. The principle of the Euclidean clustering algorithm is that in the point cloud data, the distance between two points in the point cloud cluster of the same object is less than a certain value, while the distance between the point cloud clusters of different objects is greater than a certain value. Points with distances less than the set distance threshold are merged into one category, thereby completing the clustering process.

[0058] The local path planning in step 6 is divided into the following four steps:

[0059] 6.1) Extract the local path from the global path based on the vehicle’s current position and maximum planned distance;

[0060]

[0061] Where S is the set of points contained in the local path; (x i ,y i ) is the set of points contained in the global path; (x c ,y c ) is the current position of the vehicle; D is the threshold distance;

[0062] 6.2) Generate candidate paths: Starting from the current position of the vehicle, perpendicular to the direction of the local path (usually an orthogonal direction based on the vehicle direction), set the sampling interval, the number of candidate paths generated, and the candidate path interval, and perform sampling to generate multiple candidate paths. These candidate paths are generated in a way that gradually expands from the center of the vehicle to cover the entire area from the current position of the vehicle to the planned end point;

[0063] The specific calculation formula is as follows:

[0064] (x′ i ,y′ i )=Downsample(x i ,y i )

[0065]

[0066] Where Downsample(x i ,y i ) samples the local path; (x′ i ,y′ i ) is the sampling point; Δs is the sampling interval, and the candidate path on the left side of the sampling generation is The candidate path on the right side generated by sampling is (x″ i ,y″ i ) represents the generated candidate path point; (x′ i ,y′ i ) represents the point set after local path sampling; i represents the number of candidate paths generated on the left and right sides, ranging from 1 to N / 2, where N represents the total number of candidate paths; Δd represents the candidate path interval;

[0067] 6.3) Smooth candidate paths: The candidate trajectories are smoothed using cubic spline interpolation, which is a linear interpolation optimization technique that eliminates the curvature discontinuity caused by the sampling step, optimizes the curvature, and brings smoother driving operations. The specific calculation formula is as follows:

[0068] S i (x) = a + b (x i )+c(xx i ) 2 +d(xx i ) 3

[0069] S i (x i )=y i

[0070] S i (x i+1 )=y i+1

[0071] S′ i (x i ) = S′ i-1 (x i )

[0072] S″ i (x i )=S″ i-1 (x i )

[0073] S″0(x0)=S″ n-1 (x n )=0

[0074] Where x represents the interval [x i ,x i+1 ] interpolation point. x i is the interval [x i ,x i+1 ] is the left endpoint, the spline function S i (x) is defined within this interval;

[0075] 6.4) Evaluate each generated candidate path based on the priority cost, lane change cost, and collision cost, and select the one with the smallest total cost as the optimal path. The specific calculation formula is as follows:

[0076] Cost=Center_cost+Transition_cost+Collision_cost

[0077] Center_cost=i*Δd

[0078] Transition_cost=|i c ±i g |*Δd

[0079] if||O(x,y)-Obstacle(x,y)||2<R

[0080] pass

[0081] else

[0082] Collision_cost=||O(x,y)-Obstacle(x,y)||2

[0083] Among them, Cost is the total cost of each candidate path; Center_cost is the priority cost; Transition_cost is the lane change cost; Collision_cost is the collision cost.

[0084] 6.4.1) Priority Cost

[0085] Each candidate path is graded. The center path has the highest priority and a cost of 0. Then the cost gradually increases from the center path to both sides. This is to keep the vehicle on the center path when there are no obstacles.

[0086] The specific calculation formula is Center_cost = i*Δd, which calculates the distance between the candidate path where the current vehicle is located and the center path, where i is the path index; Δd is the candidate path interval;

[0087] 6.4.2) Lane-changing cost

[0088] To prevent the vehicle from frequently switching between candidate paths and ensure the smoothness of the vehicle's forward movement;

[0089] The specific calculation formula is Transition_cost = i c ±i g |*Δd, calculate the distance between the candidate path where the current vehicle is located and the remaining candidate paths, where i c is the candidate path index of the current vehicle; i g is the index of the remaining candidate paths; when the two indexes are on the same side of the central path, i c -i g |The opposite is i c +i g |;

[0090] 6.4.3) Collision Cost

[0091] In order to avoid collision, there are two steps: collision detection and collision cost calculation. Collision detection: Use the "point in the circle" method, with the test point as the obstacle, the center of the circle as the path point of the candidate path, and the radius of the circle as half the width of the car plus the collision detection range. Candidate paths where the obstacle is within the circle are screened out;

[0092] The specific calculation formula is ||O(x,y)-Obstacle(x,y)||2<R, O(x,y) is the center of the circle, Obstacle(x,y) is the obstacle point, and R is the radius of the circle. Collision cost calculation: After completing collision detection, calculate the distance from the obstacle point to the remaining candidate paths. The specific calculation formula is Collision_cost=||O(x,y)-Obstacle i (x,y)||2.

[0093] In general, the input of local path planning is the global path, combined positioning, and obstacles. Through four steps of local path selection, candidate path generation, path smoothing, and cost calculation, it finally outputs an optimal local path with the lowest cost.

[0094] The image recognition license plate process in step 7 is as follows.

[0095] 7.1) Input image: The camera collects vehicle images during the inspection process in real time;

[0096] 7.2) Preprocessing: The collected vehicle images need to be grayed out to highlight the main features of the license plate and better extract the license plate area. The three components of the color image are averaged to obtain a gray value. The specific calculation formula is as follows:

[0097] f(i,j)=(R(i,j),G(i,j),B(i,j)) / 3

[0098] Where (R(i,j), G(i,j), B(i,j)) are the pixel values ​​of the three channels of the image; f(i,j) is the image after the average of the three channel pixels;

[0099] 7.3) License plate edge detection: Use Canny edge detection to calculate the image gradient and obtain the license plate edge, which is divided into the following four steps:

[0100] 7.3.1) Image denoising: Add the grayscale value of a pixel in the original image to the grayscale values ​​of its eight neighboring pixels, and then use the average value (divided by 9) as the grayscale value of the pixel in the new image.

[0101]

[0102] Where f(i,j) is the noisy image; g(i,j) is the denoised image; and N is 9.

[0103] 7.3.2) Calculate image gradient: On the denoised image, use the Sobel operator to calculate the horizontal gradient G x , vertical gradient G y , gradient amplitude G, gradient direction θ;

[0104] Horizontal gradient (G x ):

[0105]

[0106] Vertical gradient (G y ):

[0107]

[0108] Calculate the gradient magnitude G:

[0109]

[0110] Calculate the gradient direction θ:

[0111]

[0112] 7.3.3) Non-maximum suppression: If the gradient amplitude of the current pixel is not the local maximum in the gradient direction, it is set to 0 and only the local maximum is retained as the edge;

[0113] 7.3.4) Double threshold screening: To further screen out the true edge points and remove the falsely detected edges, two thresholds are applied to classify the pixels into strong edge, weak edge and non-edge points;

[0114] The gradient amplitude is divided into two thresholds T high and T low ;

[0115] The gradient amplitude is higher than T high The points with a gradient magnitude lower than T are marked as strong edges. low The points are marked as non-edges;

[0116] The gradient amplitude will be between T high and T low For the points between them, determine whether they are connected to the strong edge points. If so, mark them as weak edges, otherwise mark them as non-edges.

[0117] 7.4) Character segmentation: After completing the license plate edge detection, the license plate area is segmented into individual characters, which is divided into the following two steps:

[0118] 7.4.1) License plate binarization: Find a suitable threshold to divide the license plate area into two parts: numbers and background.

[0119]

[0120] Where G(x,y) represents the binary image; g(x,y) represents each pixel value; T represents the set pixel threshold;

[0121] 7.4.2) Character segmentation: According to the boundary points where pixels 0 and 1 jump, the license plate numbers are segmented;

[0122] 7.5) Character recognition: Based on the template matching algorithm, the segmented characters are matched with all templates and the best match is selected as the result.

[0123] In step 8, the inspection robot stores the license plate and location information of the detected vehicle in the database, and the user scans the QR code and uses the vehicle search applet to search for the vehicle by the license plate.

[0124] Beneficial effects of the present invention:

[0125] Through the mapping and trajectory recording proposed in step 1.2, it is convenient for staff to observe the location of the robot and facilitate management.

[0126] The global and local sensor fusion methods proposed in step 3 can effectively solve the problem of decreased robot positioning accuracy in complex environments.

[0127] The inspection method of global path tracking + local path planning proposed in step 4.5.6 can effectively complete the inspection task while avoiding obstacles, greatly increasing the safety of the system.

[0128] Through the license plate recognition method proposed in step 7, the license plate can be accurately recognized 24 hours a day.

[0129] The small program car search method proposed in step 8 can facilitate car owners to find their cars while reducing the pressure on on-site staff. BRIEF DESCRIPTION OF THE DRAWINGS

[0130] Figure 1 Finally build a map for the SLAM of the present invention

[0131] Figure 2 It is the GNSS inspection track point.

[0132] Figure 3 It is a schematic diagram of the multi-sensor ESKF fusion process of the present invention.

[0133] Figure 4 Schematic diagram of the global pure tracking algorithm of the present invention.

[0134] Figure 5 It is a schematic diagram of the Euclidean clustering algorithm flow of the present invention.

[0135] Figure 6 Schematic diagram of clustering results of the present invention.

[0136] Figure 7 A candidate path diagram is generated for the present invention.

[0137] Figure 8 This is a schematic diagram of candidate path smoothing of the present invention.

[0138] Fig. 9 This is a schematic diagram of the image recognition license plate process.

[0139] Fig.10 This is a schematic diagram of the car-finding applet of the present invention.

[0140] Fig.11 It is a schematic diagram of vehicle navigation of the present invention. DETAILED DESCRIPTION

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

[0142] A navigation and vehicle-finding method for an automatic inspection robot in an outdoor parking environment comprises the following steps;

[0143] 1) Control the vehicle according to the pre-set inspection route, use the NDT algorithm to perform SLAM mapping of the parking environment, and finally construct a map such as Figure 1 As shown, the formula of the NDT registration algorithm is:

[0144]

[0145] The above formula is used to obtain the robot's position in the map (R * ,t * ), which makes the sum of the distances between the target point cloud and all paired points of the source point cloud f(R * ,t * ) is the smallest. Among them, f(R * ,t * ) is the sum of the distances of all paired points; (R * ,t * ) is the NDT estimated robot's position in the map; R * is the rotation of the robot relative to the starting point of the map; t * is the translation of the robot relative to the starting point of the map; MIN is to minimize the sum of the distances of all paired points; N P is the number of paired points, p i With q i is a pair of matching points between the target point cloud and the source point cloud; || 2 is the Euclidean distance between paired points.

[0146] 2) Record GNSS inspection track points during the mapping process, and sample and save the recorded inspection track points according to the preset distance threshold. Figure 2 shown.

[0147] 3) When GNSS has a signal, use GNSS position information, IMU attitude information, and wheel odometer as the input of Extended State Kalman Filter (ESKF) for global fusion positioning, and use the combined positioning result as the initial pose transformation (R, t) of the radar odometer to eliminate the accumulated error of NDT as the radar odometer. When GNSS is blocked and has no signal, use the radar odometer instead of GNSS, and use the radar odometer, IMU attitude information, and wheel odometer to achieve local fusion positioning. The ESKF fusion process between multiple sensors is as follows: Figure 3 As shown, there are five steps in total:

[0148] 3.1) Predict the current vehicle state based on the optimal estimated state at the previous moment Predict the current state

[0149]

[0150] in is the state transfer matrix, which indicates the current state is inferred from the previous state; is the control matrix, which represents the control quantity u t-1 the impact on the current state, such as acceleration; The ^ in the value indicates that it is an estimate, not the true value. In - Indicates that it is not a best estimate.

[0151] 3.2) Based on the optimal prediction state at the previous moment The covariance matrix P of the noise (error) t-1 , calculate the current prediction state The noise (error) covariance matrix P t - .

[0152] P t - =FP t-1 F T +Q

[0153] Where P t-1 represents the noise (error) covariance moment of the previous moment; Q represents the noise (error) covariance matrix.

[0154] 3.3) Calculate the Kalman coefficient.

[0155] K t =P t - H T (HP t - H T +R) -1

[0156] Where K t represents the Kalman coefficient; H represents the predicted state To the sensor observation state Z t The dimensional transformation relationship between them; R represents the noise (error) covariance matrix of the observation state.

[0157] 3.4) Optimize prediction status Get the best prediction state As the final output of the filter, it also serves as the input for the next iteration.

[0158]

[0159] in Represents the actual sensor observation state Z t Sensor observation state and prediction The residual between .

[0160] 3.5) Update the optimal prediction state The noise (error) covariance matrix is ​​used in the next iteration.

[0161] P t =(IK t H)P t -

[0162] Where P t The optimal prediction state The noise (error) covariance matrix of ; I is the identity matrix.

[0163] 4) Based on the combined positioning information, the global pure tracking algorithm is used to realize the navigation of the outdoor mobile robot.

[0164] The specific tracking process is as follows: Figure 4 As shown in the figure, A is the center of the rear wheel of the vehicle; B is the center of the front wheel of the vehicle; O is the steering center of the vehicle; and P is the preview point of the vehicle. The vehicle tracks the preview point to achieve tracking, which is divided into the following four steps:

[0165] 4.1) In order for the rear wheel of the vehicle to track the global trajectory Path to reach the preview point P, the sine theorem relationship needs to be satisfied in the triangle AO P.

[0166]

[0167] Simplified

[0168]

[0169] Where R represents the turning radius; l d Indicates the preview distance.

[0170] 4.2) In the Ackermann Turn model, the rear wheel performs circular motion and the turning radius R is calculated in the triangle AO B.

[0171]

[0172] where δ f represents the front wheel steering angle; L represents the vehicle distance from the axle.

[0173] 4.3) Combining 4.1 and 4.2, we can get the steering angle of the pure tracking algorithm.

[0174]

[0175] 4.4) Calculate the angle between the preview point and the vehicle body.

[0176]

[0177] Where a represents the angle between the preview point and the vehicle body; β represents the angle between the preview point and the X-axis; represents the vehicle heading angle; (x1 y1) represents the center coordinate of the vehicle's rear wheel A; (x2 y2) represents the center coordinate of the vehicle's rear wheel B; (x3 y3) represents the coordinate of the preview point P.

[0178] 5) Using LiDAR to cluster and identify obstacles is divided into the following two steps:

[0179] 5.1) Downsampling of point cloud voxels, cropping of point cloud height, and cropping of point clouds above the top of the vehicle are both aimed at reducing the amount of subsequent calculations.

[0180] 5.2) Separate the ground point cloud from the non-ground point cloud, and use the Euclidean clustering algorithm to cluster obstacles in the non-ground point cloud. The principle of the Euclidean clustering algorithm is that in the point cloud data, the distance between two points in the point cloud cluster of the same object is less than a certain value, while the distance between point cloud clusters of different objects is greater than a certain value. Points with distances less than the set distance threshold are merged into one category, thereby completing the clustering process. The process of the Euclidean clustering algorithm is as follows: Figure 5 As shown in , the point cloud is classified into separate point sets according to the distance threshold. The clustering results are shown in Figure 6 As shown, the obstacles above the ground are clustered.

[0181] 6) Based on the known combined positioning, global path, and obstacles, local path planning is performed, which is divided into the following four steps:

[0182] 6.1) Extract the local path from the global path based on the vehicle’s current position and the maximum planned distance.

[0183]

[0184] Where S is the set of points contained in the local path; (x i ,y i ) is the set of points contained in the global path; (x c ,y c ) is the current position of the vehicle; D is the threshold distance

[0185] 6.2) Generate candidate paths: Starting from the current position of the vehicle, perpendicular to the direction of the local path (usually an orthogonal direction based on the vehicle direction), set the sampling interval, the number of candidate paths generated, and the candidate path interval, and perform sampling to generate multiple candidate paths. These candidate paths are generated in a way that gradually expands from the center of the vehicle to cover the entire area from the current position of the vehicle to the planned end point. Generate candidate paths as follows Figure 7 As shown, 10 candidate paths are generated on the left and right sides with the local path as the center, for a total of 21 paths. The specific calculation formula is as follows:

[0186] (x′ i ,y′ i )=Downsample(x i ,y i )

[0187]

[0188] Where Downsample(x i ,y i ) samples the local path; (x′ i ,y′ i ) is the sampling point; Δs is the sampling interval. The candidate path on the left side of the sampling generation is The candidate path on the right side generated by sampling is (x″ i ,y″ i ) represents the generated candidate path point; (x′ i ,y′ i ) represents the point set after local path sampling; i represents the number of candidate paths generated on the left and right sides, ranging from 1 to N / 2, where N represents the total number of candidate paths; Δd represents the candidate path interval.

[0189] 6.3) Smoothing candidate paths: The generated candidate trajectories are smoothed using cubic spline interpolation, a linear interpolation optimization technique that eliminates the curvature discontinuity caused by the sampling step, optimizes the curvature, and brings smoother driving operations. Figure 8 As shown, the specific calculation formula is as follows:

[0190] S i (x) = a + b (x i )+c(xx i ) 2 +d(xx i ) 3

[0191] S i (x i )=y i

[0192] Si (x i+1 )=y i+1

[0193] S′ i (x i ) = S′ i-1 (x i )

[0194] S″ i (x i )=S″ i-1 (x i )

[0195] S″0(x0)=S″ n-1 (x n )=0

[0196] Where x represents the interval [x i ,x i+1 ] interpolation point. x i is the interval [x i ,x i+1 ] is the left endpoint of the spline function S i (x) is defined within this interval.

[0197] 6.4) Evaluate each generated candidate path based on priority cost, lane change cost, and collision cost, and select the one with the lowest total cost as the optimal path. The specific calculation formula is as follows:

[0198] Cost=Center_cost+Transition_cost+Collision_cost

[0199] Center_cost=i*Δd

[0200] Transition_cost=|i c ±i g |*Δd

[0201] if||O(x,y)-Obstacle(x,y)||2<R

[0202] pass

[0203] else

[0204] Collision_cost=||O(x,y)-Obstacle(x,y)||2

[0205] Where Cost is the total cost of each candidate path; Center_cost is the priority cost; Transition_cost is the lane change cost; Collision_cost is the collision cost

[0206] 6.4.1) Priority Cost

[0207] Each candidate path is graded. The center path has the highest priority and a cost of 0. Then the cost gradually increases from the center path to both sides. This is to keep the vehicle on the center path when there are no obstacles. The specific calculation formula is Center_cost = i*Δd, which calculates the distance between the candidate path where the current vehicle is located and the center path. Where i is the path index; Δd is the candidate path interval.

[0208] 6.4.2) Lane-changing cost

[0209] In order to prevent the vehicle from frequently switching between candidate paths and ensure the smoothness of the vehicle's progress, the specific calculation formula is Transition_cost = |i c ±i g |*Δd, calculate the distance between the candidate path where the current vehicle is located and the other candidate paths. c is the candidate path index of the current vehicle; i g is the index of the remaining candidate paths; when the two indexes are on the same side of the central path, |i c -i g |The opposite is|i c +i g |.

[0210] 6.4.3) Collision Cost

[0211] In order to avoid collisions, it is divided into two steps: collision detection and collision cost calculation. Collision detection: Use the "point in the circle" method, the test point is the obstacle, the center of the circle is the path point of the candidate path, and the radius of the circle is half the width of the car plus the collision detection range. Candidate paths where the obstacle is within the circle are screened out. The specific calculation formula is ||O(x,y)-Obstacle(x,y)||2<R, O(x,y) is the center of the circle, Obstacle(x,y) is the obstacle point, and R is the radius of the circle. Collision cost calculation: After completing the collision detection, calculate the distance from the obstacle point to the remaining candidate paths. The specific calculation formula is Collision_cost=||O(x,y)-Obstacle i (x,y)||2.

[0212] In general, the input of local path planning is the global path, combined positioning, and obstacles. Through four steps of local path selection, candidate path generation, path smoothing, and cost calculation, it finally outputs an optimal local path with the lowest cost.

[0213] 7) During the robot inspection process, the image recognition algorithm is used to identify the license plate, which makes it easier for subsequent users to find the car based on the license plate. The image recognition license plate process is as follows: Fig. 9 shown.

[0214] 7.1) Input image: The camera collects vehicle images during the inspection process in real time.

[0215] 7.2) Preprocessing: The collected image needs to be grayscaled to highlight the main features of the license plate and better extract the license plate area. The three components of the color image are averaged to obtain a grayscale value. The specific calculation formula is as follows:

[0216] f(i,j)=(R(i,j),G(i,j),B(i,j)) / 3

[0217] Where (R(i,j), G(i,j), B(i,j)) are the pixel values ​​of the three channels of the image; f(i,j) is the image after the average of the three channel pixels.

[0218] 7.3) License plate edge detection: Use Canny edge detection to calculate the image gradient and obtain the license plate edge, which is divided into the following four steps:

[0219] 7.3.1) Image denoising: Add the grayscale value of a pixel in the original image to the grayscale values ​​of its eight neighboring pixels, and then use the average value (divided by 9) as the grayscale value of the pixel in the new image.

[0220]

[0221] Where f(i,j) is the noisy image; g(i,j) is the denoised image.

[0222] 7.3.2) Calculate image gradient: On the denoised image, use the Sobel operator to calculate the horizontal gradient G x , vertical gradient G y , gradient amplitude G, gradient direction θ.

[0223] Horizontal gradient (G x ):

[0224]

[0225] Vertical gradient (G y ):

[0226]

[0227] Calculate the gradient magnitude G:

[0228]

[0229] Calculate the gradient direction θ:

[0230]

[0231] 7.3.3) Non-maximum suppression: If the gradient amplitude of the current pixel is not the local maximum in the gradient direction, it is set to 0 and only the local maximum is retained as the edge.

[0232] 7.3.4) Double threshold screening: To further screen out the true edge points and remove the falsely detected edges, two thresholds are applied to classify the pixels into strong edge, weak edge and non-edge points.

[0233] The gradient amplitude is divided into two thresholds T high and T low .

[0234] The gradient amplitude is higher than T high The points with a gradient magnitude lower than T are marked as strong edges. low The points are marked as non-edges.

[0235] The gradient amplitude will be between T high and T low For the points between , determine whether they are connected to the strong edge points. If so, mark them as weak edges, otherwise mark them as non-edges.

[0236] 7.4) Character segmentation: After completing the license plate edge detection, the license plate area is segmented into individual characters, which is divided into the following two steps:

[0237] 7.4.1) License plate binarization: Find a suitable threshold to divide the license plate area into two parts: numbers and background.

[0238]

[0239] Where G(x,y) represents the binary image; g(x,y) represents each pixel value; T represents the set pixel threshold.

[0240] 7.4.2) Character segmentation: According to the boundary points where pixels 0 and 1 transition, the license plate numbers are segmented.

[0241] 7.5) Character recognition: Based on the template matching algorithm, the segmented characters are matched with all templates and the best match is selected as the result.

[0242] 8) Scan code to find the car based on WeChat applet: Users scan the code to find the car based on the license plate, which is divided into the following four steps:

[0243] 8.1) The inspection robot stores the license plate and location information of the detected vehicle into the database.

[0244] 8.2) The user scans the QR code and enters the license plate number.

[0245] 8.3) The applet obtains the user’s current location and queries the backend database for the location corresponding to the corresponding license plate.

[0246] 8.4) Route planning is performed based on the user's current location and the vehicle's location, and the user searches for the vehicle along the route. Fig.10 As shown, after entering the license plate, the vehicle navigation is as follows Fig.11 shown.

Claims

1. A navigation and car-finding method for an automatic inspection robot in an outdoor parking environment, characterized in that: The steps include: Step 1: Control the vehicle remotely according to the pre-set inspection route and use the NDT algorithm to perform SLAM mapping of the parking lot environment; Step 2: Record GNSS inspection track points during SLAM mapping, and sample and save the recorded inspection track points according to the preset distance threshold; Step 3: When GNSS has a signal, use the GNSS position information, IMU attitude information, and wheel odometer as the input of the extended state Kalman filter (ESKF) for global fusion positioning, and use the combined positioning result as the initial pose transformation (R, t) of the radar odometer to eliminate the accumulated error of NDT as the radar odometer; When GNSS is blocked and has no signal, the radar odometer is used to replace the GNSS position information, and the radar odometer, IMU attitude information, and wheel odometer are used to achieve local fusion positioning; Step 4: Based on the combined positioning results of global fusion positioning and local fusion positioning, the global pure tracking algorithm is used to realize the navigation of the outdoor mobile robot; Step 5: Use the laser radar on the top of the robot to cluster and identify obstacles; Step 6: Based on the known combined positioning, global path, and obstacles, perform local path planning; Step 7: During the robot inspection process, the image recognition algorithm is used to identify the license plate, so that subsequent users can find the car based on the license plate; Step 8: Scan the QR code to find the car based on the WeChat applet.

2. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 1, characterized in that: The formula of the NDT registration algorithm in step 1 is: The above formula is used to obtain the robot's position in the map (R * ,t * ), which makes the sum of the distances between the target point cloud and all paired points of the source point cloud f(R * ,t * ) is the smallest, where f(R * ,t * ) is the sum of the distances of all paired points; (R * ,t * ) is the position and posture of the robot in the map estimated by the NDT algorithm; R * is the rotation of the robot relative to the starting point of the map; t * is the translation of the robot relative to the starting point of the map; MIN is to minimize the sum of the distances of all paired points; N P is the number of paired points, p i With q i is a pair of matching points between the target point cloud and the source point cloud; | | 2 is the Euclidean distance between paired points.

3. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 1, characterized in that: The step 3 is specifically as follows: The ESKF fusion process between GNSS position information, IMU attitude information, wheel odometer, and laser odometer is divided into the following five steps: 3.1) Predict the current vehicle state based on the optimal estimated state at the previous moment Predict the current state in is the state transfer matrix, which indicates the current state is inferred from the previous state; is the control matrix, which represents the control quantity u t-1 Impact on current status; The ^ in the value indicates that it is an estimate, not the true value. The - in the figure indicates that it is not the best estimate; 3.2) Based on the optimal prediction state at the previous moment The covariance matrix P of the noise (error) t-1 , calculate the current prediction state The noise (error) covariance matrix P t - =FP t-1 F T +Q Where P t-1 represents the noise (error) covariance moment of the previous moment; Q represents the noise (error) covariance matrix; 3.3) Calculate the Kalman coefficient; K t =P t - H T (HP t - H T +R) -1 Where K t represents the Kalman coefficient; H represents the predicted state To the sensor observation state Z t The dimensional transformation relationship between them; R represents the noise (error) covariance matrix of the observed state; 3.4) Optimize the prediction status Get the best prediction state As the final output of the filter, it also serves as the input for the next iteration; in Represents the actual sensor observation state Z t Sensor observation state and prediction The residual between 3.5) Update the optimal prediction state The noise (error) covariance matrix is ​​used in the next iteration; P t =(I-K t H)P t - Where P t The optimal prediction state The noise (error) covariance matrix of ; I is the identity matrix.

4. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 3, characterized in that: The step 4 is specifically as follows: The specific tracking process is divided into the following four steps: 4.1) In order for the rear wheel of the vehicle to track the global trajectory Path to reach the preview point P, the sine theorem relationship needs to be satisfied in the triangle AOP. The vehicle can track the preview point to achieve tracking. Simplified Where R represents the turning radius; l d Indicates preview distance; 4.2) In the Ackerman front wheel steering model, circular motion is performed with the rear wheel as the center, and the steering radius R is calculated in the triangle AOB; where δ f represents the front wheel steering angle; L represents the vehicle distance axis; A represents the center of the vehicle's rear wheels; B represents the center of the vehicle's front wheels; O represents the vehicle's steering center; P represents the vehicle's preview point, 4.3) Combine 4.1) and 4.2) to get the steering angle of the pure tracking algorithm; 4.4) Calculate the angle between the preview point and the vehicle body; Where a represents the angle between the preview point and the vehicle body; β represents the angle between the preview point and the X-axis; represents the vehicle heading angle; (x1 y1) represents the center coordinate of the vehicle's rear wheel A; (x2 y2) represents the center coordinate of the vehicle's rear wheel B; (x3 y3) represents the coordinate of the preview point P.

5. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 4, characterized in that: The step 5 is divided into the following two steps: 5.1) Downsampling of point cloud voxels, point cloud height cropping, cropping of point clouds above the top of the vehicle; 5.2) Separate the ground point cloud from the non-ground point cloud, and use the Euclidean clustering algorithm to cluster the obstacles of the non-ground point cloud. The principle of the Euclidean clustering algorithm is that in the point cloud data, the distance between two points in the point cloud cluster of the same object is less than a certain value, while the distance between the point cloud clusters of different objects is greater than a certain value. Points with distances less than the set distance threshold are merged into one category, thereby completing the clustering process.

6. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 1, characterized in that: The local path planning in step 6 is divided into the following four steps: 6.1) Extract the local path from the global path based on the vehicle’s current position and maximum planned distance; Where S is the set of points contained in the local path; (x i ,y i ) is the set of points contained in the global path; (x c ,y c ) is the current position of the vehicle; D is the threshold distance; 6.2) Generate candidate paths: Starting from the current position of the vehicle, perpendicular to the direction of the local path, set the sampling interval, the number of candidate paths generated, and the candidate path interval, and perform sampling to generate multiple candidate paths. These candidate paths are generated in a way that gradually expands from the center of the vehicle to cover the entire area from the current position of the vehicle to the planned end point; The specific calculation formula is as follows: (x i ',and i ′)=Downsample(x i ,and i ) Where Downsample(x i ,y i ) samples the local path; (x i ′,y i ′) is the sampling point; Δs is the sampling interval, and the candidate path on the left side of the sampling generation is The candidate path on the right side generated by sampling is (x i ″,y i ″) represents the generated candidate path points; (x i ′,y i ′) represents the point set after local path sampling; i represents the number of candidate paths generated on the left and right sides, ranging from 1 to N / 2, where N represents the total number of candidate paths; Δd represents the candidate path interval; 6.3) Smoothing candidate paths: The candidate trajectories are smoothed using cubic spline interpolation. The specific calculation formula is as follows: S i (x)=a+b(x-x i )+c(x-x i ) 2 +d(x-x i ) 3 S i (x i )=y i S i (x i+1 )=y i+1 S i ′(x i )=S i ′ -1 (x i ) S i ″(x i )=S i ″ -1 (x i ) S0″(x0)=S n ″ -1 (x n )=0 Where x represents the interval [x i ,x i+1 ] interpolation point within. x i is the interval [x i ,x i+1 ] is the left endpoint, the spline function S i (x) is defined within this interval; 6.4) Evaluate each generated candidate path based on the priority cost, lane change cost, and collision cost, and select the one with the smallest total cost as the optimal path. The specific calculation formula is as follows: Cost=Center_cost+Transition_cost+Collision_cost Center_cost=i*Δd Transition_cost=|i c ±i g |*Δd if||O(x,y)-Obstacle(x,y)||2<R pass else Collision_cost=||O(x,y)-Obstacle(x,y)||2 Among them, Cost is the total cost of each candidate path; Center_cost is the priority cost; Transition_cost is the lane change cost; Collision_cost is the collision cost.

7. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 6, characterized in that: 6.4.1) Priority Cost Each candidate path is graded. The center path has the highest priority and a cost of 0. Then the cost gradually increases from the center path to both sides. This is to keep the vehicle on the center path when there are no obstacles. The specific calculation formula is Center_cost = i*Δd, which calculates the distance between the candidate path where the current vehicle is located and the center path, where i is the path index; Δd is the candidate path interval; 6.4.2) Lane-changing cost In order to prevent the vehicle from frequently switching between candidate paths and ensure the smoothness of the vehicle's progress, the specific calculation formula is Transition_cost = |i c ±i g |*Δd, calculate the distance between the candidate path where the current vehicle is located and the remaining candidate paths, where i c is the candidate path index of the current vehicle; i g is the index of the remaining candidate paths; when the two indexes are on the same side of the central path, |i c -i g |The opposite is|i c +i g |; 6.4.3) Collision Cost Use the "point in circle" method, with the test point as the obstacle, the center of the circle as the path point of the candidate path, and the radius of the circle as half the width of the vehicle plus the collision detection range. Candidate paths with obstacles within the circle are screened out; The specific calculation formula is ||O(x,y)-Obstacle(x,y)||2<R, O(x,y) is the center of the circle, Obstacle(x,y) is the obstacle point, and R is the radius of the circle. Collision cost calculation: After completing collision detection, calculate the distance from the obstacle point to the remaining candidate paths. The specific calculation formula is Collision_cost=||O(x,y)-Obstacle i (x,y)||2.

8. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 1, characterized in that: The image recognition license plate process in step 7 is as follows. 7.1) Input image: The camera collects vehicle images during the inspection process in real time; 7.2) Preprocessing: The collected vehicle image needs to be grayed out, and the three components of the color image are averaged to obtain a gray value. The specific calculation formula is as follows: f(i,j)=(R(i,j),G(i,j),B(i,j)) / 3 Where (R(i,j), G(i,j), B(i,j)) are the pixel values ​​of the three channels of the image; f(i,j) is the image after the average of the three channel pixels; 7.3) License plate edge detection: Use Canny edge detection to calculate the image gradient and obtain the license plate edge, which is divided into the following four steps: 7.3.1) Image denoising: Add the grayscale value of a pixel in the original image to the grayscale values ​​of its neighboring pixels, and then use the average value as the grayscale value of the pixel in the new image; Where f(i,j) is the image with noise; g(i,j) is the image after denoising; 7.3.2) Calculate image gradient: On the denoised image, use the Sobel operator to calculate the horizontal gradient G x , vertical gradient G y , gradient amplitude G, gradient direction θ; Horizontal gradient (G x ): Vertical gradient (G y ): Calculate the gradient magnitude G: Calculate the gradient direction θ: 7.3.3) Non-maximum suppression: If the gradient amplitude of the current pixel is not the local maximum in the gradient direction, it is set to 0 and only the local maximum is retained as the edge; 7.3.4) Double threshold screening: To further screen out the true edge points and remove the falsely detected edges, two thresholds are applied to classify the pixels into strong edge, weak edge and non-edge points; The gradient amplitude is divided into two thresholds T high and T low ; The gradient amplitude is higher than T high The points with a gradient magnitude lower than T are marked as strong edges. low The points are marked as non-edges; The gradient amplitude will be between T high and T low For the points between them, determine whether they are connected to the strong edge points. If so, mark them as weak edges, otherwise mark them as non-edges. 7.4) Character segmentation: After completing the license plate edge detection, the license plate area is segmented into individual characters, which is divided into the following two steps: 7.4.1) License plate binarization: Find a suitable threshold to divide the license plate area into two parts: numbers and background. Where G(x,y) represents the binary image; g(x,y) represents each pixel value; T represents the set pixel threshold; 7.4.2) Character segmentation: According to the boundary points where pixels 0 and 1 jump, the license plate numbers are segmented; 7.5) Character recognition: Based on the template matching algorithm, the segmented characters are matched with all templates and the best match is selected as the result.

9. The method for navigating and finding a vehicle by an automatic inspection robot in an outdoor parking lot environment according to claim 1, characterized in that: In step 8, the inspection robot stores the license plate and location information of the detected vehicle in the database, and the user scans the QR code and uses the vehicle search applet to search for the vehicle by the license plate.

Images