Multi-sensor fusion positioning method for complex port environments

Abstract

This invention discloses a multi-sensor fusion positioning method for complex port environments, comprising: S1, establishing a multi-sensor coordinate system; S2, extracting constraints in environments with GPS signals and rich structure; S3, extracting constraints in environments without GPS signals but with rich structure; S4, extracting constraints in environments without GPS signals and with degraded structure; and S5, establishing a nonlinear fusion positioning framework for different sensors, fusing all nonlinear constraints obtained from different sensors in different environments. This method achieves seamless vehicle positioning under changing scene features and environments, effectively reduces linearization errors, and solves the positioning problem in degraded structure environments.

Description

Technical Field

[0001] This invention relates to the field of positioning technology, and specifically to a multi-sensor fusion positioning method for complex port environments. Background Technology

[0002] Advances in autonomous navigation technology, particularly in robot localization in GPS-blind environments, have led to the rapid development of robot applications in inspection tasks. While numerous methods have been proposed for robot localization using onboard sensors such as cameras and lidar, achieving robust localization in geometrically degraded environments, such as tunnels, remains a challenging problem.

[0003] LiDAR data is characterized by high accuracy, real-time performance, and data stability. Furthermore, LiDAR is easy to install, making it suitable as a detector in small environmental perception systems. The LOAM (Lidar Odometry and Mapping) algorithm is a SLAM algorithm based on LiDAR. However, because LiDAR captures geometric information by scanning the environment, it is more susceptible to degradation in geometrically degraded environments such as tunnels.

[0004] Ultra-wideband (UWB) is a relatively new wireless communication technology. A key advantage of UWB-based ranging is that its broadband capability allows for higher accuracy in measuring Time of Arrival (ToA) and Time of Response (RTT) compared to other wireless technologies. In recent years, UWB-based spatial positioning technology has been widely applied in various military and civilian fields. While UWB effectively addresses the problem of GPS signal blockage indoors, it is not suitable for global positioning.

[0005] Due to the complexity of different scenarios, a single sensor cannot solve the localization problem in all situations, often requiring the fusion of multiple sensors for localization. Laser sensors offer high data accuracy and strong environmental adaptability, but they cannot distinguish geometrically continuous and repetitive scenes. Fusion with UWB sensor data enables localization in degraded environments.

[0006] Traditional fusion frameworks, such as Kalman filters (including their variants, the Extended Kalman Filter (EKF), the Unscented Kalman Filter (UKF), the Invariant Extended Kalman Filter algorithm, and the Invariant EKF), are very popular due to their mature technology, simple implementation, and high computational efficiency. However, in tunnels, the performance of integrating GNSS / INS via EKF degrades significantly.

[0007] In addition, existing fusion frameworks are relatively simple and can only solve positioning problems in single scenarios, such as positioning in degraded environments and positioning in scenarios without GPS signals. However, there is a lack of a universal fusion framework to achieve high-precision vehicle positioning that can be seamlessly integrated in different environments. Summary of the Invention

[0008] To overcome the shortcomings of the prior art, the present invention provides a multi-sensor fusion positioning method for complex port environments.

[0009] Therefore, the present invention adopts the following technical solution:

[0010] A multi-sensor fusion positioning method for complex port environments includes the following steps:

[0011] S1, Multi-sensor coordinate system one: The constructed map is based on the world coordinate system. The vehicle coordinate system is based on the LiDAR coordinate system, through a transformation matrix. Transform the positioning results of the LiDAR odometry to the world coordinate system In the process, the transformation matrix between the LiDAR coordinate system and the GPS coordinate system is obtained through the SVD decomposition method. Decompose into rotation matrices Translation matrix

[0012] S2, extracting constraints in environments with GPS signals and rich structures, includes the following steps:

[0013] S21, Obtain GPS signals through a GPS receiver and convert them into GPS positioning factors:

[0014] When the vehicle travels a certain distance, the GPS sensor receives a set of GPS data, including the GPS positioning factor at time k. The definition is as follows:

[0015]

[0016]

