CN117782102A - A fully automatic parking positioning and mapping method based on surround vision - Google Patents
A fully automatic parking positioning and mapping method based on surround vision Download PDFInfo
- Publication number
- CN117782102A CN117782102A CN202311830221.7A CN202311830221A CN117782102A CN 117782102 A CN117782102 A CN 117782102A CN 202311830221 A CN202311830221 A CN 202311830221A CN 117782102 A CN117782102 A CN 117782102A
- Authority
- CN
- China
- Prior art keywords
- time
- vehicle
- point cloud
- map
- imu
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种基于环视视觉的全自动泊车定位与建图方法，包括：1.通过车辆安装的四个环视鱼眼相机获取车辆周围的环视图像信息；2.利用深度学习方法提取环视图像中的车道线以及障碍物信息；3.对提取的车道线以及障碍物信息进行点云转化处理；4.融合IMU和里程计数据，进行位姿估计并生成全局停车场地图；5.通过回环检测进行地图优化，生成栅格地图用于路径导航规划。本发明结合环视相机、IMU和里程计的各自优势，提升了停车场定位与建图的精度，有助于提高泊车效率与准确度。
The invention discloses a fully automatic parking positioning and mapping method based on surround vision, which includes: 1. Obtaining surround image information around the vehicle through four surround-view fisheye cameras installed on the vehicle; 2. Using deep learning methods to extract surround view Lane lines and obstacle information in the image; 3. Perform point cloud conversion processing on the extracted lane lines and obstacle information; 4. Fusion of IMU and odometry data, perform pose estimation and generate a global parking lot map; 5. Through Loop detection performs map optimization and generates raster maps for path navigation planning. This invention combines the respective advantages of the surround-view camera, IMU and odometer to improve the accuracy of parking lot positioning and mapping, and helps to improve parking efficiency and accuracy.
Description
Technical Field
The invention relates to the field of automatic driving of vehicles, in particular to a full-automatic parking positioning and mapping method.
Background
With the rapid development of cities and the increase of the holding capacity of automobiles year by year, the shortage of parking spaces and the complex parking environment have become a great trouble for drivers, and a great deal of time and energy are wasted. In addition, the parking operation needs to be compatible with the conditions of the rear and the two sides of the vehicle, the space is compact, and no guidance is available in the process, so that the requirements on the proficiency of the driving operation of the driver are high. Even a driver with skilled operation is plagued by the problems of tedious parking process, inability to find a proper parking space, and the like. Based on the above requirements, intelligent parking technology has been developed, and the technology not only can solve the trouble of drivers, but also has great benefits for the efficient utilization of the parking space and the safety of the parking process.
The main problems faced by the autonomous parking positioning technology of the indoor parking lot include two aspects, namely the problem that the indoor environment and GNSS signals are often shielded and cannot be used; on the other hand, the autonomous parking technology is difficult to popularize due to the high cost of the sensor. Although the ability to accurately sense the environment can be increased by mounting a sensor such as a lidar, there is a potential for an increase in cost. Under the background of continuously reducing the cost and improving the industrial competitiveness in the automatic driving field, how to realize the optimization of the parking effect under the limited cost becomes the current problem to be solved urgently.
Disclosure of Invention
Aiming at the problem of optimizing the parking location and the map construction under the limited cost, the invention provides the method for optimizing the parking location and the map construction based on the looking-around vision, so that the map construction precision is improved by optimizing the location and the map construction under the condition of not changing the sensor configuration, the safe running of the vehicle in the underground parking lot can be ensured, and the improvement of the parking efficiency and the parking accuracy is facilitated.
In order to achieve the aim of the invention, the invention adopts the following technical scheme:
the invention discloses a full-automatic parking positioning and mapping method based on looking-around vision, which is characterized by comprising the following steps of:
step 1: in the moving process of the vehicle, after the fisheye cameras arranged at the front, the back, the left and the right of the vehicle acquire the looking-around image at the current time t, the looking-down image I at the time t is obtained through the operations of camera distortion removal, inverse perspective transformation and image stitching t ；
Step 2: extracting the top view image I by adopting deep Lab V3+ model of convolutional neural network t The lane lines and the obstacle information in the road are marked to obtain an image I 'with the lane lines and the obstacle information' t ；
Step 3: performing point cloud conversion on the extracted lane lines and the obstacle information;
step 3.1, step I' t The pixel points corresponding to the lane lines and the obstacle information in the road are converted into point clouds P t ；
Step 3.2, vehicle IMU sensor point cloud P according to t moment t Performing horizontal calibration to obtain point cloud P 'after horizontal calibration' t ；
Step 3.3, the point cloud P 'after horizontal calibration' t Radius filtering to obtain accurate point cloud P t ；
Step 4: fusing data of an IMU sensor and an odometer, estimating the pose of the vehicle at the time t+1 and generating a global parking lot map;
step 4.1, calculating the IMU estimated vehicle acceleration at the time t by using the method (1)And vehicle angular velocity>
In the formula (1), i represents an IMU sensor coordinate system,vehicle acceleration indicative of IMU sensor output at time t,/->Gaussian white noise indicative of the acceleration of the vehicle at time t, < >>Random walk noise indicative of vehicle acceleration at time t,/->The angular velocity of the vehicle output by the IMU sensor at the moment t is represented;Gaussian white noise indicative of the angular velocity of the vehicle at time t,/->Random walk noise representing the angular velocity of the vehicle at time t;
step 4.2, calculating the IMU estimated vehicle speed at time t+1 using (2)And vehicle angle->
In the formula (2), delta t Represents the difference between time t and time t+1, delta v Indicating the amount of change in speed, delta θ Indicating the amount of change in angle;and->Respectively representing the estimated vehicle speed and the estimated vehicle angle of the IMU at the time t; when t=0, let +.>And->All are zero;
step 4.3, calculating the estimated vehicle speed of the mileometer at the time t+1 by using the step (3)And vehicle angle->
In the formula (3), o represents an odometer coordinate system, v r,t+1 And v l,t+1 Respectively representing the right wheel speed and the left wheel speed, s of the output of the mileage meter at the time t+1 r,t+1 Sum s l,t+1 Respectively represent delta t Inner right and left wheel mileage, len is the vehicle body width,the vehicle angle estimated by the mileometer at the moment t; when t=0, let +.>Zero;
step 4.3, estimating the vehicle speed v after the fusion at the time t+1 by using the step (4) t+1 And a vehicle angle theta t+1 ：
In the formula (4), mu 1 Represents the weight, mu corresponding to the IMU sensor 2 Representing the weight corresponding to the odometer; mu is set when the vehicle is straight 1 ＜μ 2 The method comprises the steps of carrying out a first treatment on the surface of the Mu is set when the vehicle turns 1 ＞μ 2 ；
4.4, taking the initial position of the vehicle as the origin of coordinates, taking the direction of the vehicle as the positive direction of the Y axis, rotating the direction of the vehicle clockwise by 90 degrees along the plane of the vehicle as the positive direction of the X axis, and establishing a two-dimensional map coordinate system;
define the pose W of the vehicle at the time t t ＝(X t ,Y t ,V t ) Wherein X is t ,Y t For the two-dimensional coordinates of the vehicle at time t, V t The included angle between the direction of the vehicle and the positive direction of the X axis at the time t; let W when t=0 0 ＝(0,0,90°)；
Step 4.5, calculating the pose W of the vehicle at the time t+1 by using the step (5) t+1 ＝(X t+1 ,Y t+1 ,V t+1 )：
In the formula (5), X t+1 And Y t+1 Representing the two-dimensional coordinates of the vehicle at time t+1, V t+1 The included angle between the vehicle orientation and the positive X-axis direction of the vehicle at the time t+1 is shown;
step 4.6, pose W at time t t Adding accurate point cloud information P t To form a local point cloud map M at time t p,t The method comprises the steps of carrying out a first treatment on the surface of the According to the pose W at time t+1 t+1 Using normal conditionsThe distribution transformation algorithm calculates the accurate point cloud information P' at the time t+1 t+1 And M is as follows p,t A rotation matrix R therebetween t+1 Translation matrix T t+1 Thereby utilizing the formula (6) to obtain the point cloud P' at the time t+1 t+1 Performing transformation operation to obtain a local point cloud map M at the time t+1 p,t+1 Further obtain the global map M at time t+1 g,t+1 ＝M p,t +M p,t+1 ：
M p,t+1 ＝R t+1 *P″ t+1 +T t+1 (6)
Step 5: optimizing the global map through loop detection, and generating a grid map for path navigation planning;
step 5.1, obtaining an accurate point cloud set P "= { P" from the initial time to the t time " m M=0, 1,2,3, … t }, where P "is" m Representing an accurate point cloud at the mth moment;
step 5.2, performing loop detection on the accurate point cloud set P ' at the time t+1 to obtain an updated global map M ' at the time t+1 ' g,t+1 ：
Step 5.3, updating the global map M 'after t+1 time' h,t+1 Converting to grid map G at time t+1 t+1 And after being stored, the system is used for full-automatic parking of the vehicle.
The full-automatic parking positioning and mapping method based on the looking-around vision is also characterized in that in the step 3.2, the point cloud P is pointed by the vehicle IMU sensor according to the time t t Performing horizontal calibration to obtain point cloud P 'after horizontal calibration' t . Comprising the following steps:
step 3.2.1, defining a plane equation of the vehicle IMU sensor as follows: ax+by+cz+d=0, wherein x, y and z are x-axis, y-axis and z-axis of the vehicle IMU sensor plane respectively, a, b and c are normal vectors of x, y and z respectively, and d is the offset of the vehicle IMU sensor plane relative to the origin of coordinates thereof;
step 3.2.2, defining a vertical vector in a vehicle coordinate system as L, and rotating normal vectors a, b and c of the vehicle IMU plane through the vertical vector L to obtain a rotation matrix m r ；
Step 3.2.3,According to the rotation matrix m r Pair P of point cloud rotation functions t Processing to obtain a rotated point cloud, and setting the z-axis height value of the rotated point cloud to 0 to obtain a horizontally calibrated point cloud P '' t 。
In step 3.3, the point cloud P 'after horizontal calibration' t Radius filtering to obtain accurate point cloud P t Comprising:
step 3.3.1, selecting the point cloud P 'after the horizontal calibration' t Any one point of the two points is marked as a point A, the point A is used as a circle center, a distance threshold is used as a radius, a filter circle is formed, whether at least two points are contained in the filter circle is judged, and if the at least two points are contained in the filter circle, the point A is reserved; otherwise, deleting the point A;
step 3.3.2, repeatedly executing the step 3.3.1 until the point cloud P' t All the points in the map are processed until the processing is completed, so that all the reserved points form an accurate point cloud P t 。
The normal distribution transformation algorithm in the step 4.6 is carried out according to the following steps:
step 4.6.1, mapping the local point cloud M at the time t p,t After the down-sampling treatment is carried out on the point cloud in the map, a local point cloud map M 'after the treatment at the moment t is obtained' p,t ；
Step 4.6.2, M' p,t Discretizing into three-dimensional grids, and calculating the mean value and covariance matrix of point clouds in each grid to obtain a Gaussian distribution GD 1 ；
Step 4.6.3, accurate point cloud information P' at time t+1 according to step 4.6.1 and step 4.6.2 t+1 Processing to obtain another Gaussian distribution GD 2 ；
Step 4.6.4, calculate GD 1 And GD 2 KL divergence between the two, and minimizing KL divergence by gradient descent method, thereby obtaining optimal rotation matrix R t+1 Translation matrix T t+1 。
Step 5.2 is performed as follows:
step 5.2.1, rapidly searching the accurate point cloud P 'between the P' and the t+1 moment by using a Kd-tree algorithm t+1 Accurate point cloud P' with maximum similarity value k ，P″ k ∈P”,0≤k≤t；
Step 5.2.2, calculating the accurate point cloud P' at the time t+1 t+1 With P k Degree of similarity S of (2) t+1 ；
Step 5.2.3, calculating the accurate point cloud P' at the time t+1 by using an iterative nearest point algorithm t+1 With P k Euclidean distance d between t+1 The method comprises the steps of carrying out a first treatment on the surface of the When S is t+1 Is greater than a set similarity threshold, and d t+1 If the distance is smaller than the set Euclidean distance threshold, the vehicle returns to the previous historical position, and the step 5.2.4 is executed, otherwise, the vehicle is not in a loop state, and the updated global map M 'at the time of t+1 is obtained' g,t+1 ＝M g,t+1 ；
Step 5.2.4, the pose W at the time t+1 is subjected to nonlinear optimization algorithm t+1 Optimizing to minimize the accurate point cloud P' at the time t+1 t+1 With P k The error between the two is obtained, so that the pose W 'after the optimization at the time of t+1 is obtained' t+1 ；
According to W' t+1 For a local point cloud map M at time t+1 p,t+1 Updating to obtain a local point cloud map M 'updated at the time t+1' p,t+1 Thereby obtaining the global map M 'updated at the time t+1' g,t+1 ＝M p,t +M' p,t+1 。
Step 5.3 is performed as follows:
step 5.3.1 for grid map G t+1 Initializing, including: grid map G with t+1 time set t+1 According to M' g,t+1 Boundary-computing grid map G of (1) t+1 Is provided with G t+1 The origin of (2) is also the initial position of the vehicle;
step 5.3.2, p M' g,t+1 After radius filtering and downsampling processing, a lightweight global map M' at the time t+1 is obtained g,t+1 ；
Step 5.3.3, traversing M g,t+1 The point clouds in the map are mapped to the grid map G one by one t+1 And marks the probability of the occupied position in the grid mapDenoted as 100, the probability of unoccupied locations is marked as 0, resulting in a final grid map G at time t+1 t+1 。
The electronic equipment comprises a memory and a processor, wherein the memory is used for storing a program for supporting the processor to execute the full-automatic parking positioning and mapping method, and the processor is configured to execute the program stored in the memory.
The invention relates to a computer readable storage medium, wherein a computer program is stored on the computer readable storage medium, and the computer program is executed by a processor to execute the steps of the full-automatic parking positioning and mapping method.
Compared with the prior art, the invention has the beneficial effects that:
1. according to the invention, through fusing the information of the looking-around fish-eye camera, the IMU and the odometer, rich parking space information and obstacle information are obtained, so that the sensing capability of the parking environment is improved under the condition of not increasing the cost of the sensor, and the problem of inaccurate positioning caused by independently using the camera is solved.
2. According to the method for horizontally calibrating the vehicle according to the vehicle posture, when the vehicle passes through the deceleration strip and the hollow zone, the deviation generated when the vehicle is interfered by road factors can be corrected, the currently acquired point cloud is horizontally calibrated by utilizing the vehicle IMU sensor information, and the accuracy of acquiring the parking space and obstacle position information is improved.
3. The invention provides a method for fusing measurement values of an IMU sensor and an odometer, which is used for carrying out weight distribution according to the running state of a vehicle and the characteristics of the sensor. The advantages of the IMU sensor in relatively high accuracy in vehicle turning and the odometer in vehicle straight running are facilitated to be combined, and the accuracy of positioning and image building is further improved.
Drawings
FIG. 1 is a flow chart of a positioning and mapping method provided by the invention;
FIG. 2 is a schematic diagram of a global point cloud map after loop detection optimization provided by the invention;
Detailed Description
The technical scheme of the invention is further described in detail below with reference to the attached drawings and specific embodiments.
In this embodiment, as shown in fig. 1, a full-automatic parking positioning and mapping method based on looking-around vision includes the following steps:
step 1: in the moving process of the vehicle, after the fisheye cameras arranged at the front, the back, the left and the right of the vehicle acquire the looking-around image at the current time t, the looking-down image I at the time t is obtained through the operations of camera distortion removal, inverse perspective transformation and image stitching t The method comprises the steps of carrying out a first treatment on the surface of the In practical application, the horizontal angle of view of the fisheye camera is 196 degrees, the vertical angle of view is 147 degrees, the detection distance is more than 10m, and the looking-around image is transmitted to a vision processing unit of a full-automatic parking controller mounted on a vehicle through a video decoder to perform the generation operation of the looking-down image.
Step 2: deepLab V3+ model using convolutional neural network to extract top view image I t The lane lines and the obstacle information in the road are marked to obtain an image I 'with the lane lines and the obstacle information' t ；
Step 2.1, installing Labelme software in a Ubuntu system, marking the acquired 2000 parking lot overlook pictures by using an image marking tool Labelme, marking lane lines and barrier information, and obtaining a marked parking lot picture data set D;
step 2.2, inputting the marked parking lot picture data set D into a DeepLab V < 3+ > model for training, carrying out forward propagation, calculating a loss function, and then adjusting model parameters through backward propagation to finally obtain an optimized model M;
step 2.3, using the optimized model M to look down the image I t Processing is performed to extract lane lines and obstacle information, and different information types are marked with different colors, for example, lane lines are marked with red, and RGB values are respectively 255,0 and 0. The obstacle is marked as blue, and RGB values are 0,0 and 255 respectively; further, an image I 'with lane lines and obstacle information is obtained' t 。
Step 3: performing point cloud conversion on the extracted lane lines and the obstacle information;
step 3.1, step I' t The pixel points corresponding to the lane lines and the obstacle information in the road are converted into point clouds P t The method comprises the steps of carrying out a first treatment on the surface of the The default point cloud format is pcl:PointXYZRGB format.
Step 3.2, vehicle IMU sensor point cloud P according to t moment t Performing horizontal calibration to obtain point cloud P 'after horizontal calibration' t ：
Step 3.2.1, defining a plane equation of the vehicle IMU sensor as follows: ax+by+cz+d=0, wherein x, y and z are x-axis, y-axis and z-axis of the vehicle IMU sensor plane respectively, a, b and c are normal vectors of x, y and z respectively, and d is the offset of the vehicle IMU sensor plane relative to the origin of coordinates thereof;
step 3.2.2, defining a vertical vector L in a vehicle coordinate system, wherein the vertical vector L in a default vehicle coordinate system is (0, 1), and rotating normal vectors a, b and c of a vehicle IMU plane through the vertical vector L to obtain a rotation matrix m r ；
Step 3.2.3 according to the rotation matrix m r Pair P of point cloud rotation functions t Processing to obtain a rotated point cloud, and setting the z-axis height value of the rotated point cloud to 0 to obtain a horizontally calibrated point cloud P '' t 。
Step 3.3, the point cloud P 'after horizontal calibration' t Radius filtering to obtain accurate point cloud P t ：
Step 3.3.1, selecting the point cloud P 'after horizontal calibration' t Any one point of the two points is marked as a point A, the point A is used as a circle center, a distance threshold is used as a radius, the distance threshold is set to be 0.1m by default, a filter circle is formed, whether at least two points are contained in the filter circle is judged, and if the at least two points are contained in the filter circle, the point A is reserved; otherwise, deleting the point A;
step 3.3.2, repeatedly executing the step 3.3.1 until the point cloud P' t All the points in the map are processed until the processing is completed, so that all the reserved points form an accurate point cloud P t 。
Step 4: fusing data of an IMU sensor and an odometer, estimating the pose of the vehicle at the time t+1 and generating a global parking lot map;
step 4.1, calculating the IMU estimated vehicle acceleration at the time t by using the method (1)And vehicle angular velocity>
In the formula (1), i represents an IMU sensor coordinate system,vehicle acceleration indicative of IMU sensor output at time t,/->Gaussian white noise indicative of the acceleration of the vehicle at time t, < >>Random walk noise indicative of vehicle acceleration at time t,/->The angular velocity of the vehicle output by the IMU sensor at the moment t is represented;Gaussian white noise indicative of the angular velocity of the vehicle at time t,/->Random walk noise indicating the angular velocity of the vehicle at time t.
Step 4.2, calculating the IMU estimated vehicle speed at time t+1 using (2)And vehicle angle->
In the formula (2), delta t Represents the difference between time t and time t+1, delta v Indicating the amount of change in speed, delta θ Indicating the amount of change in angle;and->Respectively representing the estimated vehicle speed and the estimated vehicle angle of the IMU at the time t; when t=0, let +.>And->All are zero.
Step 4.3, calculating the estimated vehicle speed of the mileometer at the time t+1 by using the step (3)And vehicle angle->
In the formula (3), o represents an odometer coordinate system, v r,t+1 And v l,t+1 Respectively representing the right wheel speed and the left wheel speed, s of the output of the mileage meter at the time t+1 r,t+1 Sum s l,t+1 Respectively represent delta t Inner right wheel mileage and left wheel mileageIn the process, len is the width of the vehicle body,the vehicle angle estimated by the mileometer at the moment t; when t=0, let +.>Zero.
Step 4.3, estimating the vehicle speed mu after the fusion at the time t+1 by using the step (4) +1 And a vehicle angle theta t+1 ：
In the formula (4), mu 1 Represents the weight, mu corresponding to the IMU sensor 2 Representing the weight corresponding to the odometer; mu is set when the vehicle is straight 1 ＜μ 2 The method comprises the steps of carrying out a first treatment on the surface of the Mu is set when the vehicle turns 1 ＞μ 2 The method comprises the steps of carrying out a first treatment on the surface of the Default setting mu when the vehicle is straight 1 30%, mu 2 70%. Mu when the vehicle turns 1 70%, mu 2 30%.
4.4, taking the initial position of the vehicle as the origin of coordinates, taking the direction of the vehicle as the positive direction of the Y axis, rotating the direction of the vehicle clockwise by 90 degrees along the plane of the vehicle as the positive direction of the X axis, and establishing a two-dimensional map coordinate system;
define the pose W of the vehicle at the time t t ＝(X t ,Y t ,V t ) Wherein X is t ,Y t For the two-dimensional coordinates of the vehicle at time t, V t The included angle between the direction of the vehicle and the positive direction of the X axis at the time t; let W when t=0 0 ＝(0,0,90°)。
Step 4.5, calculating the pose W of the vehicle at the time t+1 by using the step (5) t+1 ＝(X t+1 ,Y t+1 ,V t+1 )：
In the formula (5), the amino acid sequence of the compound,X t+1 and Y t+1 Representing the two-dimensional coordinates of the vehicle at time t+1, V t+1 The angle between the vehicle orientation and the positive X-axis direction at time t+1 is shown.
Step 4.6, pose W at time t t Adding accurate point cloud information P t To form a local point cloud map M at time t p,t The method comprises the steps of carrying out a first treatment on the surface of the According to the pose W at time t+1 t+1 Accurate point cloud information P' at time t+1 is calculated by using normal distribution transformation algorithm t+1 And M is as follows p,t A rotation matrix R therebetween t+1 Translation matrix T t+1 ：
Step 4.6.1, mapping the local point cloud M at the time t p,t The point cloud in the map is subjected to downsampling processing by using a pcl VoxelGrid function to obtain a local point cloud map M' processed at the moment t. p,t ；
Step 4.6.2, M' p,t Discretizing into a three-dimensional grid, and setting the resolution of the grid to be 1m by default. And calculating the mean value and covariance matrix of point clouds in each grid to obtain a Gaussian distribution GD 1 ；
Step 4.6.3, accurate point cloud information P' at time t+1 according to step 4.6.1 and step 4.6.2 t+1 Processing to obtain another Gaussian distribution GD 2 ；
Step 4.6.4, calculate GD 1 And GD 2 KL divergence between the two, minimizing KL divergence by a gradient descent method, setting the iteration step length of the gradient descent method to be 0.01m by default and the maximum iteration number to be 50 times, thereby obtaining the optimal rotation matrix R t+1 Translation matrix T t+1 。
Point cloud P', at time t+1, using equation (6) t+1 Performing transformation operation to obtain a local point cloud map M at the time t+1 p,t+1 Further obtain the global map M at time t+1 g,t+1 ＝M p,t +M p,t+1 ：
M p,t+1 ＝R t+1 *P″ t+1 +T t+1 (6)
Step 5: optimizing the global map through loop detection, and generating a grid map for path navigation planning;
step 5.1, obtaining an accurate point cloud set P "= { P" from the initial time to the t time " m M=0, 1,2,3, … t }, where P "is" m Representing the exact point cloud at the mth moment.
Step 5.2, performing loop detection on the accurate point cloud set P' at the time t+1:
step 5.2.1, rapidly searching the accurate point cloud P 'between the P' and the t+1 moment by using a Kd-tree algorithm t+1 Accurate point cloud P' with maximum similarity value k ，P″ k ∈P”,0≤k≤t；
Step 5.2.2, calculating the accurate point cloud P' at the time t+1 t+1 With P k Degree of similarity S of (2) t+1 ；
Step 5.2.3, calculating the accurate point cloud P' at the time t+1 by using an iterative nearest point algorithm t+1 With P k Euclidean distance d between t+1 The method comprises the steps of carrying out a first treatment on the surface of the The similarity degree threshold is set to 90% by default, and the euclidean distance threshold is set to 0.03m. When S is t+1 Is greater than a set similarity threshold, and d t+1 If the distance is smaller than the set Euclidean distance threshold, the vehicle returns to the previous historical position, and the step 5.2.4 is executed, otherwise, the vehicle is not in a loop state, and the updated global map M 'at the time of t+1 is obtained' g,t+1 ＝M g,t+1 。
Step 5.2.4, the pose W at the time t+1 is subjected to nonlinear optimization algorithm t+1 Optimizing to minimize the accurate point cloud P' at the time t+1 t+1 With P k The error between the two is obtained, so that the pose W 'after the optimization at the time of t+1 is obtained' t+1 ；
According to W' t+1 For a local point cloud map M at time t+1 p,t+1 Updating to obtain a local point cloud map M 'updated at the time t+1' p,t+1 Thereby obtaining the global map M 'updated at the time t+1' g,t+1 ＝M p,t +M' p,t+1 As shown in fig. 2.
Step 5.3, updating the global map M 'after t+1 time' g,t+1 Converting to grid map G at time t+1 t+1 ：
Step 5.3.1 for grid map G t+1 Initializing, including: grid map G with t+1 time set t+1 According to M' g,t+1 Boundary-computing grid map G of (1) t+1 Is provided with G t+1 The origin of (2) is also the initial position of the vehicle;
step 5.3.2, p M' g,t+1 After radius filtering and downsampling processing, a lightweight global map M' at the time t+1 is obtained g,t+1 ；
Step 5.3.3, traversing M g,t+1 The point clouds in the map are mapped to the grid map G one by one t+1 The probability of the occupied position in the grid map is marked as 100, and the probability of the unoccupied position is marked as 0, so that the final grid map G at the time t+1 is obtained t+1 And after being stored, the system is used for full-automatic parking of the vehicle.
In this embodiment, an electronic device includes a memory for storing a program supporting the processor to execute the above method, and a processor configured to execute the program stored in the memory.
In this embodiment, a computer-readable storage medium stores a computer program that, when executed by a processor, performs the steps of the method described above.
Claims (8)
1. The full-automatic parking positioning and mapping method based on the looking-around vision is characterized by comprising the following steps of:
step 1: in the moving process of the vehicle, after the fisheye cameras arranged at the front, the back, the left and the right of the vehicle acquire the looking-around image at the current time t, the looking-down image I at the time t is obtained through the operations of camera distortion removal, inverse perspective transformation and image stitching t ；
Step 2: extracting the top view image I by adopting deep Lab V3+ model of convolutional neural network t The lane lines and the obstacle information in the road are marked to obtain an image I 'with the lane lines and the obstacle information' t ；
Step 3: performing point cloud conversion on the extracted lane lines and the obstacle information;
step 3.1, step I' t The pixel points corresponding to the lane lines and the obstacle information in the road are converted into point clouds P t ；
Step 3.2, vehicle IMU sensor point cloud P according to t moment t Performing horizontal calibration to obtain point cloud P 'after horizontal calibration' t ；
Step 3.3, the point cloud P 'after horizontal calibration' t Radius filtering to obtain accurate point cloud P t ；
Step 4: fusing data of an IMU sensor and an odometer, estimating the pose of the vehicle at the time t+1 and generating a global parking lot map;
step 4.1, calculating the IMU estimated vehicle acceleration at the time t by using the method (1)And vehicle angular velocity>
In the formula (1), i represents an IMU sensor coordinate system,vehicle acceleration indicative of IMU sensor output at time t,/->Gaussian white noise indicative of the acceleration of the vehicle at time t, < >>Random walk noise indicative of vehicle acceleration at time t,/->The angular velocity of the vehicle output by the IMU sensor at the moment t is represented;Gaussian white noise indicative of the angular velocity of the vehicle at time t,/->Random walk noise representing the angular velocity of the vehicle at time t;
step 4.2, calculating the IMU estimated vehicle speed at time t+1 using (2)And vehicle angle->
In the formula (2), delta t Represents the difference between time t and time t+1, delta v Indicating the amount of change in speed, delta θ Indicating the amount of change in angle;and->Respectively representing the estimated vehicle speed and the estimated vehicle angle of the IMU at the time t; when t=0, let +.>And->All are zero;
step 4.3, calculating t+1 using formula (3)Vehicle speed estimated by time odometerAnd vehicle angle->
In the formula (3), o represents an odometer coordinate system, v r,t+1 And v l,t+1 Respectively representing the right wheel speed and the left wheel speed, s of the output of the mileage meter at the time t+1 r,t+1 Sum s l,t+1 Respectively represent delta t Inner right and left wheel mileage, len is the vehicle body width,the vehicle angle estimated by the mileometer at the moment t; when t=0, let +.>Zero;
step 4.3, estimating the vehicle speed v after the fusion at the time t+1 by using the step (4) t+1 And a vehicle angle theta t+1 ：
In the formula (4), mu 1 Represents the weight, mu corresponding to the IMU sensor 2 Representing the weight corresponding to the odometer; mu is set when the vehicle is straight 1 ＜μ 2 The method comprises the steps of carrying out a first treatment on the surface of the Mu is set when the vehicle turns 1 ＞μ 2 ；
4.4, taking the initial position of the vehicle as the origin of coordinates, taking the direction of the vehicle as the positive direction of the Y axis, rotating the direction of the vehicle clockwise by 90 degrees along the plane of the vehicle as the positive direction of the X axis, and establishing a two-dimensional map coordinate system;
define the pose W of the vehicle at the time t t ＝(X t ,Y t ,V t ) Wherein X is t ,Y t For the two-dimensional coordinates of the vehicle at time t, V t The included angle between the direction of the vehicle and the positive direction of the X axis at the time t; let W when t=0 0 ＝(0,0,90°)；
Step 4.5, calculating the pose W of the vehicle at the time t+1 by using the step (5) t+1 ＝(X t+1 ,Y t+1 ,V t+1 )：
In the formula (5), X t+1 And Y t+1 Representing the two-dimensional coordinates of the vehicle at time t+1, V t+1 The included angle between the vehicle orientation and the positive X-axis direction of the vehicle at the time t+1 is shown;
step 4.6, pose W at time t t Adding accurate point cloud information P t To form a local point cloud map M at time t p,t The method comprises the steps of carrying out a first treatment on the surface of the According to the pose W at time t+1 t+1 Accurate point cloud information P' at time t+1 is calculated by using normal distribution transformation algorithm t+1 And M is as follows p,t A rotation matrix R therebetween t+1 Translation matrix T t+1 Thereby utilizing the formula (6) to obtain the point cloud P' at the time t+1 t+1 Performing transformation operation to obtain a local point cloud map M at the time t+1 p,t+1 Further obtain the global map M at time t+1 g,t+1 ＝M p,t +M p,t+1 ：
M p,t+1 ＝R t+1 *P″ t+1 +T t+1 (6)
Step 5: optimizing the global map through loop detection, and generating a grid map for path navigation planning;
step 5.1, obtaining an accurate point cloud set P "= { P" from the initial time to the t time " m M=0, 1,2,3, … t }, where P "is" m Representing an accurate point cloud at the mth moment;
step 5.2, the accurate point cloud set P' is advanced at the time t+1Performing loop detection to obtain a global map M 'updated at time t+1' g,t+1 ：
Step 5.3, updating the global map M 'after t+1 time' g,t+1 Converting to grid map G at time t+1 t+1 And after being stored, the system is used for full-automatic parking of the vehicle.
2. The full-automatic round-the-clock vision based parking positioning and mapping method according to claim 1, wherein in step 3.2, the vehicle IMU sensor points to the cloud P according to the time t t Performing horizontal calibration to obtain point cloud P 'after horizontal calibration' t . Comprising the following steps:
step 3.2.1, defining a plane equation of the vehicle IMU sensor as follows: ax+by+cz+d=0, wherein x, y and z are x-axis, y-axis and z-axis of the vehicle IMU sensor plane respectively, a, b and c are normal vectors of x, y and z respectively, and d is the offset of the vehicle IMU sensor plane relative to the origin of coordinates thereof;
step 3.2.2, defining a vertical vector in a vehicle coordinate system as L, and rotating normal vectors a, b and c of the vehicle IMU plane through the vertical vector L to obtain a rotation matrix m r ；
Step 3.2.3, according to the rotation matrix m r Pair P of point cloud rotation functions t Processing to obtain a rotated point cloud, and setting the z-axis height value of the rotated point cloud to 0 to obtain a horizontally calibrated point cloud P '' t 。
3. The full-automatic parking positioning and mapping method based on the looking-around vision as claimed in claim 2, wherein the horizontal calibrated point cloud P 'is in step 3.3' t Radius filtering to obtain accurate point cloud P t Comprising:
step 3.3.1, selecting the point cloud P 'after the horizontal calibration' t Any one point of the two points is marked as a point A, the point A is used as a circle center, a distance threshold is used as a radius, a filter circle is formed, whether at least two points are contained in the filter circle is judged, and if the at least two points are contained in the filter circle, the point A is reserved; otherwise, delete point A；
Step 3.3.2, repeatedly executing the step 3.3.1 until the point cloud P' t All the points in the map are processed until the processing is completed, so that all the reserved points form an accurate point cloud P t 。
4. The full-automatic parking positioning and mapping method based on the look-around vision as claimed in claim 3, wherein the normal distribution transformation algorithm in the step 4.6 is performed as follows:
step 4.6.1, mapping the local point cloud M at the time t p,t After the down-sampling treatment is carried out on the point cloud in the map, a local point cloud map M 'after the treatment at the moment t is obtained' p,t ；
Step 4.6.2, M' p,t Discretizing into three-dimensional grids, and calculating the mean value and covariance matrix of point clouds in each grid to obtain a Gaussian distribution GD 1 ；
Step 4.6.3, accurate point cloud information P' at time t+1 according to step 4.6.1 and step 4.6.2 t+1 Processing to obtain another Gaussian distribution GD 2 ；
Step 4.6.4, calculate GD 1 And GD 2 KL divergence between the two, and minimizing KL divergence by gradient descent method, thereby obtaining optimal rotation matrix R t+1 Translation matrix T t+1 。
5. The full-automatic parking positioning and mapping method based on the look-around vision as claimed in claim 4, wherein the step 5.2 is performed as follows:
step 5.2.1, rapidly searching the accurate point cloud P 'between the P' and the t+1 moment by using a Kd-tree algorithm t+1 Accurate point cloud P' with maximum similarity value k ，P″ k ∈P”,0≤k≤t；
Step 5.2.2, calculating the accurate point cloud P' at the time t+1 t+1 With P k Degree of similarity S of (2) t+1 ；
Step 5.2.3, calculating the accurate point cloud P' at the time t+1 by using an iterative nearest point algorithm t+1 With P k Euclidean distance d between t+1 The method comprises the steps of carrying out a first treatment on the surface of the When S is t+1 Is greater than a set similarity threshold, and d t+1 If the distance is smaller than the set Euclidean distance threshold, the vehicle returns to the previous historical position, and the step 5.2.4 is executed, otherwise, the vehicle is not in a loop state, and the updated global map M 'at the time of t+1 is obtained' g,t+1 ＝M g,t+1 ；
Step 5.2.4, the pose W at the time t+1 is subjected to nonlinear optimization algorithm t+1 Optimizing to minimize the accurate point cloud P' at the time t+1 t+1 With P k The error between the two is obtained, so that the pose W 'after the optimization at the time of t+1 is obtained' t+1 ；
According to W' t+1 For a local point cloud map M at time t+1 p,t+1 Updating to obtain a local point cloud map M 'updated at the time t+1' p,t+1 Thereby obtaining the global map M 'updated at the time t+1' g,t+1 ＝M p,t +M' p,t+1 。
6. The full-automatic parking positioning and mapping method based on the look-around vision as claimed in claim 5, wherein the step 5.3 is performed as follows:
step 5.3.1 for grid map G t+1 Initializing, including: grid map G with t+1 time set t+1 According to M' g,t+1 Boundary-computing grid map G of (1) t+1 Is provided with G t+1 The origin of (2) is also the initial position of the vehicle;
step 5.3.2, p M' g,t+1 After radius filtering and downsampling processing, a lightweight global map M' at the time t+1 is obtained g,t+1 ；
Step 5.3.3, traversing M g,t+1 The point clouds in the map are mapped to the grid map G one by one t+1 The probability of the occupied position in the grid map is marked as 100, and the probability of the unoccupied position is marked as 0, so that the final grid map G at the time t+1 is obtained t+1 。
7. An electronic device comprising a memory and a processor, wherein the memory is configured to store a program that supports the processor to perform the full-automatic park locating and mapping method of any of claims 1-6, the processor being configured to execute the program stored in the memory.
8. A computer readable storage medium having a computer program stored thereon, wherein the computer program when executed by a processor performs the steps of the full-automatic park locating and mapping method of any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311830221.7A CN117782102A (en) | 2023-12-28 | 2023-12-28 | A fully automatic parking positioning and mapping method based on surround vision |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311830221.7A CN117782102A (en) | 2023-12-28 | 2023-12-28 | A fully automatic parking positioning and mapping method based on surround vision |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117782102A true CN117782102A (en) | 2024-03-29 |
Family
ID=90384762
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311830221.7A Pending CN117782102A (en) | 2023-12-28 | 2023-12-28 | A fully automatic parking positioning and mapping method based on surround vision |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117782102A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118864303A (en) * | 2024-09-26 | 2024-10-29 | 合肥工业大学 | A dynamic object removal method based on average blur |
| CN120368929A (en) * | 2025-05-08 | 2025-07-25 | 浙江科维检测认证有限公司 | Measuring device for quick measurement of desktop horizontal reference |
-
2023
- 2023-12-28 CN CN202311830221.7A patent/CN117782102A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118864303A (en) * | 2024-09-26 | 2024-10-29 | 合肥工业大学 | A dynamic object removal method based on average blur |
| CN120368929A (en) * | 2025-05-08 | 2025-07-25 | 浙江科维检测认证有限公司 | Measuring device for quick measurement of desktop horizontal reference |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114526745B (en) | Drawing construction method and system for tightly coupled laser radar and inertial odometer | |
| EP3842755B1 (en) | Lane line positioning method and device, storage medium and electronic device | |
| CN107246876B (en) | Method and system for autonomous positioning and map construction of unmanned automobile | |
| CN111263960B (en) | Apparatus and method for updating high definition maps | |
| CN109166140B (en) | A method and system for vehicle trajectory estimation based on multi-line lidar | |
| CN114677663B (en) | Vehicle positioning method, device, electronic device, and computer-readable storage medium | |
| CN109443348B (en) | Underground garage position tracking method based on fusion of look-around vision and inertial navigation | |
| US11158065B2 (en) | Localization of a mobile unit by means of a multi hypothesis kalman filter method | |
| CN111862673A (en) | Vehicle self-localization and map construction method in parking lot based on top view | |
| CN113920198B (en) | Coarse-to-fine multi-sensor fusion positioning method based on semantic edge alignment | |
| CN117782102A (en) | A fully automatic parking positioning and mapping method based on surround vision | |
| CN115683128B (en) | Port unmanned integrated card visual characteristic matching and positioning method | |
| WO2023065342A1 (en) | Vehicle, vehicle positioning method and apparatus, device, and computer-readable storage medium | |
| CN113554705A (en) | A robust localization method for lidar in changing scenes | |
| Mounier et al. | Lidar-based multisensor fusion with 3-d digital maps for high-precision positioning | |
| CN114427863A (en) | Vehicle positioning method and system, automatic parking method and system, and storage medium | |
| CN118279468A (en) | Lane marking method, electronic device and computer storage medium | |
| CN116858269A (en) | Tobacco industry finished product warehouse flat warehouse inventory robot path optimization method based on laser SLAM | |
| CN111521996A (en) | A method of installation and calibration of lidar | |
| CN118408545A (en) | An air-ground collaborative intelligent assisted driving navigation method and collaborative system | |
| CN117705145A (en) | Laser inertial odometer method for local geometric information fusion | |
| JP7337617B2 (en) | Estimation device, estimation method and program | |
| CN117075158A (en) | Position and orientation estimation method and system of unmanned deformable motion platform based on lidar | |
| CN114563006B (en) | Vehicle global positioning method and device based on reference line matching | |
| CN113869203B (en) | Vehicle positioning method and system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |