Inertial-laser relative navigation positioning method and device under global signal denial

Abstract

This invention discloses an inertial-laser relative navigation positioning method and device under global signal denial. Based on target detection of collaborative nodes in the surrounding environment by a laser radar, the invention screens and corrects the target detection prediction frame of the laser radar based on the node IMU measurement information, ensuring that the laser radar target detection results always generate positive gains for the collaborative navigation results. Furthermore, a multi-source relative navigation algorithm based on federated filtering is proposed. After obtaining the collaborative navigation measurement information, a federated Kalman filter is used to perform Kalman filtering fusion on the collaborative node's own navigation measurement information and the collaborative navigation measurement information to obtain the final navigation result, thereby improving the robustness of the navigation result.

Description

Technical Field

[0001] The present invention relates to the field of relative navigation technology, and in particular to an inertial-laser relative navigation positioning method in a global signal denial environment. Background Art

[0002] Currently, there are a wide variety of in-vehicle navigation systems available, and single-vehicle navigation technology has reached a relatively mature stage. A representative navigation sensor is the inertial measurement unit (IMU), which uses a three-axis gyroscope and accelerometer to calculate navigation information such as the current object's posture. LiDAR (LiDAR) can also be used as a sensor for single-vehicle navigation. LiDAR emits infrared light to detect the surrounding environment and uses methods such as feature matching to achieve object navigation. Simultaneous localization and mapping (SLAM) is based on this feature and is currently a research direction for high-precision single-vehicle navigation. However, due to the size and power limitations of single-vehicle navigation systems, single-vehicle navigation systems do not perform well in more complex tasks. This has led to the emergence of relative navigation systems. Relative navigation technology primarily utilizes sensor information from multiple navigation nodes as navigation measurement information. Through inter-node communication, this information is shared and calculated, ultimately achieving mutual positioning among multiple nodes. Relative navigation technology has the advantages of good flexibility and high robustness, which to some extent makes up for the problems of bicycle navigation in processing complex tasks. Therefore, it is also a hot topic of research in the field of navigation.

[0003] Currently, relative navigation algorithms are primarily based on global positioning sensors such as the Global Navigation Satellite System (GNSS) and Ultra Wide Band (UWB). These sensors can provide relatively accurate latitude and longitude for cooperating nodes. The primary implementation method utilizes differential information from mobile base stations to locate cooperating nodes, then uses inter-node communication for correction. Common algorithms include carrier phase difference, double difference, and double difference rate of change. These relative navigation algorithms are all strongly dependent on global positioning sensors. In conditions of poor satellite signals, GNSS and UWB signals are distorted, significantly reducing the confidence level of relative navigation. Current collaborative approaches utilize nodes with strong satellite signals to assist in the positioning of nodes with poor signals. Regarding collaborative positioning using sensors, domestic researchers have employed visual-inertial navigation systems in conjunction with GNSS systems. However, due to the inherent sparsity of visual systems, this approach is not very stable. Therefore, research is urgently needed on how to perform relative navigation between nodes in global signal denial environments. Summary of the Invention

[0004] In view of this, the present invention provides an inertial-laser relative navigation positioning method in a global signal denial environment, which can effectively achieve precise positioning under global signal denial.

[0005] The inertial-laser relative navigation positioning method in a global signal denial environment of the present invention comprises:

[0006] Step 1: Use LiDAR to detect the collaborative nodes in the surrounding environment, and obtain the target detection prediction box, position and posture, and the probability that the prediction box is the target collaborative node;

[0007] Step 2: Recursively calculate the speed and position of the node based on the IMU measurement information, and correct the target detection prediction frame obtained in step 1 based on the IMU recursive position data. Specifically:

[0008] (1) If the number of target detection prediction boxes obtained in step 1 is 0, the correct threshold of target detection reasoning is lowered and target detection is performed again. If the number of prediction boxes detected again is still 0, it is determined that the collaborative node target is lost in the field of view;

[0009] (2) If the number of target detection prediction boxes obtained in step 1 is 1, take the center of the prediction box and the length, width and height of the prediction box and perform the following calculations:

[0010]

[0011] in are the center position, length, width and height of the prediction box respectively; s imu is the position data recursively derived by the IMU; l0, w0, h0 are the length, width, and height of the known target collaborative node; ω1, ω2 are the set weights, which depend on the size of the target collaborative node; η is the probability that the predicted box is the target collaborative node;

[0012] If χ is greater than the set threshold α, the position and posture information of the prediction frame is output as the information of the collaborative node; if χ is less than or equal to α, the prediction frame is considered to be a false detection, and the correct threshold of the target detection reasoning is lowered to re-detect the target. If no new prediction frame is detected, the collaborative node target is judged to be lost; if a new prediction frame is detected, it is judged whether the χ of the new prediction frame is greater than α. If so, the position and posture information of the new prediction frame is output as the information of the collaborative node; if not, the collaborative node target is judged to be lost; if more than two new prediction frames are detected, κ is calculated according to formula (7) in (3). If κ is greater than α, the position and posture information of the prediction frame corresponding to the maximum value is taken as the information output of the collaborative node. Otherwise, the collaborative node target is judged to be lost.

[0013] (3) If the number of target detection prediction boxes obtained in step 1 is more than 2, the center and length, width and height of each prediction box are taken and calculated as follows:

[0014]

[0015] Among them, η i is the probability value of the i-th prediction box; is the center position, length, width and height of the i-th prediction box;

[0016] If κ is greater than α, the position and posture information of the predicted box corresponding to the maximum value is taken as the information output of the collaborative node; if κ is less than or equal to α, it is judged as a false detection, and the correct threshold of the target detection reasoning is lowered to re-detect the target. If the newly detected κ is still less than or equal to α, it is judged that the collaborative node target is lost. If the newly detected κ is greater than α, the position and posture information of the predicted box corresponding to the maximum value is taken as the information output of the collaborative node;.

[0017] In step three, the information of the collaborative nodes obtained in step two is converted into coordinates to obtain coordinates in the world coordinate system. Then, the information of the inertial navigation source is combined to perform multi-source relative navigation based on federated filtering.

[0018] Preferably, in step 1, a 3D target detection network, corner feature detection or optical flow feature detection is used to detect target collaborative nodes.

[0019] Preferably, the 3D object detection network adopts Pointnet++, PV-rcnn or Second-point network.

[0020] Preferably, the method further includes step 0, wherein the 3D object detection network is trained in advance using the constructed target collaborative node dataset; the step 1 is to perform target detection using the 3D object detection network trained in step 0;

[0021] The method for establishing the target collaborative node data set is as follows:

[0022] Using the on-board LiDARs of other collaborative nodes as information sources, 3D point cloud data is collected from multiple angles, distances, and whether or not there is occlusion on the target collaborative node. 3D point cloud data is also collected between multiple target collaborative nodes.

[0023] Preprocessing the obtained point cloud data so that characteristics of the point cloud data correspond, the characteristics including coordinates of the point cloud data, beam intensity, line bundle, and number of loops;

[0024] Use the 3D point cloud annotation tool to annotate the target collaborative nodes in the visualized point cloud, select the target collaborative nodes with a 3D wireframe, record the center coordinates, size and rotation angle of the 3D wireframe, and record the corresponding point cloud image in a text file to complete the establishment of the target collaborative node dataset.

[0025] Preferably, the α is [0.4, 0.7].

[0026] Preferably, in the step three of federated filtering, information of satellite navigation sources and / or visual navigation sources is added to perform multi-source relative navigation.

[0027] The present invention also provides an inertial-laser relative navigation and positioning device in a global signal denial environment, which uses the above method for navigation and positioning.

[0028] Beneficial effects:

