A vehicle positioning method in the highway tunnel environment

Abstract

The present invention proposes a vehicle positioning method based on the combination of in-vehicle sensor data and auxiliary maps to achieve vehicle positioning in a highway tunnel environment. This method directly registers the original point cloud onto the local map through a fast and direct lidar inertial odometer and updates the local map, and uses the subtle features in the tunnel environment to improve the positioning accuracy, thereby achieving vehicle positioning when traffic signs are unavailable in the tunnel environment. For sections with traffic signs, the point cloud data needs to be made into a traffic sign map, and a threshold for the size of the sign map is set. Once the detected sign map exceeds the set threshold, the pre-made traffic sign map can be used to achieve vehicle positioning. Finally, the positioning results of the inertial odometer are fused with the positioning data obtained based on the auxiliary map to improve the positioning accuracy and stability of the vehicle in the entire highway tunnel environment.

Description

Technical Field

[0001] The present invention relates to a vehicle positioning method in a highway tunnel environment, belonging to the technical field of vehicle positioning. Background Art

[0002] With the continuous development of science and technology, the intelligent driving technology of automobiles has been significantly improved. As one of the most important components of intelligent driving technology, the requirements for vehicle positioning are also increasing day by day. Currently, the technologies relied on for vehicle positioning mainly include satellite navigation systems (such as GPS positioning technology), inertial navigation systems, high-precision maps, and vehicle-mounted sensors, etc.

[0003] In a signal shielding environment such as a tunnel, due to the existence of multipath effects, the GPS positioning technology will cause large errors in the positioning results. And the vehicle-mounted inertial navigation system will drift during long-term operation in the tunnel, resulting in a decrease in positioning accuracy. In addition, the environment around the tunnel is mainly composed of elliptical walls, with a uniform and single structure, and there are very few geometric structure features available for matching, resulting in unreliable or even invalid feature matching. For vehicle positioning based on vehicle-mounted sensors and map-assisted positioning, it is very difficult to achieve vehicle positioning in the tunnel. The above problems make vehicle positioning in highway tunnels a key problem in autonomous vehicle navigation. The method proposed in this paper uses the original point cloud for matching registration without feature extraction, makes full use of the subtle physical signs existing in the tunnel, solves the problem of unreliable feature matching caused by few geometric features, and at the same time uses a pre-made map for assisted positioning. This method can improve the positioning accuracy and stability of vehicles in highway tunnels, and brings an innovative solution to the vehicle positioning problem in the tunnel. Summary of the Invention

[0004] Object of the Invention: Aiming at the deficiencies in the prior art, the present invention provides a vehicle positioning method in a highway tunnel environment. This method directly registers the original point cloud to the local map through a fast and direct lidar inertial odometer, and then updates the map without feature extraction. In this way, the subtle features in the tunnel environment can be used to improve the positioning accuracy, so as to achieve vehicle positioning when there are no traffic signs or the traffic signs are blurred and unavailable in the tunnel environment. For sections with traffic signs, the point cloud data is made into a traffic sign map and the threshold of the sign map size is set. When the detected sign map is larger than the threshold, the pre-made traffic sign map is used to complete vehicle positioning. This can ensure that the currently detected traffic signs can be accurately matched with the pre-made map, thereby improving the positioning accuracy based on the traffic sign map. Finally, the positioning result of the inertial odometer is combined with the positioning result obtained based on the auxiliary map, so as to achieve vehicle positioning in the tunnel environment.

[0005] Technical solution: A vehicle positioning method in a highway tunnel environment, comprising the following steps:

[0006] S1: Laser radar inertial odometer for state estimation:

[0007] S1.1: Derive the system model of the inertial odometer, which consists of a state transition model and a measurement model;

[0008] S1.2: Forward the measurement values of the IMU to obtain an initial state quantity and transmit the covariance of the corresponding error state quantity;

[0009] S1.3: Use the measurement model to calculate the residuals of each LiDAR feature point to the fitted plane of its five nearest points;

[0010] S1.4: Combine the prior distribution obtained by applying a prior Gaussian distribution to the unknown state with the transmitted state and covariance obtained by forwarding the IMU with the transformed measurement model to obtain the maximum a posteriori estimate (MAP) of the error state quantity and calculate the Kalman gain K, and then complete the iterative update;

[0011] S1.5: Use an incremental k-d tree, i.e., ikd-Tree, for local map maintenance;

[0012] S2: Positioning based on the traffic sign map:

[0013] S2.1: Extract traffic signs from the collected tunnel point cloud data and retain the three-dimensional coordinate information of the feature points to create a traffic sign map;

[0014] S2.2: When passing through the available sections of traffic signs, use the created traffic sign map for vehicle positioning;

[0015] S3: Combine the positioning results obtained by the inertial odometer with the positioning results based on the traffic sign map to achieve vehicle positioning in the highway tunnel environment.

[0016] Specifically, S1 is as follows:

[0017] The lidar inertial odometer adopts a tightly coupled iterative Kalman filter and uses ikd-Tree to organize the point cloud in the local map; To facilitate the explanation of state estimation, define operators and to parameterize the state error on the manifold with n dimensions:

[0018]

[0019] In formula (1), is the exponential map on SO(3), Log(·) is its logarithmic map. For the composite manifold i.e., the Cartesian product between its submanifold components, there is:

[0020]

[0021] The state transition model and measurement model in S1.1 are specifically as follows:

[0022] Denote the coordinate system of the IMU at the initial time as I and define it as the global coordinate system G. Let I T L =( I R L , I p L ) be the external parameters between the LiDAR and the IMU. Then the motion model of the IMU is:

[0023]

[0024] In formula (3), G p I and G R I represent the position and attitude of the IMU in the global coordinate system, G v I is the velocity of the IMU in the global coordinate system, G g is the gravity vector in the global coordinate system, a m and ω m are the acceleration and angular velocity measured by the IMU, η a and ·η ω are the white noise of the IMU measurement values, b a and b ω are the biases of the IMU. (a) ∧ represents the skew-symmetric matrix of the vector ;

[0025] Define the subscript i as the index of the IMU measurement. The continuous motion model (3) is discretized with the IMU sampling period Δt, and the state transition model can be obtained as:

[0026]

[0027] In formula (4), the function f, state x, input u, and noise w are defined as follows:

[0028]

[0029] In the measurement model, lidar usually samples in a point-by-point manner; therefore, when the lidar undergoes continuous movement during the sampling period, the obtained points will correspond to different poses, which will lead to motion distortion; to correct this motion during scanning, i.e., motion compensation, a backpropagation method is adopted. This method estimates the LiDAR pose of each point in the scan relative to the pose at the end of the scan based on the IMU measurements. The estimated relative pose can project all points to the end time of the scan according to the exact sampling time of each individual point in the scan. Therefore, all points in the scan can be regarded as being sampled simultaneously at the end of the scan;

[0030] Let the subscript k be the index of the LiDAR scan, and { L p j , j = 1, …, m} be the points sampled in the LiDAR local coordinate system L at the end of the k-th scan. The motion compensation formula for backpropagation is:

[0031]

[0032] In formula (6), is the estimated relative pose, is the local measurement of the LiDAR;

[0033] Due to the measurement noise of the lidar, each measurement point is usually affected by noise. Removing the noise can obtain the true point position in the LiDAR local coordinate system

[0034]

[0035] The above-mentioned true points are projected into the global coordinate system by using the corresponding LiDAR pose and the external parameter I T L and search for the five closest points to it in the map represented by the ikd-Tree. Then, use the found closest neighboring points to fit a local plane. The measurement model is:

[0036]

[0037] In formula (8), G u j is the normal vector of the corresponding plane, G q j is a point on the corresponding plane.

[0038] The specific content of S1.2 is as follows: Let the optimal estimate after the fusion of the (k - 1)-th LiDAR scan be The covariance matrix is The forward pass is performed when the IMU measurements arrive; by setting the process noise wi is zero, and the state and covariance are propagated according to Equation (4):

[0039]

[0040] Q in Equation (9) i is the covariance of the noise w i , and the calculation formulas of the matrices and are as follows:

[0041]

[0042] Propagate forward until reaching the new scan, that is, stop at the end time of the k-th time, where the propagated state and covariance are expressed as and

[0043] The residual calculation in the above S1.3 is specifically as follows: Let the estimated value of the state x k at the current iterative update be When k = 0, that is, before the first iteration, which is the predicted state quantity propagated in (9);

[0044] Convert each LiDAR measurement point to the global coordinate system

[0045] By expanding the measurement model equation (8) at the first-order approximation at , the following can be obtained:

[0046]

[0047] In Equation (11), is equivalent to is the Jacobian matrix of the measurement model h j (x k , L η j ) with respect to , and is the residual, that is, the distance between the global coordinate of the converted measurement point and the nearest plane in the map:

[0048]