[0017] in It is the GPS positioning result at time k, including the vehicle's x, y, and z coordinates; This is the translation vector of the vehicle's state at time k, and also the vehicle's position, which is an unknown quantity, including the vehicle's x, y, and z coordinates; the covariance matrix is ​​used. Weight the positioning factors. The design is as follows:

[0018]

[0019] in, and These are the weights of GPS positioning in the x, y, and z directions, all of which are scalars, determined by the GPS positioning accuracy and GPS positioning status in the x, y, and z directions.

[0020] S22, using a lidar sensor for SLAM localization, obtains the relative pose factor per unit time:

[0021] Based on the LiDAR odometry method proposed in LOAM, the relative pose measurement values ​​[Δα] between consecutive LiDAR frames k and k+1 are obtained. k Δβ k ] T ,in Relative pose factor at time Defined as:

[0022]

[0023]

[0024] in It is the translation vector in the vehicle state at time k-1 and k; is the rotation vector of the vehicle state at time k-1 and k, both of which are unknowns; World coordinate system and lidar coordinate system The rotation matrix between them is calculated from the rotation matrix in S1. Instead; use the covariance matrix Weighting of relative attitude factors;

[0025] S3, extract constraints in environments with rich structures but no GPS signal;

[0026] S4, extract constraints in environments with no GPS signal and structural degradation;

[0027] S5 establishes a nonlinear fusion positioning framework for different sensors, fusing all nonlinear constraints obtained from different sensors under different environments:

[0028]

[0029] When there is a GPS signal, a1 = 1 and a3 = 0; otherwise, a1 = 0 and a3 = 1.

[0030] When in a structure-rich environment, a0 = 1, a2 = 0; otherwise, a0 = 0, a2 = 1.

[0031] Solve the above equation using the factor graph principle to obtain the vehicle state X under the condition of minimizing all constraint factors. k =[t k r k ] T ,

[0032] The transformation matrix mentioned in step S1 above It is expressed as follows:

[0033]

[0034] Where G is the GPS positioning result set and L is the LiDAR SLAM positioning result set.

[0035] In step S22 above, the covariance matrix... Design it in the following way:

[0036]

[0037] in It is the rotation weight, which is corrected through plane detection; The transformation weights are designed as follows:

[0038]

[0039] in As a scalar, its calculation method is as follows:

[0040] Using the following vehicle motion equations, predict the vehicle position at time k based on the vehicle positions at the previous two time points:

[0041]

[0042] Predicted location calculated by LiDAR odometry With position t k Error distance and weight between The larger the error distance, the smaller the weight; a Gaussian model is used to adjust the weights based on the error distance. The model is performed as follows:

[0043]

[0044] The variable σ is set according to the actual situation.

[0045] Step S3 above includes the following sub-steps:

[0046] S31, same as step S22, uses a lidar sensor to perform SLAM positioning and obtains the relative pose factor per unit time:

[0047] S32, using a UWB receiver to receive signals transmitted by roadside cooperative positioning equipment, calculate the distance measurement factor between the UWB receiver and the roadside cooperative positioning equipment:

[0048] The UWB sensor measures the distance from the roadside cooperative positioning device to the receiver, and the distance between the UWB receiver and the j-th roadside cooperative positioning device is denoted as... For scalars; assuming the receiver is located at the origin of the LiDAR coordinate system L, the distance measurement factor from the UWB receiver is determined by the following formula:

[0049]

[0050]

[0051] in, It is the j-th roadside cooperative positioning device U j exist The coordinate system position, with a maximum distance measurement coefficient of 5 centimeters from the roadside cooperative positioning device, is determined by... Weighting, It is a scalar. Therefore, It should have a higher weight.

[0052] Step S4 above includes the following sub-steps:

[0053] S41, using a lidar sensor to detect the wall and calculate the distance between itself and the wall, then matching it with walls on the map to convert it into a distance measurement factor:

[0054] In the map generation step, the plane equations of all walls in W are manually measured and represented by the following equations:

[0055]