[0029] Based on the target detection of collaborative nodes in the surrounding environment by the laser radar, the present invention screens and corrects the target detection prediction frame of the laser radar based on the node IMU measurement information, ensuring that the laser radar target detection result always produces a positive gain on the collaborative navigation result; at the same time, a multi-source relative navigation algorithm based on federated filtering is proposed. After obtaining the collaborative navigation measurement information, the federated Kalman filter is used to perform Kalman filtering fusion on the navigation measurement information of the collaborative node itself and the collaborative navigation measurement information to obtain the final navigation result, thereby improving the robustness of the navigation result. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] Figure 1 This is a flow chart of the inertial-laser relative navigation and positioning method of the present invention.

[0031] Figure 2 Schematic diagram of the process of correcting the predicted position based on IMU information.

[0032] Figure 3 Schematic diagram of the federated Kalman filter structure.

[0033] Figure 4 Schematic diagram of the relative navigation test scene.

[0034] Figure 5 The result of relative navigation trajectory solution. DETAILED DESCRIPTION

[0035] The present invention is described in detail below with reference to the accompanying drawings and embodiments.

[0036] The present invention provides an inertial-laser relative navigation positioning method in a global signal denial environment. The method flow chart is as follows: Figure 1 As shown, the specific steps include:

[0037] Step 1: Use LiDAR to detect the surrounding environment and perform target detection on the collaborative node to obtain the predicted position and posture of the target collaborative navigation vehicle, as well as the probability that the detection prediction box is the target;

[0038] Target detection methods such as 3D target detection network, corner feature detection, and optical flow feature detection can be used to detect collaborative nodes. This embodiment uses a 3D target detection network for target detection, which specifically includes the following sub-steps:

[0039] S11, establish target collaborative node data set

[0040] This embodiment first uses the on-board laser radars of other collaborative nodes as the information source to collect 3D point cloud data for the target collaborative node. Point cloud data is collected for specific target nodes from multiple angles, multiple distances, and with or without occlusion to form a unique point cloud dataset. Point cloud collection is also performed between multiple target collaborative nodes, and the obtained point cloud data is preprocessed so that the coordinates, beam intensity, line bundle, number of rings and other characteristics of the point cloud data correspond to each other for subsequent processing. Subsequently, a special 3D point cloud annotation tool (such as SusTech-points) is used to annotate the target collaborative node in the visualized point cloud. The target collaborative node is selected with a three-dimensional wireframe, and the center coordinates, size and rotation angle of the three-dimensional wireframe are recorded. The corresponding point cloud image is recorded in a text file. The number of annotations, that is, the size of the dataset, is determined by the identifiability of the laser radar's line bundle and the target collaborative node. Finally, the dataset is divided into a training set and a validation set. At this point, the dataset of the target collaborative node is established, and the 3D target detection network can be trained.

[0041] S12: Build a 3D target detection network and perform network training based on the target collaborative node dataset built in S11

[0042] Existing 3D object detection networks can be used, such as Pointnet++, PV-rcnn, Second-point network, etc.

[0043] Based on the training set in the dataset constructed by S11, the corresponding learning rate, decay rate, number of training rounds, number of training channels, etc. are selected to train the 3D object detection network to obtain a trained 3D object detection network; then, the network parameters of the trained 3D object detection network can be verified and adjusted based on the validation set in the dataset constructed by S11 until the training results are the best.

[0044] S13, use the trained 3D target detection network to perform target detection on the data obtained by the lidar in real time

[0045] A trained 3D object detection network is used to infer real-time LiDAR navigation scan data to derive the predicted position and pose of the target collaborative navigation vehicle. This inference significantly reduces computational time and ensures the real-time nature of collaborative navigation. The resulting score represents the probability that the object selected by the predicted box is the target collaborative node. This score and the corresponding predicted box information are stored as a vector. Finally, this vector group is transmitted to the target collaborative vehicle via a communication system (such as the ROS robot operating system) to facilitate correction of the predicted box in step 2.

[0046] Step 2: Correct the prediction frame based on IMU measurement information; specifically, it includes the following sub-steps:

[0047] S21, vehicle IMU recursion at this node

[0048] In continuous time, the kinematic equation of the IMU of the vehicle at this node is:

[0049]