[0049] And is the total measurement noise of the original LiDAR measurement noise, and the covariance is R j .

[0050] The above S1.4 is specifically as follows: The state and covariance propagated forward by the IMU impose a prior Gaussian distribution on the unknown state; specifically speaking, represents the covariance of the following error state variables, i.e., the covariance of the prior distribution:

[0051]

[0052] J in formula (13) κ is the partial derivative at; for the first iteration, there is J κ = I;

[0053] In addition to the above prior distribution, there is also a state distribution generated by the deformed measurement model equation (11):

[0054]

[0055] Combining the prior distribution in equation (13) with the deformed measurement model in equation (14), the maximum a posteriori estimate (MAP) that can be obtained is:

[0056]

[0057] In formula (15), for any invertible matrix A and vectors n and m of appropriate dimensions, m is the number of measurement points; substituting the prior linearization in (13) into (15) and optimizing gives a quadratic cost. To simplify the notation, let R = diag(R 1 ,…R m ), and Then this maximum a posteriori estimation problem can be solved by using an iterative Kalman filter, and the calculation is as follows:

[0058] K = (H T R -1 H + P -1 ) -1 H T R -1

[0059]

[0060] Then use the updated state to calculate the residuals and repeat this process until convergence (i.e., ); after convergence, the optimal state estimate and covariance are:

[0061]

[0062] State After the update is complete, the LiDAR points in the k-th scan ( L pj )All are transformed into the global coordinate system:

[0063]

[0064] Specifically, S1.5 is as follows:

[0065] The incremental k-d tree, i.e., ikd-Tree, not only has the efficient nearest neighbor search of the original k-d tree but also supports incremental map updates, including point insertion, tree downsampling, and point deletion, while performing dynamic rebalancing with the minimum computational cost. Therefore, ikd-Tree can effectively represent large-scale dense point cloud maps and achieve the maintenance of local maps.

[0066] Specifically, S2.1 is as follows:

[0067] The traffic signs available in highway tunnels include lane operation signs, exit signs, and fire signs. These traffic signs are extracted from the point cloud data through the neural network RangeNet++ combined with Euclidean distance clustering; then, the extracted traffic signs are made into a traffic sign map.

[0068] Specifically, S2.2 is as follows:

[0069] Set the area size of a complete traffic sign to 1, and set a detection threshold. The value range of the threshold is 0.65 - 0.8. When the vehicle detects a traffic sign and the area of the detected traffic sign is greater than the set threshold, register the pre-made traffic sign map with the currently obtained sign map, and then realize vehicle positioning in combination with the global pose of the traffic sign map.

[0070] Specifically, S3 is as follows:

[0071] The spacing between the selected traffic signs in the tunnel is greater than 50 meters; therefore, the positioning result based on the traffic sign map is used to reduce the cumulative error of the inertial lidar odometer and play an auxiliary positioning role.

[0072] Beneficial effects:

[0073] 1. Aiming at the problem of unreliable positioning of traditional methods in highway tunnels, the present invention proposes a method combining inertial odometry and map-assisted positioning, effectively solving the problem of unreliable positioning and improving the positioning accuracy of vehicles in tunnels.

[0074] 2. The present invention uses a fast and direct inertial odometer to directly register the original point cloud on the map and update the local map, solving the problem that there are few geometric features available for matching in tunnels and making full use of the subtle features in the environment, thereby improving the positioning accuracy.

[0075] 3. The present invention uses an incremental k-d tree (ikd-Tree) to maintain a local dense point cloud map, enabling the local map to be updated at the rate required by the inertial odometer and providing efficient nearest neighbor search.

[0076] 4. The present invention creates a map of traffic signs existing in the tunnel, enabling the vehicle to reduce the cumulative error of the inertial odometer through map-assisted positioning and further improving the accuracy of positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art. Obviously, the drawings in the following description are only the embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on the provided drawings.

[0078] Figure 1 It is the overall flowchart of the present invention.

[0079] Figure 2 It is a schematic diagram of the measurement model in the present invention.

[0080] Figure 3 It is a schematic diagram of the local map maintenance in the present invention.

[0081] Figure 4 It is a schematic diagram of the traffic signs in the selected tunnel of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0082] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of them. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0083] In the description of the present invention, it should be understood that the orientation or positional relationship indicated by the terms "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc. is based on the orientation or positional relationship shown in the drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of the present invention.

