Inertia-laser relative navigation positioning method and device under global signal rejection

Abstract

The invention discloses an inertia-laser relative navigation positioning method and device under global signal rejection. According to the invention, on the basis that the laser radar carries out target detection on surrounding environment cooperative nodes, a target detection prediction frame of the laser radar is screened and corrected based on node IMU measurement information, and it is ensured that a target detection result of the laser radar always generates a forward gain for a cooperative navigation result; meanwhile, a multi-source relative navigation algorithm based on federated filtering is provided, after collaborative navigation measurement information is obtained, the navigation measurement information of the collaborative nodes and the collaborative navigation measurement information are subjected to Kalman filtering fusion by using federated Kalman filtering, a final navigation result is obtained, and the robustness of the navigation result is improved.

Description

Technical Field

[0001] The 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] At present, there are many types of vehicle navigation systems, and the single-vehicle navigation technology has developed to a relatively mature stage. Among them, the representative navigation sensor is the inertial measurement unit (IMU), which uses a three-axis gyroscope and accelerometer to calculate the current object's attitude and other navigation information. Lidar can also be used as a sensor to realize single-vehicle navigation. Lidar detects the surrounding environment by emitting infrared rays in all directions, and can realize object navigation by using methods such as feature matching. The laser SLAM in the simultaneous localization and mapping (SLAM) is developed based on this feature and is currently the research direction of single-vehicle high-precision navigation. However, due to the size and capacity limitations of the single navigation system, the performance of the single-vehicle navigation system is not very good in some more complex tasks, so the relative navigation system came into being. Relative navigation technology mainly uses the sensor information of multiple navigation nodes as navigation measurement information, and shares and calculates information through mutual communication between nodes, and finally realizes the mutual positioning of multiple nodes. Relative navigation technology has the advantages of good flexibility and high robustness, which to a certain extent makes up for the problems of single-vehicle navigation in processing complex tasks. Therefore, it is also a hot topic in the research field of navigation.

[0003] At present, the algorithms for relative navigation are mainly implemented 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 of cooperative nodes. The main implementation method is to use the differential information of the mobile base station type to locate the cooperative nodes, and then use the communication between nodes for correction. Commonly used algorithms include carrier phase difference algorithm, double difference and double difference rate of change algorithm. The above relative navigation algorithms are strongly related to global positioning sensors. Under the condition of poor satellite signals, GNSS and UWB signals are distorted, and the relative navigation confidence will be greatly reduced. The current collaborative method is to use nodes with good satellite signals to assist in positioning nodes with poor satellite signals. In terms of collaborative positioning using sensors, domestic researchers use visual-inertial navigation systems with GNSS systems for positioning, but due to the sparse characteristics of the visual system itself, the stability of this solution is not high. Therefore, how to perform relative navigation between nodes in an environment of global signal denial needs to be studied urgently. 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 laser radar 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 deduce 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 re-detected is still 0, it is determined that the target of the collaborative node 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 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 box is output as the information of the collaborative node; if χ is less than or equal to α, the prediction box 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 box is detected, it is determined that the target of the collaborative node is lost; if a new prediction box is detected, it is determined whether χ of the new prediction box is greater than α. If so, the position and posture information of the new prediction box is output as the information of the collaborative node; if not, it is determined that the target of the collaborative node is lost; if more than two new prediction boxes are detected, κ is calculated according to formula (7) in (3). 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, otherwise, it is determined that the target of the collaborative node is lost;

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

[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 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;.

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

[0018] Preferably, in step one, 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 comprises step 0, using the constructed target collaborative node data set to complete the training of the 3D target detection network in advance; the step 1 is to perform target detection using the 3D target detection network trained in step 0;

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

[0022] Using the vehicle-mounted laser radars of other collaborative nodes as information sources, 3D point cloud data collection is performed on the target collaborative node from multiple angles, multiple distances, and whether there is occlusion. 3D point cloud data collection is also performed between multiple target collaborative nodes.

[0023] Preprocessing the obtained point cloud data so that the characteristics of the point cloud data correspond, the characteristics including the coordinates of the point cloud data, the beam intensity, the line bundle and the 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] The present invention screens and corrects the target detection prediction frame of the laser radar based on the node IMU measurement information, based on the target detection of the surrounding environment collaborative nodes by the laser radar, to ensure 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 federal filtering is proposed. After obtaining the collaborative navigation measurement information, the federal 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 The figure 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 scenario.

[0034] Figure 5 It is 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 and 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 laser radar to detect the surrounding environment, perform target detection on the collaborative node, and 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] The detection of collaborative nodes can be achieved by using target detection methods such as 3D target detection network, corner feature detection, and optical flow feature detection. 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] In this embodiment, the vehicle-mounted laser radar of other collaborative nodes is first used as the information source to collect 3D point cloud data for the target collaborative node, and point cloud data for specific target nodes is collected from multiple angles, multiple distances, and with or without occlusion to form a unique point cloud data set; point cloud collection is also performed between multiple target collaborative nodes, and the obtained point cloud data is pre-processed 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, and 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, and the number of annotations, that is, the size of the data set is determined by the identifiability of the laser radar's line bundle and the target collaborative node. Finally, the data set is divided into a training set and a validation set. At this point, the data set 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 data set 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 data set constructed by S11 until the training result is 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] The trained 3D target detection network is used to infer the real-time laser radar navigation scanning data to obtain the predicted position and posture information of the target collaborative navigation vehicle; the reasoning using the trained 3D target detection network can greatly reduce the time of calculation and inference, ensuring the real-time performance of collaborative navigation. At the same time, the score of the inference result represents the probability that the object selected by the prediction box is the target collaborative node. This score and the corresponding prediction box information are stored in the form of a vector, and finally the vector group information is transmitted to the target collaborative vehicle through the communication system (such as ROS robot operating system, etc.) to facilitate the correction of the prediction box in step 2.