[0050] Where R and p correspond to the rotation matrix and position in the world coordinate system, the first-order and second-order velocities v and acceleration a of the position. The world coordinate system takes the initial starting point as the origin and the initial starting direction as the X-axis, which conforms to the right-hand coordinate system. ω is the angular velocity, ω^ is the Lie algebraic operator symbol, and q is the corresponding quaternion. Considering gravity, for the IMU measurement value, we have:

[0051]

[0052] in, To eliminate the acceleration and angular velocity affected by gravity, R T is the conversion matrix between the world coordinate system and the vehicle coordinate system. The vehicle coordinate system takes the vehicle center as the origin and the forward direction of the lidar as the X axis, which conforms to the right-hand coordinate system. g is the acceleration of gravity. Assuming the IMU is placed horizontally, its value can be (0, 0, 9.8). T , then considering the noise, integrating, the state from time t to t+Δt is:

[0053]

[0054] The recursion of speed is:

[0055]

[0056] The recursion of the position is:

[0057]

[0058] Among them, b g , ba Represent the zero bias of the gyroscope and accelerometer respectively.

[0059] S22, correct the predicted position obtained in step 1 based on the IMU information

[0060] IMU information corrects the predicted position Figure 2 As shown. For the prediction frame vector information transmitted to the target cooperative vehicle through communication, the number of vectors is first determined: there are three cases, and the present invention will explain each case separately:

[0061] (1) No prediction vector (the prediction result is empty)

[0062] At this time, there are two situations in the point cloud image: one is that there is a target collaborative vehicle in the point cloud view, but the learning network does not detect the target. In this case, the candidate options for the target vehicle prediction box can be increased by lowering the correct threshold of the target detection model reasoning, and then the prediction box can be judged and revised according to formula (6) or formula (7); the other is that there is indeed no target collaborative vehicle in the point cloud view. At this time, after lowering the correct threshold of the target detection model reasoning, the number of predicted vectors is still 0, then it is judged that the collaborative target is lost in the field of view, and the loop judgment program is directly exited.

[0063] (2) There is only one prediction vector (there is only one prediction box result)

[0064] At this time, the prediction box results will also have two situations, which need to be judged: take the center of the prediction box and the length, width and height of the prediction box, and design the following judgment formula:

[0065]

[0066] in are the center position, length, width, and height of the prediction box respectively; s imu is the position recursively calculated by the IMU, l0, w0, h0 are the length, width and height of the known target cooperative vehicle; ω1, ω2 are the set weights, which depend on the specific size of the target vehicle.

[0067] Compare the χ value with the set threshold α (the value of α needs to be obtained based on actual engineering experience, usually 0.4 to 0.7, and 0.5 in this embodiment); if χ is greater than α, the predicted frame is considered to be the target position, and the position and posture information of the predicted frame are output as collaborative information; if χ is less than or equal to α, the predicted frame is considered to be a false positive. At this time, the correct threshold of the target detection model reasoning is also lowered to increase the candidate target vehicle prediction frame. If no new prediction frame is added after lowering the correct threshold, it is judged that the target is lost and the loop is exited; if more than one prediction frame is added, the judgment and revision of the new prediction frame are performed according to formula (6) or formula (7);

[0068] (3) There are multiple prediction vectors (multiple prediction boxes)

[0069] The prediction results at this time need to filter out unique navigation information and design the following discriminant:

[0070]

[0071] Among them, η i is the accuracy of the i-th prediction box; is the center position, length, width and height of the i-th prediction box; l0, w0, h0 are the length, width and height of the known target cooperative vehicle; ω1, ω2 are the set weights, which depend on the specific size of the target vehicle.

[0072] If κ is greater than α, the position and posture information of the prediction frame corresponding to the κ value is taken as the collaborative information output; if κ is less than or equal to α, it is judged as a false detection. At this time, the correct threshold of the target detection model inference is also lowered to increase the candidate prediction frame of the target vehicle. If no new prediction frame is added after lowering the correct threshold, it is judged that the target is lost and the loop is exited; if more than one prediction frame is added, the judgment and revision of the new prediction frame are performed according to formula (6) or formula (7).

