Urban driving vehicle continuous positioning system and method

Abstract

The application provides a continuous positioning system and method for a vehicle driving in a city. The method constructs a three-dimensional map of a city environment; obtains a relative pose transformation of adjacent two time points of the three-dimensional point cloud of the environment and a position of the vehicle in the three-dimensional point cloud of the environment at a current time point through registration processing; performs coordinate transformation processing in combination with the relative pose to obtain a matching position of the three-dimensional map of the city environment at the current time point; obtains a plurality of voxels of the three-dimensional point cloud of the environment at the current time point after screening through screening; calculates the weight of each three-dimensional point in each voxel of the three-dimensional point cloud of the environment at the current time point after screening, and further calculates a registration score; calculates a relative pose weight matrix at the current time point according to the registration score; obtains a fused position through a graph optimization method in combination with the relative pose weight matrix at the current time point, a GNSS weight and a weight of the matching position of the three-dimensional map of the environment; and further corrects the fused position to obtain a corrected relative pose. The application improves the positioning accuracy and availability in a complex city environment.

Description

Technical Field

[0001] This invention belongs to the field of multi-sensor fusion positioning technology, and particularly relates to a continuous positioning system and method for urban driving vehicles. Background Technology

[0002] Global Navigation Satellite System (GNSS) offers numerous advantages, including fast, efficient, time-saving, and high-accuracy positioning, making it widely used in outdoor positioning applications. However, GNSS signal propagation is easily hindered by the environment. In urban environments, the stability of GNSS signals is difficult to guarantee due to obstruction from man-made structures such as tall buildings and overpasses. GNSS signals are even harder to receive in environments like underground parking lots. Furthermore, reflections from glass curtain walls can cause multipath effects, further impacting positioning accuracy. Therefore, positioning in environments where GNSS is unavailable has become an urgent scientific problem that needs to be solved, leading to the continuous emergence of relevant methods and technological advancements.

[0003] Due to the rapid development of LiDAR manufacturing technology and its widespread application in autonomous driving, LiDAR-based positioning technology—LiDAR odometry—has become a research hotspot. This positioning method calculates the position and attitude changes of the vehicle based on two consecutive frames of LiDAR data, thus achieving continuous positioning. Simultaneously, this method can also be used as part of simultaneous localization and mapping (SMR) technology, constructing a 3D point cloud map of the surrounding environment based on the achieved positioning. However, LiDAR odometry is a recursive method for calculating position, which suffers from error accumulation. Compared to LiDAR odometry, which only provides relative position and attitude, LiDAR global positioning methods can provide the absolute position and attitude of the vehicle in a geographic coordinate system. LiDAR global positioning methods provide absolute position and attitude by matching LiDAR scan data with a pre-constructed 3D map. The matching between LiDAR scan data and the 3D map is calculated based on various descriptor operators, which can be manually defined or constructed through deep learning. Since single positioning methods often have significant limitations, modern positioning systems generally integrate multiple positioning technologies to achieve complementary accuracy and continuity. However, these fusion methods generally only integrate laser odometry and GNSS. When GNSS is unavailable for an extended period, its positioning accuracy will continue to decrease. On the one hand, it is necessary to find another absolute positioning method to play a certain role when GNSS is unavailable. On the other hand, it is necessary to study how to reduce the error divergence rate of laser odometry in order to ensure high accuracy and high availability of positioning in complex scenarios. Summary of the Invention

[0004] To address the issues of low positioning accuracy and poor usability of existing positioning technologies in complex urban environments, a continuous positioning system and method for urban driving vehicles is proposed.

[0005] The technical solution of the present invention is a continuous positioning system for urban driving vehicles, comprising: a three-dimensional lidar, a host computer, a GNSS positioning module, and a POS system;

[0006] The aforementioned three-dimensional lidar, GNSS positioning module, and POS system are installed on the top of the vehicle, while the host computer is installed inside the vehicle.

[0007] The host computer is connected in sequence to the three-dimensional lidar, the GNSS positioning module, and the POS system.

[0008] The three-dimensional lidar is used to collect environmental three-dimensional point clouds and scan line fields at multiple times, and transmit the environmental three-dimensional point clouds at multiple times to the host computer.

[0009] The GNSS positioning module is used to collect vehicle positions at multiple times and transmit the vehicle positions at multiple times to the host computer.

[0010] The POS system is used to collect vehicle poses at multiple times and transmit the vehicle poses at multiple times to the host computer.

[0011] The host computer constructs a 3D map of the urban environment; it registers the 3D point clouds of the environment at two adjacent time points to obtain the attitude transformation and translation at those two time points, constructs the relative pose transformation at those two time points, and further obtains the vehicle position in the 3D point cloud of the environment at the current time; it combines the relative pose with coordinate transformation to obtain the matching position of the 3D map of the urban environment at the current time; it obtains multiple voxels of the 3D point cloud of the environment at the current time through filtering; it calculates the weight of each 3D point in each voxel of the 3D point cloud of the environment at the current time; it further calculates the registration score and calculates the relative pose weight matrix of the current time based on the registration score; it combines the relative pose weight matrix of the current time, the GNSS weight of the current time, and the matching position weight of the 3D map of the environment at the current time to obtain the fused position through graph optimization; and it further corrects the position to obtain the corrected relative pose.