[0084] In the present invention, unless otherwise clearly specified and defined, the first feature being "above" or "below" the second feature may include direct contact between the first and second features, or may include the situation where the first and second features are not in direct contact but in contact through additional features therebetween. Moreover, the first feature being "above", "over" and "on top of" the second feature includes the first feature being directly above and obliquely above the second feature, or simply indicating that the horizontal height of the first feature is higher than that of the second feature. The first feature being "below", "beneath" and "under" the second feature includes the first feature being directly below and obliquely below the second feature, or simply indicating that the horizontal height of the first feature is less than that of the second feature.

[0085] A vehicle positioning method in a highway tunnel environment includes the following steps:

[0086] S1: The lidar inertial odometer realizes state estimation, specifically:

[0087] The lidar inertial odometer adopts a tightly coupled iterative Kalman filter and uses an ikd-Tree to organize the point cloud in the local map; to facilitate the explanation of state estimation, the operators and are defined to parameterize the state error on the manifold with n dimensions:

[0088]

[0089] In formula (1), is the exponential map on SO(3), Log(·) is its logarithmic map, for the composite manifold which is the Cartesian product between its submanifold components, there is:

[0090]

[0091] S1.1: Derive the system model of the inertial odometer, which consists of a state transition model and a measurement model;

[0092] The state transition model and the measurement model are specifically:

[0093] Denote the coordinate system of the IMU at the initial time as I and define it as the global coordinate system G. Let I T L =( I R L , I p L ) be the external parameters between the LiDAR and the IMU. Then the motion model of the IMU is:

[0094]

[0095] In formula (3),G p I and G R I represent the position and attitude of the IMU in the global coordinate system. G v I is the velocity of the IMU in the global coordinate system. G g is the gravity vector in the global coordinate system, a m and ω m are the measured acceleration and angular velocity of the IMU, η a and ·η ω are the white noise of the IMU measurement value, b a and b ω are the biases of the IMU, (a) ∧ represents the skew-symmetric matrix of the vector;

[0096] Define the subscript i as the index of the IMU measurement. The continuous motion model (3) is discretized with the IMU sampling period Δt, and the state transition model can be obtained as:

[0097]

[0098] In formula (4), the function f, state x, input u, and noise w are defined as follows:

[0099]

[0100] In the measurement model, lidar usually samples in a point-by-point manner; therefore, when the lidar experiences continuous motion during the sampling period, the obtained points will correspond to different poses, which will lead to motion distortion; to correct this motion in the scan, i.e., motion compensation, the backpropagation method is adopted. This method estimates the LiDAR pose of each point in the scan relative to the pose at the end of the scan based on the IMU measurement value. The estimated relative pose can project all points to the end time of the scan according to the exact sampling time of each individual point in the scan. Therefore, all points in the scan can be regarded as being sampled simultaneously at the end of the scan;

[0101] Let the subscript k be the index of the LiDAR scan, { L p j , j = 1, …, m} be the points sampled at the end of the k-th scan in the LiDAR local coordinate system L. The motion compensation formula for backpropagation is:

[0102]

[0103] In formula (6) is the estimated relative pose, is the local measurement value of the LiDAR;

[0104] Due to the measurement noise of the lidar, each measurement point is usually affected by noise. Removing the noise can obtain the true point position in the LiDAR local coordinate system

[0105]

[0106] The above true points are obtained by using the corresponding LiDAR pose and external parameters I T L Projected into the global coordinate system, search for the five points closest to it in the map represented by the ikd-Tree, and then use the found closest neighboring points to fit a local plane. The measurement model is:

[0107]

[0108] In formula (8), G u j is the normal vector of the corresponding plane, G q j is a point on the corresponding plane.

[0109] S1.2: The measurement value of the IMU is propagated forward to obtain an initial state quantity, and the covariance of the corresponding error state quantity is propagated; Let the optimal estimate after the k-1th LiDAR scan fusion be The covariance matrix is The forward propagation is performed when the IMU measurement arrives; By setting the process noise w i to zero, the state and covariance are propagated according to equation (4):

[0110]

[0111] In formula (9), Q i is the covariance of the noise w i , and the calculation formulas of the matrices and are as follows:

[0112]

[0113] The forward propagation stops until the new scan is reached, that is, the end time of the kth scan. The propagated state and covariance are expressed as and

[0114] S1.3: Calculate the residuals of each LiDAR feature point to the plane fitted by its five closest points using the measurement model;

[0115] The specific calculation of the residuals is as follows: Let the estimate of the state x k at the current iterative update be When k = 0, i.e., before the first iteration, which is the predicted state quantity passed in (9);