[0073] Step 3: Multi-source relative navigation algorithm based on federated filtering:

[0074] The present invention uses the collaborative target information of the laser collaborative positioning obtained in step 2 as the navigation source, combines the inertial navigation device and laser radar navigation source of the node vehicle itself, and adopts federal filtering to realize multi-source navigation.

[0075] The federated filter is a multi-stage filter consisting of a main filter and multiple sub-filters. The sub-filters are parallel structures, such as Figure 3 As shown in the figure, the main filter's primary function is to update time and integrate multiple sub-filters. Each sub-filter represents a type of navigation information, and the predicted position information, as collaborative information, becomes a separate sub-filter. Each sub-filter independently performs time and state updates. A reference system is typically used to ensure a common state variable. In an integrated navigation system, the inertial navigation system serves as the reference system for the entire system, while other navigation source information is fed into the sub-filters separately.

[0076] S1 collaborative information sub-filter construction

[0077] As a sub-filter in the federated Kalman filter, collaborative measurement information requires updates to its covariance and navigation measurement information. Covariance directly reflects the reliability of navigation information quality. Laser collaborative information obtains relative position information between two targets. Converting this information to absolute position requires the position information of the master collaborative node. Since the covariance of this information is consistent with the covariance of the master collaborative node's information, the covariance of the master collaborative node's position information can be used as the collaborative information covariance in the sub-filter calculation. The master collaborative node's navigation information and its covariance can be obtained through federated filtering using its own navigation sensor.

[0078] Multi-source relative navigation solution for S2 target node

[0079] After all independent navigation source information is input into the sub-filter, it is filtered separately with the reference system, and the state information and covariance of the corresponding sub-filter are output and then enter the main filter for fusion. The variance upper bound elimination algorithm is used, and the weights are reset after fusion to eliminate the correlation between the sub-filters. The specific implementation method is as follows:

[0080] Each independent navigation source uses a different sensor, so the local state estimates are independent of each other. If there are N local state estimates and the corresponding estimation error covariance matrix P 11 , P 22 ,…P NN , then there is an optimal estimate

[0081]

[0082] in, Represents each navigation source, P 11 Represents the covariance matrix with itself.P g is the feedback matrix, which is:

[0083]

[0084] In general, the estimates of each sub-filter are correlated. To solve this problem, the variance upper bound technique is used to appropriately transform the filtering process so that the local estimates are actually uncorrelated, thus performing the filtering. We only consider the common state.

[0085] According to the information distribution principle, the amount of information in the state equation is inversely proportional to the noise variance, so Q -1 Represents the amount of information in the state equation; the measurement can also be done using the inverse of the noise covariance R -1 , distributing the total noise to each sub-filter, we have:

[0086]

[0087] in It is a variance upper bound elimination technique, and the coefficient sum is 1.

[0088] The synthesized global estimate and covariance matrix are then fed back to the sub-filter after variance elimination to reset the sub-filter estimate. This completes an update.

[0089] If there are other navigation sources on the vehicle, such as satellite, visual and other navigation source information, corresponding sub-filters can also be constructed to achieve federated filtering together.

[0090] The following is an example to illustrate:

[0091] Control unmanned vehicle A and unmanned vehicle B in the following Figure 4 The following scene trajectory is as follows Figure 5 As shown, this involves basic relative motion scenarios such as start-stop, overtaking, and parallel operation. Both vehicles were tested under good satellite information reception conditions. At the beginning of the collaborative experiment, RTK was enabled as the true trajectory of both vehicles to facilitate subsequent calculations.

[0092] The navigation trajectory results are as follows Figure 5 As shown in the figure, during the entire 200m journey, the maximum error of relative navigation is 2m and the average error is 0.79m.