[0012] The technical solution of this invention is a continuous positioning method for urban driving vehicles, comprising the following steps:

[0013] Step 1: The host computer combines the 3D point cloud of the environment at each moment with the vehicle pose at the corresponding moment to construct a 3D map of the urban environment.

[0014] Step 2: The host computer registers the environmental 3D point clouds from two adjacent time points to obtain the pose transformation and translation of the two adjacent time points, constructs the relative pose transformation of the two adjacent time points, and further obtains the relative pose of the environmental 3D point cloud at the current time point and the environmental 3D point cloud at the first time point. The vehicle position at the current time point is combined with the relative pose point to perform coordinate transformation to obtain the vehicle position in the environmental 3D point cloud at the current time point.

[0015] Step 3: The current environmental 3D point cloud and the urban environmental 3D map are matched to obtain the vehicle position in the urban environmental 3D map at the current time. The relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time is combined to perform coordinate transformation to obtain the matching position in the urban environmental 3D map at the current time.

[0016] Step 4: Divide the current 3D point cloud of the environment into voxels to obtain multiple voxels of the current 3D point cloud of the environment. Combine the multiple voxels of the current 3D point cloud of the environment with a voxel point cloud threshold for filtering to obtain multiple voxels of the current 3D point cloud of the environment after filtering.

[0017] Step 5: After filtering each voxel of the current environmental 3D point cloud, use principal component analysis to obtain the linear eigenvalue, planar eigenvalue, and point eigenvalue of each voxel. Then, use the eigenvalue determination method to obtain the weight of each voxel of the current environmental 3D point cloud, and further obtain the weight of each 3D point in each voxel of the current environmental 3D point cloud.

[0018] Step 6: Combine the environmental 3D point cloud from the previous time step with the relative pose transformations of two adjacent time steps to obtain the environmental 3D point cloud after the previous time step transformation. Combine the environmental 3D point cloud after the previous time step transformation with the environmental 3D point cloud after the previous time step transformation and obtain the corresponding 3D point in the environmental 3D point cloud after the previous time step transformation of each voxel after the current time step transformation using the nearest neighbor search method. Calculate the residual of each 3D point in each voxel after the current time step transformation and further calculate the registration score. Calculate the relative pose weight matrix at the current time step based on the registration score.

[0019] Step 7: Calculate the GNSS weight of the vehicle position in the 3D point cloud at the current moment using the GNSS covariance matrix method. Calculate the environmental 3D map matching position weight at the current moment using the completeness determination method. Combine the relative pose weight matrix, the GNSS weight, and the environmental 3D map matching position weight at the current moment to construct a fused position function model using graph optimization. Minimize the fused position function model as the optimization objective and solve it using the Gauss-Newton method to obtain the fused position.

[0020] Step 8: Combine the fused position with the error correction model to estimate the cumulative pose transformation error, and further correct the relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time to obtain the corrected relative pose;

[0021] Preferably, the relative pose transformation between two adjacent moments in step 2 is defined as:

[0022]

[0023] i∈[2,K]

[0024] Among them, T i，i-1 R represents the relative pose transformation at time i relative to time i-1. i，i-1 This represents the attitude change at time i relative to time i-1. Let i represent the shift relative to time i-1, and K represent the number of time points.

[0025] Step 1 describes obtaining the relative pose transformation between the current environmental 3D point cloud and the environmental 3D point cloud at the first time step, as follows:

[0026] T i =T i,i-1 ·T i-1,i-2 ·...·T 2,1

[0027] Among them, T i This represents the relative pose transformation between the current environmental 3D point cloud and the environmental 3D point cloud at the first time step.

[0028] The vehicle position in the current 3D point cloud of the environment, as described in step 2, is specifically defined as follows:

[0029]

[0030] i∈[2,K]

[0031] in, Let x represent the vehicle's position in the 3D point cloud of the environment at time i.i The x-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i is y. i The z-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i represents the position of the vehicle at time i. i The Z-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i represents the position of the vehicle at time i, and K represents the number of time points.

[0032] As a preferred embodiment, the current location of the urban environment 3D map matching in step 3 is defined as follows:

[0033]

[0034] i∈[2,K]

[0035] in, Let x represent the location of the urban environment 3D map matching at time i. i The x-axis coordinate of the location matched on the 3D map of the urban environment at time i is y. i Let z represent the Y-axis coordinate of the urban environment 3D map matching location at time i. i Let Z represent the Z-axis coordinate of the urban environment 3D map matching location at time i, and K represent the number of time points.

[0036] Preferably, step 4 involves filtering multiple voxels of the current 3D point cloud environment based on a voxel point cloud threshold, as detailed below:

[0037] Voxels with a number of 3D points greater than the voxel point cloud threshold are selected from multiple voxels of the current environment 3D point cloud to construct multiple voxels of the current environment 3D point cloud as described in step 4.