[0116] Convert each LiDAR measurement point to the global coordinate system

[0117] By performing a first-order approximation expansion of the measurement model equation (8) at the following can be obtained:

[0118]

[0119] In formula (11), is equivalent to is the Jacobian matrix of the measurement model h in formula (8) j (x k , L η j ) with respect to and is the residual, i.e., the distance between the global coordinate of the converted measurement point and the nearest plane in the map:

[0120]

[0121] While is the total measurement noise of the original LiDAR measurement noise with covariance R j .

[0122] S1.4: Combine the prior distribution obtained by imposing a prior Gaussian distribution on the unknown state with the transfer state and covariance obtained by passing the IMU forward and the transformed measurement model to obtain the maximum a posteriori estimate (MAP) of the error state quantity and calculate the Kalman gain K, and then complete the iterative update;

[0123] Specifically: The state and covariance passed forward by the IMU impose a prior Gaussian distribution on the unknown state; specifically, represents the covariance of the following error state quantity, i.e., the covariance of the prior distribution:

[0124]

[0125] In formula (13), J κ is the partial derivative of at; for the first iteration, there is J κ = I;

[0126] In addition to the above prior distribution, there is also a state distribution generated by the deformed measurement model equation (11):

[0127]

[0128] Combining the prior distribution in Equation (13) with the deformation measurement model in Equation (14), the maximum a posteriori estimate (MAP) that can be obtained is as follows:

[0129]

[0130] In Equation (15), For any invertible matrix A and vectors n and m of appropriate dimensions, where m is the number of measurement points; substituting the prior linearization in (13) into (15) and optimizing gives a quadratic cost. To simplify the notation, let R = diag(R 1 , … R m ), and Then this maximum a posteriori estimation problem can be solved by using an iterative Kalman filter, and the calculation is as follows:

[0131] K = (H T R -1 H + P -1 ) -1 H T R -1

[0132]

[0133] Then use the updated state to calculate the residuals and repeat this process until convergence (i.e., ); after convergence, the optimal state estimate and covariance are:

[0134]

[0135] State After the update is complete, the LiDAR points ([[]] L p j ) in the k-th scan are all transformed into the global coordinate system:

[0136]

[0137] Specifically, S1.5 is: an incremental k-d tree, i.e., ikd-Tree, not only has the efficient nearest neighbor search of the original k-d tree but also supports incremental map updates, including point insertion, tree downsampling, and point deletion, while performing dynamic rebalancing at the lowest computational cost. Therefore, ikd-Tree can effectively represent large and dense point cloud maps and achieve the maintenance of local maps, as shown in the appendix Figure 3 as shown

[0138] S2: Positioning based on the traffic sign map:

[0139] S2.1: Extract traffic signs from the collected tunnel point cloud data and retain the three-dimensional coordinate information of the feature points to create a traffic sign map. Specifically:

[0140] The available traffic signs in highway tunnels include lane operation signs, exit signs, and fire signs. These traffic signs are extracted from the point cloud data through the neural network RangeNet++ combined with Euclidean distance clustering. Then, the extracted traffic signs are made into a traffic sign map.

[0141] S2.2: When passing through the available sections of traffic signs, use the created traffic sign map for vehicle positioning. Specifically:

[0142] Set the area size of a complete traffic sign to 1 and set a detection threshold. The value range of the threshold is 0.65 - 0.8. When the vehicle detects a traffic sign and the area of the detected traffic sign is greater than the set threshold, register the previously created traffic sign map with the currently obtained sign map, and then combine the global pose of the traffic sign map to achieve vehicle positioning.

[0143] S3: Combine the positioning results obtained from the inertial odometer with the positioning results based on the traffic sign map to achieve vehicle positioning in the highway tunnel environment. Specifically:

[0144] The spacing between the selected traffic signs in the tunnel is greater than 50 meters. Therefore, use the positioning results based on the traffic sign map to reduce the cumulative error of the inertial lidar odometer and play an auxiliary positioning role.

[0145] The working method of this application is as follows:

[0146] Set the area size of a complete traffic sign to 1, and then set a detection threshold. The value range of the threshold is 0.65 - 0.8. The threshold selected in this embodiment is 0.7, and the threshold selection changes dynamically according to different highway tunnel conditions;

[0147] In the experimental stage, when the test vehicle performs a driving test simulating a tunnel once, according to the experimental data obtained by the test vehicle, match it with the previously created map to obtain a matching accuracy score. When the matching accuracy score is lower than 0.3, it proves that the data error value is within the qualified range, that is, the selected threshold is applicable to the current tunnel conditions. If it is higher than 0.3, it proves that the error is large, and the threshold needs to be adjusted for further testing until the matching accuracy score is lower than 0.3.