[0056] in It is the normal vector of the i-th plane. and d represents the components of the normal vector in the x, y, and z directions, all of which are scalars; i It is the intercept of the i-th plane, and is a scalar;

[0057] When the lidar sensor scans the lidar point cloud, a plane detection method based on RANSAC is used to extract lidar points on the plane; the distance between the lidar sensor and the i-th wall is... express, For scalars; the distance measurement coefficient from planar detection is defined by the following formula:

[0058]

[0059]

[0060] At time k, l wall surfaces are detected. The distance measurement coefficient detected from the i-th plane reaches a maximum of 2cm, and is then... Weighted, It is a scalar. Therefore, It should have a higher weight.

[0061] S42, the same as S32, uses a UWB receiver to receive signals transmitted by the roadside cooperative positioning device and calculates the distance measurement factor between the UWB receiver and the roadside cooperative positioning device.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] 1. The fusion localization method of this invention has a smaller error compared to traditional fusion localization methods. Factor graph-based data fusion constructs a factor graph by combining current and historical data, establishes a batch optimization cost function based on the factor graph, and then optimizes all historical and current information together. In factor graph optimization, multiple iterations effectively reduce linearization errors.

[0064] 2. The fusion positioning method of this invention can achieve seamless vehicle positioning under changing scene features and environmental conditions, such as from a structurally rich environment to a structurally degraded environment, or from an environment with GPS signal to an environment without GPS signal. Such environmental changes can cause positioning problems in the absence of GPS signals, as well as problems in selecting and fusing different positioning methods.

[0065] 3. This invention solves the positioning problem under structural degradation environment. Attached Figure Description

[0066] Figure 1 This is a schematic diagram of the vehicle sensor installation in this invention;

[0067] Figure 2 A schematic diagram of a location system with GPS and complex structures;

[0068] Figure 3 A schematic diagram of positioning in a complex environment without GPS;

[0069] Figure 4 This is a schematic diagram of positioning in an environment without GPS and with degraded structures. Detailed Implementation

[0070] The fusion positioning method of the present invention will be described in detail below with reference to the accompanying drawings and specific examples.

[0071] This invention presents a multi-sensor fusion positioning method for complex port environments. Based on a high-precision map, it employs roadside collaborative positioning equipment and vehicle-mounted multi-sensor positioning equipment to form a multi-sensor assisted positioning system for complex port environments. The roadside collaborative equipment uses a UWB transmitter, and the vehicle-mounted multi-sensor system includes a lidar sensor, a GPS receiver, and a UWB receiver. The sensor installation diagram is shown below. Figure 1 As shown.

[0072] The multi-sensor assisted positioning system used in this invention includes one of the following: vehicle-mounted multi-sensor, lane-level map, roadside cooperative positioning device, and GPS, wherein:

[0073] The vehicle-mounted multi-sensor includes a GPS receiver, a lidar sensor, and a UWB receiver; the GPS receiver is used to acquire GPS signals; the lidar sensor is used to acquire lidar point clouds; and the UWB receiver is used to acquire UWB signals sent by the vehicle-road cooperative positioning device.

[0074] The roadside cooperative positioning device is a UWB transmitter used to transmit UWB signals and is only installed in areas without GPS signals.

[0075] The lane-level map includes information on the structure of buildings within the port and the location information of roadside cooperative equipment.

[0076] It employs sensors such as GPS, LiDAR, and UWB to collect GPS signals, 3D laser point clouds, and UWB ranging information. This information is then combined with lane-level maps to convert constraints into various scales, and factor graph fusion is used to achieve positioning.

[0077] Based on high-precision maps, roadside collaborative positioning equipment and vehicle-mounted multi-sensor positioning equipment are used to form a multi-sensor assisted positioning system for complex port environments.

[0078] For environments with GPS and complex structures, a GPS and LiDAR SLAM positioning method is used, such as... Figure 2 As shown. For environments without GPS and with complex structures (such as underground parking lots or freight warehouses), a fusion positioning method using LiDAR SLAM and UWB is employed, such as... Figure 3 As shown. For environments without GPS and with degraded structures (such as under a gantry bridge), a fusion positioning method using lidar and UWB is employed, such as... Figure 4 As shown.