[0038] Preferably, step 5 involves obtaining the weight of each voxel in the current moment's 3D point cloud of the environment after filtering using the eigenvalue determination method, as detailed below:

[0039] If the planar feature value of each voxel in the current environment 3D point cloud is the largest after filtering, then the weight of each voxel in the current environment 3D point cloud is 0.

[0040] If the point feature value of each voxel in the current environment 3D point cloud is the largest after filtering, then the weight of each voxel in the current environment 3D point cloud after filtering is 1.

[0041] If the linear eigenvalue of each voxel is maximized after filtering the 3D point cloud of the environment:

[0042] Based on the scan line field at the current moment, determine whether the points in the voxel come from the same scan line. If the 3D points of each voxel in the current environmental 3D point cloud come from the same scan line after filtering, the weight of each voxel in the current environmental 3D point cloud is 0.

[0043] Based on the scan line field at the current moment, determine whether the points in the voxel come from the same scan line. If the 3D points of each voxel in the current environment 3D point cloud come from different scan lines after filtering, then it is a true line feature in the environment. The weight of each voxel in the current environment 3D point cloud after filtering is 1.

[0044] The weight of each voxel in the current 3D point cloud of the environment is specifically defined as follows:

[0045]

[0046] i∈[2,K]

[0047] λ∈[1,V i ]

[0048] Among them, V i This represents the number of voxels in the 3D point cloud of the environment at time i after filtering. Let λ represent the weight of the λth voxel of the 3D point cloud of the environment at time i, and K represent the number of time points.

[0049] Step 5 further obtains the weight of each 3D point in each voxel of the current environment 3D point cloud after filtering, as follows:

[0050] The weight of each 3D point in each voxel of the current environment's 3D point cloud after filtering is the weight of the corresponding filtered voxel, specifically defined as:

[0051]

[0052] i∈[2,K],λ∈[1,V] i ]

[0053] Among them, V i This represents the number of voxels in the 3D point cloud of the environment at time i after filtering. denoted by λ, where r is the weight of the r-th 3D point in the λ-th voxel of the environmental 3D point cloud at time i, and K represents the number of time points.

[0054] Preferably, step 6 involves calculating the residual of each 3D point in each voxel of the current environment's 3D point cloud after filtering, using the following formula:

[0055]

[0056] i∈[2,K]

[0057] λ∈[1, V i ]

[0058] r∈[1, N] λ ]

[0059] in, This represents the residual of the r-th 3D point in the λ-th voxel after filtering the environmental 3D point cloud at time i. This represents the r-th 3D point in the λ-th voxel after filtering the environmental 3D point cloud at time i. N represents the corresponding point in the 3D point cloud of the environment at time i-1, where N is the r-th 3D point in the λ-th voxel after filtering at time i. λ V represents the number of 3D points in the λ-th voxel after filtering the 3D point cloud of the environment at time i. i Let i represent the number of voxels in the 3D point cloud of the environment after filtering, and K represent the number of time points.

[0060] Step 6 involves further calculating the registration score, as detailed below:

[0061]

[0062] i∈[2,K]

[0063] λ∈[1, V i ]

[0064] r∈[1, N] λ ]

[0065] in, Let represent the weight of the r-th 3D point in the λ-th voxel of the environmental 3D point cloud at time i. The score represents the residual of the r-th 3D point in the λ-th voxel after filtering the environmental 3D point cloud at time i. i N represents the score of the relative pose at time i. λ V represents the number of 3D points in the λ-th voxel after filtering the 3D point cloud of the environment at time i. i Let i represent the number of voxels in the 3D point cloud of the environment after filtering, and K represent the number of time points.

[0066] Step 6, which calculates the relative pose weight matrix at the current moment based on the registration score, is as follows:

[0067]

[0068] i∈[2,K]

[0069] Among them, Ω i The weight matrix representing the relative pose at time i, score iLet K be the relative pose registration score at time i, where I is the identity matrix and K represents the number of time points.

[0070] Preferably, the fused position described in step 7 is specifically defined as follows:

[0071]