[0148] When the vehicle detects a traffic sign and the area of the detected traffic sign is smaller than the set threshold, the inertial odometer is used for state estimation as described in step S1. Since the sampling frequency of the lidar is very high, the radar frequency setting value in this application is: 10 Hz. Before performing state estimation, the received points are accumulated for a certain time, and then the collected point cloud data is processed at one time. The time set in this application is: 10 milliseconds to 100 milliseconds. The accumulated point cloud is called a single scan and sent to the vehicle-mounted computing system. While the LiDAR (radar) collects data, the IMU (inertial measurement unit) also measures the gyroscope angular velocity value and accelerometer acceleration value of the moving vehicle in real time and sends the measured values to the vehicle-mounted computing system.

[0149] Then the vehicle-mounted computing system processes the received data. First, a state transition model as described in step S1.1 is established based on the measured values of the IMU. Then, according to equation (4) of the state transition model, the state and covariance at the previous moment are propagated forward to estimate an initial state quantity and the covariance of the propagated error state quantity. The estimated initial state quantity is used for the subsequent backward propagation, i.e., motion compensation, and corrects the motion distortion generated during the LiDAR sampling process according to equation (6). Next, the measurement noise of the LiDAR is removed according to equation (7) to obtain the true position of the measurement point in the LiDAR coordinate system. After projecting these true points into the global coordinate system, the measurement model in S1.1 is obtained, as shown in the appendix Figure 2 as shown.

[0150] After obtaining the measurement model, the residual as described in step S1.3 is calculated using the local map maintained by the ikd-Tree according to equation (12). The calculated residual can deduce the state distribution generated by the measurement model according to equation (11). The prior distribution of the error state is obtained according to equation (13) from the state and covariance propagated forward by the IMU. The obtained prior distribution is combined with the state distribution generated by the measurement model to obtain the maximum a posteriori estimate (MAP) of the error state. Then, an iterative Kalman filter is used to solve this maximum a posteriori estimate. First, the Kalman gain K is calculated according to equation (16) and then iteratively updated. When the convergence condition is reached, the optimal state estimate is obtained, and the iterative update as described in step S1.4 is completed.

[0151] After state update, all the LiDAR points in the scan are converted to the global coordinate system. The converted LiDAR points are inserted into the map maintained by the ikd-Tree at the rate of the inertial odometer, and the local map is dynamically updated. And step S1.5 is to prevent the local map from growing continuously and keep the size of the local map within a certain range. Therefore, the ikd-Tree is used to maintain the local map so that the local map only retains the point cloud within a range of length L around the current position of the LiDAR, as shown in the appendix Figure 3 as shown.

[0152] Then, when the vehicle detects a traffic sign in the tunnel and its size is within the threshold of 0.65-0.8, the traffic sign map prepared in step S2.1 is used for positioning. The traffic signs selected for making the traffic sign map include lane operation signs, exit signs, fire signs, pedestrian tunnel signs and tunnel lights, as shown in the attached figure. Figure 4 As shown. Among them, the lane operation signs, exit signs, and fire signs are spaced more than 50 meters apart in highway tunnels, and their number is less than that of pedestrian tunnel signs and tunnel lights. Therefore, they are special and can be used to make maps for auxiliary positioning. After using the traffic sign map to perform ICP matching with the currently acquired traffic signs, the current posture of the vehicle is calculated in combination with the global posture of the traffic sign map to complete the vehicle positioning described in step S2.2. Step S3 is to use the positioning result based on the traffic sign map as the initial posture of the next section of the inertial odometer, so as to reduce the cumulative error of the previous section of the odometer. The positioning result of the inertial odometer is then combined with the positioning result based on the traffic sign map, allowing the vehicle to be positioned and navigated in the highway tunnel.

[0153] In order to verify the method proposed in this invention, a simulated tunnel experimental scene is first established, and traffic signs of appropriate proportions are made and arranged in the simulated tunnel according to the installation positions in the tunnel, so as to simulate the environment in the highway tunnel as much as possible. An intelligent driving test car is used as the experimental data platform. The car is equipped with a velodyne 64-line laser radar, IMU and on-board computing system, which provides the required perception data for the experiment.