[0079] The multi-sensor fusion positioning method for complex port environments of the present invention includes the following steps:

[0080] S1, Perform multi-sensor coordinate system one:

[0081] In the vehicle positioning system within a tunnel environment, the coordinate systems involved include the world coordinate system W and the LiDAR coordinate system L. The constructed map is based on the world coordinate system. The vehicle coordinate system is based on the LiDAR coordinate system, through a transformation matrix. The positions of the GPS receiver and UWB receiver in the LiDAR coordinate system are determined. The LiDAR sensor receives a set of LiDAR data. The LiDAR coordinate system of the first frame is used as the LiDAR coordinate system. The transformation matrix between the LiDAR coordinate system and the GPS coordinate system is... It is represented as follows:

[0082]

[0083] Where G is the GPS positioning result set and L is the LiDAR SLAM positioning result set;

[0084] By transforming the matrix Transform the positioning results of the LiDAR odometry to the world coordinate system In the process, the transformation matrix between the LiDAR coordinate system and the GPS coordinate system is obtained through the SVD decomposition method. Decompose into rotation matrices Translation matrix

[0085] S2, extracting constraints in environments with GPS signals and rich structures, includes the following steps:

[0086] S21, Obtain GPS signals through a GPS receiver and convert them into GPS positioning factors:

[0087] When the vehicle travels a certain distance, the GPS sensor receives a set of GPS data, including the GPS positioning factor at time k. The definition is as follows:

[0088]

[0089]

[0090] in It is the GPS positioning result at time k, including the vehicle's x, y, and z coordinates; This is the translation vector of the vehicle's state at time k, and also the vehicle's position, which is an unknown quantity, including the vehicle's x, y, and z coordinates; the covariance matrix is ​​used. Weight the positioning factors. The design is as follows:

[0091]

[0092] in, and These are the weights of GPS positioning in the x, y, and z directions, all of which are scalars, determined by the GPS positioning accuracy and GPS positioning status in the x, y, and z directions.

[0093] S22, using a lidar sensor for SLAM localization, obtains the relative pose factor per unit time:

[0094] Based on the LiDAR odometry method proposed in LOAM, the relative pose measurement values ​​[Δα] between consecutive LiDAR frames k and k+1 are obtained. k Δβ k ] T ,in Relative pose factor at time Defined as:

[0095]

[0096]

[0097] in It is the translation vector in the vehicle state at time k-1 and k; is the rotation vector of the vehicle state at time k-1 and k, both of which are unknowns; World coordinate system and lidar coordinate system The rotation matrix between them is calculated from the rotation matrix in S1. Instead; use the covariance matrix Weighting of relative attitude factors. Covariance matrix. Design it in the following way:

[0098]

[0099] in It is the rotation weight, which is corrected through plane detection; The transformation weights are designed as follows:

[0100]

[0101] in As a scalar, its calculation method is as follows:

[0102] Using the following vehicle motion equations, predict the vehicle position at time k based on the vehicle positions at the previous two time points:

[0103]

[0104] Predicted location calculated by LiDAR odometry With position t k Error distance and weight between The larger the error distance, the smaller the weight; a Gaussian model is used to adjust the weights based on the error distance. The model is performed as follows:

[0105]

[0106] The variable σ is set according to the actual situation.

[0107] S3, extracting constraints in environments with rich structures but no GPS signal, includes the following sub-steps:

[0108] S31, same as step S22, uses a lidar sensor to perform SLAM positioning and obtains the relative pose factor per unit time:

[0109] S32, using a UWB receiver to receive signals transmitted by roadside cooperative positioning equipment, calculate the distance measurement factor between the UWB receiver and the roadside cooperative positioning equipment:

[0110] The UWB sensor measures the distance from the roadside cooperative positioning device to the receiver, and the distance between the UWB receiver and the j-th roadside cooperative positioning device is denoted as... For scalars; assuming the receiver is located at the origin of the LiDAR coordinate system L, the distance measurement factor from the UWB receiver is determined by the following formula:

[0111]

[0112]

[0113] in, It is the j-th roadside cooperative positioning device U j exist The coordinate system position, with a maximum distance measurement coefficient of 5 centimeters from the roadside cooperative positioning device, is determined by... Weighting, It is a scalar.

[0114] S4, extracting constraints in environments with no GPS signal and degraded structures, includes the following sub-steps:

[0115] S41, using a lidar sensor to detect the wall and calculate the distance between itself and the wall, then matching it with walls on the map to convert it into a distance measurement factor:

[0116] In the map generation step, the plane equations of all walls in W are manually measured and represented by the following equations:

[0117]

[0118] in It is the normal vector of the i-th plane. and d represents the components of the normal vector in the x, y, and z directions, all of which are scalars; iIt is the intercept of the i-th plane, and is a scalar;

[0119] When the lidar sensor scans the lidar point cloud, a plane detection method based on RANSAC is used to extract lidar points on the plane; the distance between the lidar sensor and the i-th wall is... express, For scalars; the distance measurement coefficient from planar detection is defined by the following formula:

[0120]

[0121]

[0122] At time k, l wall surfaces are detected. The distance measurement coefficient detected from the i-th plane reaches a maximum of 2cm, and is then... Weighted, It is a scalar;

[0123] S42, the same as S32, uses a UWB receiver to receive signals transmitted by the roadside cooperative positioning device and calculates the distance measurement factor between the UWB receiver and the roadside cooperative positioning device.

[0124] S5 establishes a nonlinear fusion positioning framework for different sensors, fusing all nonlinear constraints obtained from different sensors under different environments:

[0125]

[0126] When there is a GPS signal, a1 = 1 and a3 = 0; otherwise, a1 = 0 and a3 = 1.

[0127] When in a structure-rich environment, a0 = 1, a2 = 0; otherwise, a0 = 0, a2 = 1.

[0128] Solve the above equation using the factor graph principle to obtain the vehicle state X under the condition of minimizing all constraint factors. k =[t k r k ] T ,

[0129] In one embodiment of the present invention, the container truck is 6.1 meters long, 2.4 meters wide, and 2.5 meters high. GPS, lidar, and UWB are respectively installed at three locations on the container truck. Figure 1 As shown. The parameters of each sensor are as follows:

[0130] 1. LiDAR: Model RPLIDAR S2; Scanning frequency: 10Hz; Angular resolution: 0.12°; Ranging error: ±30mm.

[0131] 2. GPS: Model CodingCooper BD3U; Positioning accuracy is 2.5 meters (CEP50, open area); Positioning update frequency is 1Hz by default and 10Hz by maximum.

[0132] 3. UWB: Model: USB-S1-PRO; ranging distance: 600m (open field line of sight); positioning error: X-axis and Y-axis ±10cm, Z-axis ±20cm; ranging accuracy: ±5cm.

Claims