[0093] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. An inertial-laser relative navigation positioning method in a global signal denial environment, characterized in that: include: Step 1: Use LiDAR to detect the collaborative nodes in the surrounding environment, and obtain the target detection prediction box, position and posture, and the probability that the prediction box is the target collaborative node; Step 2: Recursively calculate the speed and position of the node based on the IMU measurement information, and correct the target detection prediction frame obtained in step 1 based on the IMU recursive position data. Specifically: (1) If the number of target detection prediction boxes obtained in step 1 is 0, the correct threshold of target detection reasoning is lowered and target detection is performed again. If the number of prediction boxes detected again is still 0, it is determined that the collaborative node target is lost in the field of view; (2) If the number of target detection prediction boxes obtained in step 1 is 1, take the center of the prediction box and the length, width and height of the prediction box and perform the following calculations: in are the center position, length, width and height of the prediction box respectively; s imu is the position data recursively derived by the IMU; l0, w0, h0 are the length, width, and height of the known target collaborative node; ω1, ω2 are the set weights, which depend on the size of the target collaborative node; η is the probability that the predicted box is the target collaborative node; If χ is greater than the set threshold α, the position and posture information of the prediction frame is output as the information of the collaborative node; if χ is less than or equal to α, the prediction frame is considered to be a false detection, and the correct threshold of the target detection reasoning is lowered to re-detect the target. If no new prediction frame is detected, the collaborative node target is judged to be lost; if a new prediction frame is detected, it is judged whether the χ of the new prediction frame is greater than α. If so, the position and posture information of the new prediction frame is output as the information of the collaborative node; if not, the collaborative node target is judged to be lost; if more than two new prediction frames are detected, κ is calculated according to formula (7) in (3). If κ is greater than α, the position and posture information of the prediction frame corresponding to the maximum value is taken as the information output of the collaborative node. Otherwise, the collaborative node target is judged to be lost. (3) If the number of target detection prediction boxes obtained in step 1 is more than 2, the center and length, width and height of each prediction box are taken and calculated as follows: Among them, η i is the probability value of the i-th prediction box; is the center position, length, width and height of the i-th prediction box; If κ is greater than α, the position and posture information of the prediction box corresponding to the maximum value is taken as the information output of the collaborative node; if κ is less than or equal to α, it is judged as a false detection, and the correct threshold of target detection reasoning is lowered to re-detect the target. If the newly detected κ is still less than or equal to α, it is judged that the collaborative node target is lost. If the newly detected κ is greater than α, the position and posture information of the prediction box corresponding to the maximum value is taken as the information output of the collaborative node; In step three, the information of the collaborative nodes obtained in step two is converted into coordinates to obtain coordinates in the world coordinate system. Then, the information of the inertial navigation source is combined to perform multi-source relative navigation based on federated filtering.

2. The method according to claim 1, wherein In the step 1, a 3D target detection network, corner feature detection or optical flow feature detection is used to detect target collaborative nodes.

3. The method according to claim 2, wherein The 3D object detection network adopts Pointnet++, PV-rcnn or Second-point network.

4. The method according to claim 2 or 3, wherein: The method further includes step 0 of pre-training a 3D object detection network using the constructed target collaborative node dataset; step 1 of performing object detection using the 3D object detection network trained in step 0; The method for establishing the target collaborative node data set is as follows: Using the on-board LiDARs of other collaborative nodes as information sources, 3D point cloud data is collected from multiple angles, distances, and whether or not there is occlusion on the target collaborative node. 3D point cloud data is also collected between multiple target collaborative nodes. Preprocessing the obtained point cloud data so that characteristics of the point cloud data correspond, the characteristics including coordinates of the point cloud data, beam intensity, line bundle, and number of loops; Use the 3D point cloud annotation tool to annotate the target collaborative nodes in the visualized point cloud, select the target collaborative nodes with a 3D wireframe, record the center coordinates, size and rotation angle of the 3D wireframe, and record the corresponding point cloud image in a text file to complete the establishment of the target collaborative node dataset.

5. The method according to claim 1, wherein The α is [0.4, 0.7].

6. The method according to claim 1, wherein In the step three of federated filtering, information of satellite navigation sources and / or visual navigation sources is added to perform multi-source relative navigation.

7. An inertial-laser relative navigation and positioning device in a global signal denial environment, characterized in that: Navigation positioning is performed using the method according to any one of claims 1 to 6.

Images