[0154] Before conducting specific experiments, the laser radar on the car is used to collect point cloud data of the entire simulated tunnel. The traffic signs in the point cloud data are extracted while retaining the three-dimensional coordinate information of the feature points. Then, a traffic sign map is made and the size threshold of the traffic sign map is set. This map is saved in the on-board computing system for subsequent auxiliary map positioning.

[0155] After completing the preparations, the experimental process of simulating the positioning of a vehicle in a tunnel environment is as follows: the car passes through the simulated tunnel at a certain speed. When no traffic signs are detected or the detected traffic signs are smaller than the threshold, the inertial odometer performs state estimation to obtain the current state of the vehicle, thereby calculating the vehicle posture to complete the positioning. When the car detects a traffic sign and the size is within the threshold range, the pre-made traffic sign map is used to perform point-to-surface ICP registration with the currently acquired sign map, and the rotation and translation matrices are calculated, and then the global posture of the traffic sign map is combined to achieve vehicle positioning. Finally, the positioning results of the inertial odometer are combined with the positioning results obtained based on the traffic sign map to achieve the positioning of the car in the entire simulated tunnel environment.

[0156] In the present specification, the various embodiments are described in a progressive manner. Each embodiment focuses on the differences from other embodiments. For the same or similar parts among the various embodiments, reference can be made to each other. For the apparatuses disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the description is relatively simple. For the relevant parts, reference can be made to the description in the method part.

[0157] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to those skilled in the art. The general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather will be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A vehicle positioning method in a highway tunnel environment, characterized in that: The following steps are involved: S1: LiDAR inertial odometer for state estimation: S1.1: Derive the system model of the inertial odometer, which consists of a state transition model and a measurement model; S1.2: The IMU measurement value is passed forward to obtain an initial state quantity, and the covariance of the corresponding error state quantity is passed; S1.3: Use the measurement model to calculate the residual error of each LiDAR feature point to the nearest five points of the fitting plane; S1.4: The prior distribution obtained by applying the prior Gaussian distribution to the unknown state by forwarding the IMU and the covariance obtained by the IMU is combined with the transformed measurement model to obtain the maximum a posteriori estimate (MAP) of the error state quantity and calculate the Kalman gain K, and then complete the iterative update; S1.5: Use incremental kd-tree, i.e. ikd-Tree, to maintain local maps; S2: Positioning based on traffic sign map: S2.1: Extract traffic signs from the collected tunnel point cloud data and retain the three-dimensional coordinate information of the feature points to create a traffic sign map; S2.2: When passing through a road section where traffic signs are available, use the prepared traffic sign map to locate the vehicle; S3: Combine the positioning result obtained by the inertial odometer with the positioning result based on the traffic sign map to realize the positioning of the vehicle in the highway tunnel environment.

2. The vehicle positioning method in a highway tunnel environment according to claim 1, characterized in that: The S1 is specifically: The lidar inertial odometer uses a tightly coupled iterative Kalman filter and uses ikd-Tree to organize the point cloud in the local map; in order to facilitate the interpretation of the state estimation, the operator is defined and To parameterize a manifold with n dimensions The state error on: In formula (1), is an exponential map on SO(3), Log(·) is its logarithmic map, and for composite manifolds That is, the Cartesian product between its submanifold components is:

3. The vehicle positioning method in a highway tunnel environment according to claim 2, characterized in that: The state transition model and measurement model in S1.1 are specifically: The initial coordinate system of the IMU is denoted as I, and it is defined as the global coordinate system G, I T L =( I R L , I p L ) is the external parameter between LiDAR and IMU, then the motion model of IMU is: In formula (3), G p I and G R I Represents the position and attitude of the IMU in the global coordinate system, G v I is the velocity of the IMU in the global coordinate system, G g is the gravity vector in the global coordinate system, a m and ω m is the acceleration and angular velocity measured by the IMU, η a and η ω is the white noise of IMU measurement, b a and b ω is the zero bias of IMU, (a) ∧ Representation vector The antisymmetric matrix of ; Subscript i is defined as the index of IMU measurement. The continuous motion model (3) is discretized with the IMU sampling period Δt, and the state transition model is obtained as follows: The function f, state x, input u, and noise w in formula (4) are defined as follows: In the measurement model, the LiDAR is usually sampled point by point; therefore, when the LiDAR undergoes continuous motion within the sampling period, the points obtained will correspond to different postures, which will cause motion distortion; in order to correct this motion during the scan, that is, motion compensation, a back-propagation method is used, which estimates the LiDAR pose of each point in the scan relative to the pose at the end of the scan based on the IMU measurement value. The estimated relative pose can project all points to the end time of the scan based on the precise sampling time of each individual point in the scan, so all points in the scan can be regarded as being sampled simultaneously at the end of the scan; Let subscript k be the index of the LiDAR scan, { L p j , j = 1, ..., m} is the point sampled in the LiDAR local coordinate system L at the end of the kth scan, and the motion compensation formula for back propagation is: In formula (6) is the estimated relative pose, is the local measurement value of LiDAR; Due to the measurement noise of the LiDAR, each measurement point is usually affected by the noise. Removing the noise can obtain the true point position in the LiDAR local coordinate system. The above ground points are obtained by using the corresponding LiDAR poses and external parameters I T L Project it to the global coordinate system and search for the five points closest to it in the map represented by the ikd-Tree. Then fit a local plane using the nearest neighboring points found. The measurement model is: In formula (8), G u j is the normal vector of the corresponding plane, G q j is a point on the corresponding plane.