1. A multi-sensor fusion positioning method for complex port environment, comprising the following steps: S1, Multi-sensor coordinate system 1: Constructed map based on world coordinate system Vehicle coordinate system based on LiDAR coordinate system, through transformation matrix Convert the positioning result of the LiDAR odometry to the world coordinate system In the middle, and through the SVD decomposition method, the transformation matrix between the LiDAR coordinate system and the GPS coordinate system Decomposed into a rotation matrix And translation matrix S2, extracting constraints in environments with GPS signals and rich structures, including the following steps: S21, obtaining GPS signals through a GPS receiver and converting them into GPS positioning factors: When the vehicle travels a certain distance, the GPS sensor receives a set of GPS data, and the GPS positioning factor at time k is defined as follows: wherein is the GPS positioning result at time k, including the x, y and z coordinates of the vehicle; is the translation vector in the vehicle state at time k, also the vehicle position, an unknown quantity, including the x, y and z coordinates of the vehicle; and is represented by a covariance matrix the positioning factors are weighted, and the design is as follows: wherein and are weights of the GPS positioning in the x, y and z directions, each being a scalar determined by the GPS positioning accuracy and the GPS positioning status in the x, y and z directions; S22, performing SLAM positioning using a LiDAR sensor to obtain relative pose factors within a unit time: Based on the LiDAR odometry proposed in LOAM, the relative pose measurement between consecutive LiDAR frames k and k+1 is obtained as [Δα k Δβ k ] T where the relative pose factor at time instant is defined as: in It is the translation vector in the vehicle state at time k-1 and k; is the rotation vector of the vehicle state at time k-1 and k, both of which are unknowns; World coordinate system and lidar coordinate system The rotation matrix between them is calculated from the rotation matrix in S1. Instead; use the covariance matrix Weighting of relative attitude factors; S3, extracting constraints in environments without GPS signals but with rich structures; S4, extracting constraints in environments without GPS signals and with degenerated structures; S5, establishing a nonlinear fusion positioning framework for different sensors to fuse all nonlinear constraints obtained through different sensors in different environments: wherein a1=1 and a3=0 when there are GPS signals, otherwise a1=0 and a3=1; a0=1 and a2=0 when in environments with rich structures, otherwise a0=0 and a2=1; Solve the above equation using factor graph principle to obtain the vehicle state under the minimum condition of all constraint factors 2. The multi-sensor fusion positioning method of claim 1, wherein: The transformation matrix in step S1 is represented as follows: wherein G is a set of GPS positioning results and L is a set of LiDAR SLAM positioning results.

3. The multi-sensor fusion positioning method of claim 1, wherein: In step S22, the covariance matrix It is designed in the following way: wherein is a weight of rotation, corrected by planar detection; is a weight of conversion, designed as follows: wherein is a scalar calculated as follows: Using the following vehicle motion equation, the vehicle position at time k is predicted based on the vehicle positions at the previous two times: predicted position computed by the LiDAR odometry error distance between the position t k and the weight is related, the greater the error distance, the smaller the weight; Using a Gaussian model to weight by error distance Modeling as follows: wherein the variable σ is set according to the actual situation.

4. The multi-sensor fusion positioning method of claim 1, wherein, Step S3 includes the following steps: S31, synchronizing with step S22, performing SLAM positioning using a LiDAR sensor to obtain relative pose factors within a unit time: S32, using a UWB receiver to receive signals transmitted by a roadside cooperative positioning device to calculate distance measurement factors between the UWB receiver and the roadside cooperative positioning device: The UWB sensor measures the distance from the roadside co-located positioning device to the receiver, and the distance between the UWB receiver and the jthroadside co-located positioning device is denoted as is a scalar; assuming the receiver is located at the origin of the LiDAR coordinate system L, the distance measurement factor from the UWB receiver is located by the following equation: wherein is the j-th road-side cooperative positioning device U j In The position in the coordinate system, the distance measurement from the road-side cooperative positioning device is maximally 5 centimeters, by Weighted, is a scalar.

5. The multi-sensor fusion positioning method of claim 1, wherein, Step S4 includes the following steps: S41, detecting a wall surface using a LiDAR sensor and calculating the distance between it and the wall surface, and converting it into a distance measurement factor by matching it with the wall surface in the map: In the map generation step, the plane equations of all wall surfaces in W are measured manually and represented by the following equation: wherein is the normal vector of the i-th plane, and are the components of the normal vector in the x, y and z directions, all being scalars; d i is the intercept of the i-th plane, being a scalar; When the lidar sensor scans the lidar point cloud, the planar lidar points are extracted by a plane detection method based on the RANSAC method; the distance between the lidar sensor and the ith wall surface is denoted as , is a scalar; the distance measurement coefficient from the plane detection is defined by the following formula: At k time, l wall surfaces are detected, the distance measurement coefficient detected from the i plane is up to 2cm, and is weighted, a scalar; S42, same as S32, using a UWB receiver to receive signals transmitted by a roadside cooperative positioning device to calculate distance measurement factors between the UWB receiver and the roadside cooperative positioning device.

Images