[0072] i∈[2,Kl

[0073] in, Let x represent the fused position at time i. i The x-axis coordinate of the fused position at time i is represented by the y-axis coordinate. i The z-axis coordinate represents the fused position at time i. i The Z-axis coordinate represents the fused position at time i, and K represents the number of time points;

[0074] Preferably, step 8 involves estimating the cumulative error of the relative pose transformation by combining the fused position with the error correction model, specifically as follows:

[0075] Specifically, the error is defined as the difference between the real-time relative pose transformation and the position after fusion in step 7. The drift error correction model is defined as follows:

[0076]

[0077] i∈[2,K]

[0078] Where, Δe i Let i be the relative pose transformation error model at time i. and These represent the relative pose transformation position at time i and the fused position at time i, respectively. This represents the merged position at time n+1. Let n represent the fused position at time n, j represent the time when the relative pose transformation error model was last updated, and K represent the number of time points.

[0079] When there is a vehicle position in a 3D point cloud or a matching position in a 3D map of the urban environment, the above formula can be used to update the relative pose transformation error model.

[0080] Step 8 further corrects the relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time step, as follows:

[0081]

[0082] i∈[2,K],h<i

[0083] In the formula, Let t' represent the position relative to the pose change at time i.i Represents the position after error correction at time i, t n+1 t represents the position relative to the pose change at time n+1. n Represents the position relative to the pose change at time n, Δe h The relative pose transformation error model at time h is used to output the corrected pose t′ in real time. i As the final positioning result.

[0084] This invention calculates the optimal estimate of the current location by fusing GNSS position, map matching positioning results, and laser odometry position; it then uses the fused position to correct the cumulative error of the laser odometry, outputting a real-time positioning result. This method effectively overcomes the problems of poor positioning accuracy and low availability of satellite positioning systems in complex urban environments, and improves the robustness of the system by using map matching and traditional satellite positioning together to correct laser odometry errors. Attached Figure Description

[0085] Figure 1 : A schematic diagram of the method flow of an embodiment of the present invention; Detailed Implementation

[0086] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0087] In specific implementation, the method proposed in the technical solution of this invention can be automatically executed by those skilled in the art using computer software technology. System devices for implementing the method, such as computer-readable storage media storing the corresponding computer program of the technical solution of this invention and computer equipment including the computer program running the corresponding computer program, should also be within the protection scope of this invention.

[0088] The technical solution of the system in this embodiment of the invention is a continuous positioning system for urban driving vehicles, including: a three-dimensional lidar, a host computer, a GNSS positioning module, and a POS system;

[0089] The aforementioned three-dimensional lidar, GNSS positioning module, and POS system are installed on the top of the vehicle, while the host computer is installed inside the vehicle.

[0090] The host computer is connected in sequence to the three-dimensional lidar, the GNSS positioning module, and the POS system.

[0091] The three-dimensional lidar is used to collect environmental three-dimensional point clouds and scan line fields at multiple times, and transmit the environmental three-dimensional point clouds at multiple times to the host computer.

[0092] The GNSS positioning module is used to collect vehicle positions at multiple times and transmit the vehicle positions at multiple times to the host computer.

[0093] The POS system is used to collect vehicle poses at multiple times and transmit the vehicle poses at multiple times to the host computer.

[0094] The host computer uses the urban driving vehicle continuous positioning method to achieve accurate vehicle positioning based on the environmental 3D point cloud at multiple times, the scan line field at multiple times, the vehicle pose at multiple times, and the vehicle position at multiple times.

[0095] The model of the 3D lidar is: Hesai XT32 lidar.

[0096] The host computer model is Intel NUC 12.

[0097] The model of the GNSS positioning module is: Ublox F9P

[0098] The model of the POS system is: Applanix POS LV

[0099] The following is combined Figure 1 This invention introduces a method for continuous positioning of urban driving vehicles, comprising the following steps:

[0100] Step 1: The host computer combines the 3D point cloud of the environment at each moment with the vehicle pose at the corresponding moment to construct a 3D map of the urban environment.

[0101] Step 2: The host computer registers the environmental 3D point clouds from two adjacent time points to obtain the pose transformation and translation of the two adjacent time points, constructs the relative pose transformation of the two adjacent time points, and further obtains the relative pose of the environmental 3D point cloud at the current time point and the environmental 3D point cloud at the first time point. The vehicle position at the current time point is combined with the relative pose point to perform coordinate transformation to obtain the vehicle position in the environmental 3D point cloud at the current time point.

[0102] The relative pose transformation between two adjacent moments in step 2 is defined as follows:

[0103]

[0104] i∈[2,K]

[0105] Among them, T i，i-1R represents the relative pose transformation at time i relative to time i-1. i，i-1 This represents the attitude change at time i relative to time i-1. K represents the translation amount at time i relative to time i-1, and K = 3000 represents the number of time points.

[0106] Step 1 describes obtaining the relative pose transformation between the current environmental 3D point cloud and the environmental 3D point cloud at the first time step, as follows:

[0107] T i =T i,i-1 ·T i-1,i-2 ·...·T 2,1

[0108] Among them, T i This represents the relative pose transformation between the current environmental 3D point cloud and the environmental 3D point cloud at the first time step.

[0109] The vehicle position in the current 3D point cloud of the environment, as described in step 2, is specifically defined as follows:

[0110]

[0111] i∈[2,K]

[0112] in, Let x represent the vehicle's position in the 3D point cloud of the environment at time i. i The x-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i is y. i The z-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i represents the position of the vehicle at time i. i The Z-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i represents the position of the vehicle at time i, and K = 3000 represents the number of time points;

[0113] Step 3: The current environmental 3D point cloud and the urban environmental 3D map are matched to obtain the vehicle position in the urban environmental 3D map at the current time. The relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time is combined to perform coordinate transformation to obtain the matching position in the urban environmental 3D map at the current time.

[0114] The location for matching the current urban environment 3D map in step 3 is defined as follows:

[0115]

[0116] i∈[2,K]