4. The vehicle positioning method in a highway tunnel environment according to claim 3, characterized in that: S1.2 is specifically: Let k-1 times, the optimal estimate after LiDAR scanning fusion is The covariance matrix is The forward pass is performed as IMU measurements arrive; by setting the process noise w i is zero, the state and covariance are transferred according to formula (4): Q in formula (9) i is the noise w i The covariance matrix and F wi The calculation formula is as follows: The forward pass stops until a new scan is reached, i.e., the end time of the kth scan, where the pass state and covariance are expressed as and 5. The vehicle positioning method in a highway tunnel environment according to claim 4, characterized in that: The residual calculation in S1.3 is specifically as follows: Let the state x at the current iteration update k The estimate is When k = 0, that is, before the first iteration, That is, the predicted state quantity transmitted in (9); Transform each LiDAR measurement point to the global coordinate system By replacing the measurement model equation (8) with The first-order approximate expansion at can be obtained: In formula (11), Equivalent to is the measurement model h in equation (8) j (x k , L η j ) relative to The Jacobian matrix of is the residual, that is, the distance between the transformed global coordinates of the measurement point and the nearest plane in the map: and is the total measurement noise of the original laser radar measurement noise, and the covariance is R j .

6. The vehicle positioning method in a highway tunnel environment according to claim 5, characterized in that: The above S1.4 is specifically: the state transmitted forward by the IMU and covariance Apply a prior Gaussian distribution to the unknown state; specifically, represents the covariance of the following error state quantities, that is, the covariance of the prior distribution: In formula (13), J κ yes right The partial differential at ; for the first iteration, There is J κ =I; In addition to the above prior distribution, there is also a state distribution generated by the deformed measurement model equation (10): Combining the prior distribution in equation (13) with the deformation measurement model in equation (14), the maximum a posteriori estimate (MAP) can be obtained as: In formula (15), For any reversible matrix A and a vector n of appropriate dimension, m is the number of measurement points; substitute the prior linearization in (13) into (15) and optimize to obtain the quadratic cost. To simplify the notation, let R=diag(R1,…R m ), as well as Then the maximum a posteriori estimation problem can be solved by using an iterative Kalman filter, as follows: The updated state is then used to calculate the residual and the process is repeated until convergence (i.e. ); after convergence, the optimal state estimate and covariance are: state After the update is completed, the LiDAR point in the kth scan ( L p j ) are all transformed into the global coordinate system:

7. The vehicle positioning method in a highway tunnel environment according to claim 1, characterized in that: The S2.1 is specifically: Traffic signs available in highway tunnels include lane operation signs, exit signs and fire signs. The above traffic signs are extracted from point cloud data through the neural network RangeNet++ combined with Euclidean distance clustering; then, the extracted traffic signs are made into a traffic sign map.

8. The vehicle positioning method in a highway tunnel environment according to claim 7, characterized in that: The S2.2 is specifically: The size of a complete traffic sign area is set to 1, and a detection threshold is set. The threshold value range is 0.65-0.

8. When the vehicle detects a traffic sign and the detected traffic sign area is larger than the set threshold, the pre-made traffic sign map is used to align with the currently acquired sign map, and then the vehicle positioning is achieved in combination with the global posture of the traffic sign map.

9. The vehicle positioning method in a highway tunnel environment according to claim 8, characterized in that: The S3 is specifically: The spacing between the selected traffic signs in the tunnel is greater than 50 meters; therefore, the positioning results based on the traffic sign map are used to reduce the cumulative error of the inertial lidar odometer, which plays a role in auxiliary positioning.

Images