[0046] Step 2: Correct the prediction frame based on IMU measurement information; specifically 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 node vehicle is:

[0049]

[0050] R, p in the formula 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, there is:

[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 center of the vehicle as the origin and the forward direction of the laser radar as the X-axis, which conforms to the right-hand coordinate system. g is the gravitational acceleration. Assuming that 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 They 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 the following three situations, and the present invention will explain them 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 prediction 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, there are two situations in the prediction box results, which need to be judged: take the prediction box center 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 recursive position value of 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 x 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 x is greater than α, the prediction box is considered to be the target position, and the position and posture information of the prediction box are output as collaborative information; if x is less than or equal to α, the prediction box 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 for the target vehicle prediction box. If the prediction box cannot be added after the correct threshold is lowered, it is judged that the target is lost and the loop is exited; if more than one prediction box is added, the judgment and revision of the new prediction box are performed according to formula (6) or formula (7);

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

[0069] At this time, the prediction results 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 box 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 of the target vehicle prediction box. If the prediction box cannot be added after lowering the correct threshold, it is judged that the target is lost and the loop is exited; if more than one prediction box is added, the judgment and revision of the new prediction box 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 cooperative target information of laser cooperative 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. The main work of the main filter is time update and fusion between multiple sub-filters; each sub-filter represents a type of navigation information, and the predicted position information becomes a sub-filter as collaborative information. The sub-filters independently perform independent time updates and state updates. Generally, there is a reference system to ensure that there are common state quantities. In the integrated navigation system, the inertial navigation system is used as the reference system of the entire system, and other navigation source information is input into the sub-filters separately.

[0076] S1 collaborative information sub-filter construction

[0077] As a sub-filter in the federated Kalman filter, the collaborative measurement information needs to have covariance and navigation measurement information updated. Covariance directly reflects the reliability of navigation information quality. Laser collaborative information obtains the relative position information of the two targets. When converting to absolute position information, the position information of the main collaborative node needs to be used, and the covariance of this information is consistent with the covariance of the main collaborative node information. Therefore, the covariance of the position information of the main collaborative node can be used as the collaborative information covariance to enter the sub-filter for calculation. The navigation information and covariance of the main collaborative node can be obtained by the navigation sensor of the main collaborative node itself through federal filtering.

[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 then 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 adopted, 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, thereby performing 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 , distribute 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 fed back to the sub-filter after variance elimination to reset the estimate of the sub-filter. 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 federal filtering together.

[0090] The following is an explanation with a specific example:

[0091] Control unmanned vehicle A and unmanned vehicle B in Figure 4 The following scene trajectory is as follows Figure 5 As shown, it involves basic relative motion situations such as start-stop, overtaking, and parallel. Both vehicles were tested in an environment with good satellite information reception conditions. At the beginning of the collaborative experiment, RTK was started as the true trajectory of the two 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 protection scope of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope 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 laser radar 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 deduce 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 re-detected is still 0, it is determined that the target of the collaborative node 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 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 box is output as the information of the collaborative node; if χ is less than or equal to α, the prediction box 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 box is detected, it is determined that the target of the collaborative node is lost; if a new prediction box is detected, it is determined whether χ of the new prediction box is greater than α. If so, the position and posture information of the new prediction box is output as the information of the collaborative node; if not, it is determined that the target of the collaborative node is lost; if more than two new prediction boxes are detected, κ is calculated according to formula (7) in (3). 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, otherwise, it is determined that the target of the collaborative node is lost; (3) If the number of target detection prediction boxes obtained in step 1 is more than 2, take the center and length, width and height of each prediction box and perform the following calculations: 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;. Step three, the information of the collaborative node obtained in step two is converted into coordinates to obtain the coordinates in the world coordinate system, and 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, characterized in that 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, characterized in that The 3D object detection network adopts Pointnet++, PV-rcnn or Second-point network.

4. The method according to claim 2 or 3, characterized in that The method further includes step 0, in which the 3D target detection network is trained in advance using the constructed target collaborative node data set; the first step is to perform target detection using the 3D target detection network trained in step 0; The method for establishing the target collaborative node data set is as follows: Using the vehicle-mounted laser radars of other collaborative nodes as information sources, 3D point cloud data collection is performed on the target collaborative node from multiple angles, multiple distances, and whether there is occlusion. 3D point cloud data collection is also performed between multiple target collaborative nodes. Preprocessing the obtained point cloud data so that the characteristics of the point cloud data correspond, the characteristics including the coordinates of the point cloud data, the beam intensity, the line bundle and the 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, characterized in that The α is [0.4, 0.7].

6. The method according to claim 1, characterized in that 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 described in any one of claims 1 to 6.

Images