[0117] in, Let x represent the location of the urban environment 3D map matching at time i. iThe x-axis coordinate of the location matched on the 3D map of the urban environment at time i is y. i Let z represent the Y-axis coordinate of the urban environment 3D map matching location at time i. i The Z-axis coordinate represents the location of the urban environment 3D map matching position at time i, and K = 3000 represents the number of time points.

[0118] Step 4: Divide the current 3D point cloud of the environment into voxels to obtain multiple voxels of the current 3D point cloud of the environment. Combine the multiple voxels of the current 3D point cloud of the environment with a voxel point cloud threshold for filtering to obtain multiple voxels of the current 3D point cloud of the environment after filtering.

[0119] Step 4 involves filtering multiple voxels of the current 3D point cloud environment based on a voxel point cloud threshold, as detailed below:

[0120] Voxels with a number of 3D points greater than the voxel point cloud threshold are selected from multiple voxels of the current environment 3D point cloud to construct multiple voxels of the current environment 3D point cloud as described in step 4.

[0121] Step 5: After filtering each voxel of the current environmental 3D point cloud, use principal component analysis to obtain the linear eigenvalue, planar eigenvalue, and point eigenvalue of each voxel. Then, use the eigenvalue determination method to obtain the weight of each voxel of the current environmental 3D point cloud, and further obtain the weight of each 3D point in each voxel of the current environmental 3D point cloud.

[0122] Step 5 describes obtaining the weight of each voxel in the current moment's 3D point cloud of the environment after filtering using the eigenvalue determination method, as follows:

[0123] If the planar feature value of each voxel in the current environment 3D point cloud is the largest after filtering, then the weight of each voxel in the current environment 3D point cloud is 0.

[0124] If the point feature value of each voxel in the current environment 3D point cloud is the largest after filtering, then the weight of each voxel in the current environment 3D point cloud after filtering is 1.

[0125] If the linear eigenvalue of each voxel is maximized after filtering the 3D point cloud of the environment:

[0126] Based on the scan line field at the current moment, determine whether the points in the voxel come from the same scan line. If the 3D points of each voxel in the current environmental 3D point cloud come from the same scan line after filtering, the weight of each voxel in the current environmental 3D point cloud is 0.

[0127] Based on the scan line field at the current moment, determine whether the points in the voxel come from the same scan line. If the 3D points of each voxel in the current environment 3D point cloud come from different scan lines after filtering, then it is a true line feature in the environment. The weight of each voxel in the current environment 3D point cloud after filtering is 1.

[0128] The weight of each voxel in the current 3D point cloud of the environment is specifically defined as follows:

[0129]

[0130] i∈[2,K]

[0131] λ∈[1, V i ]

[0132] Among them, V i =700 indicates the number of voxels in the filtered 3D point cloud of the environment at time i. The weight of the λth voxel in the 3D point cloud of the environment at time i is represented by K = 3000, where K = 3000 represents the number of time points.

[0133] Step 5 further obtains the weight of each 3D point in each voxel of the current environment 3D point cloud after filtering, as follows:

[0134] The weight of each 3D point in each voxel of the current environment's 3D point cloud after filtering is the weight of the corresponding filtered voxel, specifically defined as:

[0135]

[0136] i∈[2,K],λ∈[1,V] i ]

[0137] Among them, V i =700 indicates the number of voxels in the filtered 3D point cloud of the environment at time i. denoted by λ, where r is the weight of the r-th 3D point in the λ-th voxel after filtering in the environmental 3D point cloud at time i, and K = 3000 represents the number of time points.

[0138] Step 6: Combine the environmental 3D point cloud from the previous time step with the relative pose transformations of two adjacent time steps to obtain the environmental 3D point cloud after the previous time step transformation. Combine the environmental 3D point cloud after the previous time step transformation with the environmental 3D point cloud after the previous time step transformation and obtain the corresponding 3D point in the environmental 3D point cloud after the previous time step transformation of each voxel after the current time step transformation using the nearest neighbor search method. Calculate the residual of each 3D point in each voxel after the current time step transformation and further calculate the registration score. Calculate the relative pose weight matrix at the current time step based on the registration score.

[0139] Step 6 involves calculating the residual of each 3D point in each voxel of the current environment's 3D point cloud after filtering. The specific formula is as follows:

[0140]

[0141] i∈[2,K]

[0142] λ∈[1, V i ]

[0143] r∈[1, N] λ ]

[0144] in, This represents the residual of the r-th 3D point in the λ-th voxel after filtering the environmental 3D point cloud at time i. This represents the r-th 3D point in the λ-th voxel after filtering the environmental 3D point cloud at time i. N represents the corresponding point in the 3D point cloud of the environment at time i-1, where N is the r-th 3D point in the λ-th voxel after filtering at time i. λ V represents the number of 3D points in the λ-th voxel after filtering the 3D point cloud of the environment at time i. i =700 indicates the number of voxels in the 3D point cloud of the environment at time i after filtering, and K=3000 indicates the number of time points.

[0145] Step 6 involves further calculating the registration score, as detailed below:

[0146]

[0147] i∈[2,K]

[0148] λ∈[1, V i ]

[0149] r∈[1, N] λ ]

[0150] in, Let represent the weight of the r-th 3D point in the λ-th voxel of the environmental 3D point cloud at time i. The score represents the residual of the r-th 3D point in the λ-th voxel after filtering the environmental 3D point cloud at time i. i N represents the score of the relative pose at time i. λ V represents the number of 3D points in the λ-th voxel after filtering the 3D point cloud of the environment at time i. i =700 indicates the number of voxels in the 3D point cloud of the environment at time i after filtering, and K=3000 indicates the number of time points.

[0151] Step 6, which calculates the relative pose weight matrix at the current moment based on the registration score, is as follows:

[0152]

[0153] i∈[2,K]

[0154] Among them, Ω i The weight matrix representing the relative pose at time i, score i Let I be the relative pose registration score at time i, where I is the identity matrix and K = 3000 represents the number of time points.

[0155] Step 7: Calculate the GNSS weight of the vehicle position in the 3D point cloud at the current moment using the GNSS covariance matrix method. Calculate the environmental 3D map matching position weight at the current moment using the completeness determination method. Combine the relative pose weight matrix, the GNSS weight, and the environmental 3D map matching position weight at the current moment to construct a fused position function model using graph optimization. Minimize the fused position function model as the optimization objective and solve it using the Gauss-Newton method to obtain the fused position.

[0156] The specific definition of the fused position in step 7 is:

[0157]

[0158] i∈[2,K]

[0159] in, Let x represent the fused position at time i. i The x-axis coordinate of the fused position at time i is represented by the y-axis coordinate. i The z-axis coordinate represents the fused position at time i. i The Z-axis coordinate represents the fused position at time i, and K represents the number of time points;

[0160] Step 8: Combine the fused position with the error correction model to estimate the cumulative pose transformation error, and further correct the relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time to obtain the corrected relative pose;

[0161] Step 8 involves estimating the cumulative error of the relative pose transformation by combining the fused position with the error correction model. Specifically:

[0162] Specifically, the error is defined as the difference between the real-time relative pose transformation and the position after fusion in step 7. The drift error correction model is defined as follows:

[0163]

[0164] i∈[2,K]

[0165] Where, Δe i Let i be the relative pose transformation error model at time i. and These represent the relative pose transformation position at time i and the fused position at time i, respectively. This represents the merged position at time n+1. Let n represent the fused position at time n, j represent the time when the relative pose transformation error model was last updated, and K = 3000 represent the number of time points.

[0166] When there is a vehicle position in a 3D point cloud or a matching position in a 3D map of the urban environment, the above formula can be used to update the relative pose transformation error model.

[0167] Step 8 further corrects the relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time step, as follows:

[0168]

[0169] i∈[2,K],h<i

[0170] In the formula, Let t' represent the position relative to the pose change at time i. i Represents the position after error correction at time i, t n+1 t represents the position relative to the pose change at time n+1. n Represents the position relative to the pose change at time n, Δe h The relative pose transformation error model at time h is used to output the corrected pose t′ in real time. i As the final positioning result, K=3000 represents the number of time points.

[0171] It should be understood that any parts not described in detail in this specification belong to the prior art.

[0172] Although this paper uses terms such as 3D LiDAR, host computer, GNSS positioning module, and POS system extensively, the possibility of using other terms is not excluded. These terms are used merely for the convenience of describing the essence of this invention, and interpreting them as any additional limitation would contradict the spirit of this invention.

[0173] It should be understood that the above description of the preferred embodiments is quite detailed, but it should not be considered as a limitation on the scope of protection of this invention. Those skilled in the art, under the guidance of this invention, can make substitutions or modifications without departing from the scope of protection of the claims of this invention, and all such substitutions or modifications fall within the scope of protection of this invention. The scope of protection of this invention should be determined by the appended claims.

Claims

1. A method for continuous positioning of urban driving vehicles using an urban driving vehicle continuous positioning system, characterized in that, The continuous positioning system for urban driving vehicles includes: a 3D lidar, a host computer, a GNSS positioning module, and a POS system; The aforementioned three-dimensional lidar, GNSS positioning module, and POS system are installed on the top of the vehicle, while the host computer is installed inside the vehicle. The host computer is connected in sequence to the three-dimensional lidar, the GNSS positioning module, and the POS system. The three-dimensional lidar is used to collect environmental three-dimensional point clouds and scan line fields at multiple times, and transmit the environmental three-dimensional point clouds at multiple times to the host computer. The GNSS positioning module is used to collect vehicle positions at multiple times and transmit the vehicle positions at multiple times to the host computer. The POS system is used to collect vehicle poses at multiple times and transmit the vehicle poses at multiple times to the host computer. The host computer constructs a 3D map of the urban environment; it registers the 3D point clouds of the environment at two adjacent time points to obtain the pose transformation and translation at those two time points, constructs the relative pose transformation at those two time points, and further obtains the vehicle position in the 3D point cloud of the environment at the current time; it combines the relative pose with coordinate transformation to obtain the matching position of the 3D map of the urban environment at the current time; it filters and obtains multiple voxels of the 3D point cloud of the environment at the current time; it calculates the weight of each 3D point in each voxel of the 3D point cloud of the environment at the current time; it further calculates the registration score and calculates the relative pose weight matrix of the current time based on the registration score; it combines the relative pose weight matrix of the current time, the GNSS weight of the current time, and the matching position weight of the 3D map of the environment at the current time to obtain the fused position through graph optimization; it further corrects the position to obtain the corrected relative pose. The continuous positioning method for urban driving vehicles includes the following steps: Step 1: The host computer combines the 3D point cloud of the environment at each moment with the vehicle pose at the corresponding moment to construct a 3D map of the urban environment. Step 2: The host computer registers the environmental 3D point clouds from two adjacent time points to obtain the pose transformation and translation of the two adjacent time points, constructs the relative pose transformation of the two adjacent time points, and further obtains the relative pose of the environmental 3D point cloud at the current time point and the environmental 3D point cloud at the first time point. The vehicle position at the current time point is combined with the relative pose point to perform coordinate transformation to obtain the vehicle position in the environmental 3D point cloud at the current time point. Step 3: The current environmental 3D point cloud and the urban environmental 3D map are matched to obtain the vehicle position in the urban environmental 3D map at the current time. The relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time is combined to perform coordinate transformation to obtain the matching position in the urban environmental 3D map at the current time. Step 4: Divide the current 3D point cloud of the environment into voxels to obtain multiple voxels of the current 3D point cloud of the environment. Combine the multiple voxels of the current 3D point cloud of the environment with a voxel point cloud threshold for filtering to obtain multiple voxels of the current 3D point cloud of the environment after filtering. Step 5: After filtering each voxel of the current environmental 3D point cloud, use principal component analysis to obtain the linear eigenvalue, planar eigenvalue, and point eigenvalue of each voxel. Then, use the eigenvalue determination method to obtain the weight of each voxel of the current environmental 3D point cloud, and further obtain the weight of each 3D point in each voxel of the current environmental 3D point cloud. Step 6: Combine the environmental 3D point cloud from the previous time step with the relative pose transformations of two adjacent time steps to obtain the environmental 3D point cloud after the previous time step transformation. Combine the environmental 3D point cloud after the previous time step transformation with the environmental 3D point cloud after the previous time step transformation and obtain the corresponding 3D point in the environmental 3D point cloud after the previous time step transformation of each voxel after the current time step transformation using the nearest neighbor search method. Calculate the residual of each 3D point in each voxel after the current time step transformation and further calculate the registration score. Calculate the relative pose weight matrix at the current time step based on the registration score. Step 7: Calculate the GNSS weight of the vehicle position in the 3D point cloud at the current moment using the GNSS covariance matrix method. Calculate the environmental 3D map matching position weight at the current moment using the completeness determination method. Combine the relative pose weight matrix, the GNSS weight, and the environmental 3D map matching position weight at the current moment to construct a fused position function model using graph optimization. Minimize the fused position function model as the optimization objective and solve it using the Gauss-Newton method to obtain the fused position. Step 8: Combine the fused position with the error correction model to estimate the cumulative pose transformation error, and further correct the relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time to obtain the corrected relative pose; In step 5, the weight of each voxel in the current moment's 3D point cloud is obtained after filtering using the eigenvalue determination method, as detailed below: If the planar feature value of each voxel in the current environment 3D point cloud is the largest after filtering, then the weight of each voxel in the current environment 3D point cloud is 0. If the point feature value of each voxel in the current environment 3D point cloud is the largest after filtering, then the weight of each voxel in the current environment 3D point cloud after filtering is 1. If the linear eigenvalue of each voxel is maximized after filtering the 3D point cloud of the environment: Based on the scan line field at the current moment, determine whether the points in the voxel come from the same scan line. If the 3D points of each voxel in the current environmental 3D point cloud come from the same scan line after filtering, the weight of each voxel in the current environmental 3D point cloud is 0. Based on the scan line field at the current moment, determine whether the points in the voxel come from the same scan line. If the 3D points of each voxel in the current environment 3D point cloud come from different scan lines after filtering, then it is a true line feature in the environment. The weight of each voxel in the current environment 3D point cloud after filtering is 1. The weight of each voxel in the current 3D point cloud of the environment is specifically defined as follows: in, This represents the number of voxels in the 3D point cloud of the environment at time i after filtering. Represents the i-th time step of the 3D point cloud of the environment. The weights of each voxel after filtering, where K represents the number of time points.

2. The continuous positioning method for urban driving vehicles according to claim 1, characterized in that, The relative pose transformation between two adjacent moments in step 2 is defined as follows: i∈[2,K] in, This represents the relative pose transformation at time i relative to time i-1. This represents the attitude change at time i relative to time i-1. Let i represent the shift relative to time i-1, and K represent the number of time points. Step 2 describes obtaining the relative pose transformation between the current environmental 3D point cloud and the environmental 3D point cloud at the first time step, as follows: T i =T i,i-1 ·T i-1,i-2 ·...·T 2,1 Among them, T i This represents the relative pose transformation between the current environmental 3D point cloud and the environmental 3D point cloud at the first time step. The vehicle position in the current 3D point cloud of the environment, as described in step 2, is specifically defined as follows: in, This represents the vehicle's position in the 3D point cloud of the environment at time i. This represents the X-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i. This represents the Y-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i. The Z-axis coordinate of the vehicle's position in the 3D point cloud of the environment at time i represents the position of the vehicle at time i, and K represents the number of time points.

3. The continuous positioning method for urban driving vehicles according to claim 2, characterized in that, The location for matching the current urban environment 3D map in step 3 is defined as follows: in, This represents the location of the urban environment 3D map matching at time i. This represents the X-axis coordinate of the urban environment 3D map matching location at time i. This represents the Y-axis coordinate of the urban environment 3D map matching location at time i. Let Z represent the Z-axis coordinate of the urban environment 3D map matching location at time i, and K represent the number of time points.

4. The continuous positioning method for urban driving vehicles according to claim 3, characterized in that, Step 4 involves filtering multiple voxels of the current 3D point cloud environment based on a voxel point cloud threshold, as detailed below: Voxels with a number of 3D points greater than the voxel point cloud threshold are selected from multiple voxels of the current environment 3D point cloud to construct the selected multiple voxels of the current environment 3D point cloud described in step 4.

5. The continuous positioning method for urban driving vehicles according to claim 1, characterized in that, Step 5 further obtains the weight of each 3D point in each voxel of the current environment 3D point cloud after filtering, as follows: The weight of each 3D point in each voxel of the current environment's 3D point cloud after filtering is the weight of the corresponding filtered voxel, specifically defined as: in, This represents the number of voxels in the 3D point cloud of the environment at time i after filtering. Represents the i-th time step of the 3D point cloud of the environment. The weight of the r-th 3D point in the selected voxels, where K represents the number of time steps.

6. The continuous positioning method for urban driving vehicles according to claim 5, characterized in that, Step 6 involves calculating the residual of each 3D point in each voxel of the current environment's 3D point cloud after filtering. The specific formula is as follows: in, This indicates the i-th time step after filtering the 3D point cloud of the environment. The residual of the r-th three-dimensional point in the individual element, This indicates the i-th time step after filtering the 3D point cloud of the environment. The r-th three-dimensional point in the individual element, This indicates the i-th time step after filtering the 3D point cloud of the environment. The corresponding point in the 3D point cloud of the environment at time i-1 for the r-th 3D point in the individual element. This indicates the i-th time step after filtering the 3D point cloud of the environment. The number of three-dimensional points in an individual. represents the number of voxels in the 3D point cloud of the environment at time i after filtering, and K represents the number of time points; Step 6 involves further calculating the registration score, as detailed below: in, Represents the i-th time step of the 3D point cloud of the environment. The weight of the r-th 3D point in the filtered voxels This indicates the i-th time step after filtering the 3D point cloud of the environment. The residual of the r-th three-dimensional point in the individual element, Let i represent the score of the relative pose at time i. This indicates the i-th time step after filtering the 3D point cloud of the environment. The number of three-dimensional points in an individual. represents the number of voxels in the 3D point cloud of the environment at time i after filtering, and K represents the number of time points; Step 6, which calculates the relative pose weight matrix at the current moment based on the registration score, is as follows: in, The weight matrix represents the relative pose at time i. Let K be the relative pose registration score at time i, where I is the identity matrix and K represents the number of time points.

7. The continuous positioning method for urban driving vehicles according to claim 6, characterized in that, The specific definition of the fused position in step 7 is: in, This represents the fused position at time i. This represents the X-axis coordinate of the fused position at time i. This represents the Y-axis coordinate of the fused position at time i. Let Z represent the Z-axis coordinate of the fused position at time i, and K represent the number of time points.

8. The continuous positioning method for urban driving vehicles according to claim 7, characterized in that, Step 8 involves estimating the cumulative error of the relative pose transformation by combining the fused position with the error correction model. Specifically: Specifically, the error is defined as the difference between the real-time relative pose transformation and the fused position; The drift error correction model is defined as follows: in, Let i be the relative pose transformation error model at time i. and These represent the relative pose transformation position at time i and the fused position at time i, respectively. This represents the merged position at time n+1. Let n represent the fused position at time n, j represent the time when the relative pose transformation error model was last updated, and K represent the number of time points. When there is a vehicle position in a 3D point cloud or a matching position in a 3D map of an urban environment, the above formula can be used to update the relative pose transformation error model. Step 8 further corrects the relative pose of the current environmental 3D point cloud and the environmental 3D point cloud at the first time step, as follows: In the formula, This represents the position relative to the pose change at time i. This represents the position at time i after error correction. This represents the position relative to the pose change at time n+1. This represents the position relative to the pose change at time n. The relative pose transformation error model at time h is used to output the corrected pose in real time. As the final positioning result.

Images