An unmanned ship combined navigation method based on IMU / DVL / magnetometer / lidar

Abstract

The present application relates to a kind of unmanned ship combination navigation method based on IMU / DVL / magnetometer / LiDAR, for the hardware condition of MEMS IMU, Doppler velocity meter and magnetometer and other sensors.Point cloud distortion correction refers to the point cloud in the same data frame in different coordinate systems is transferred to the coordinate system of the starting point cloud in the frame.MEMS IMU, DVL, magnetometer all have high data update frequency, before point cloud data update, the motion of carrier is predicted and estimated.MEMS IMU outputs angular velocity and acceleration in carrier coordinate system, using the above information to update state;Magnetometer outputs magnetic field intensity in b system, DVL outputs velocity in the coordinate system of DVL, using the above information to update measurement, deduce simplified 9-dimensional state Kalman filter, estimate the motion of carrier, and then effectively correct point cloud distortion.Realize the point cloud in the same data frame is transferred to the coordinate system of the starting point cloud in the frame, complete the goal of point cloud distortion correction, improve the precision of LiDAR navigation.

Description

Technical Field

[0001] This invention pertains to navigation methods for unmanned aerial vehicles (UAVs), specifically a combined navigation method for UAVs based on IMU / DVL / magnetometer / lidar, addressing the problem of decreased navigation accuracy caused by point cloud distortion in lidar. Background Technology

[0002] Since the beginning of the 21st century, with the gradual depletion of land resources, the development of marine resources has become increasingly important. Unmanned surface vessels (USVs), as a product of intelligent technology, are intelligent surface platforms integrating automated control, navigation and positioning, multi-sensor fusion, and other technologies. They can complete relevant operations in target waters under remote control or autonomous navigation, and have advantages such as strong applicability and small size. Essentially, USVs are mobile robots for use on the water surface. Autonomous operations on the water surface often require high positioning accuracy, making a complete and high-precision navigation and positioning system indispensable. With the rapid development of computer computing power, new navigation methods based on lidar have received widespread attention. LiDAR obtains three-dimensional point cloud information of the environment by emitting and detecting reflected laser beams. LiDAR-based SLAM systems are unaffected by lighting conditions and can operate in all weather conditions, but they also have the drawback of being susceptible to point cloud distortion.

[0003] Raw LiDAR data places the point cloud within the same data frame in the coordinate system of the starting point cloud of that frame. This approach is feasible when the LiDAR is stationary and point cloud distortion is not a concern. However, in actual SLAM system operation, due to the slow LiDAR scanning speed (10Hz), sharing the starting point cloud coordinate system can lead to severe point cloud distortion during complex motion. Point cloud distortion refers to the fact that, during LiDAR data acquisition, the point cloud within the same data frame is acquired at different times due to the movement of the carrier; that is, the coordinate systems of different point clouds within a single frame are inconsistent. The carrier's motion can be decomposed into rotational motion and translational motion, and correspondingly, point cloud distortion can also be divided into rotational distortion and translational distortion.

[0004] (1) Rotational distortion

[0005] As attached Figure 1 As shown, the impact of point cloud rotation distortion is discussed in a simplified 2D scene. The blue arrow indicates the current scanning position of the LiDAR. The initial true angle difference between points A and B is θ. Due to the rotation Δθ of the LiDAR during scanning, the angle difference between points A' and B' measured by the LiDAR becomes...

[0006] The real position of point A and the position of point A' in the shared scan frame are shown in the accompanying Figure 2 As can be seen, the rotation distortion of the point cloud will cause the measured point cloud to become dense or sparse, and corresponding rotation compensation is required to restore the real point cloud coordinates. The relationship between the real angle and the measured angle can be represented by the following formula:

[0007]

[0008] (2) Translation distortion

[0009] As shown in the accompanying Figure 3 , we further simplify in a two-dimensional scene and only consider the point cloud translation distortion caused by translation along the x-axis. The distance of point A from the y-axis is t initially, and due to the translation of the laser radar during scanning, the distance of the laser radar measured point A' from the y-axis is

[0010] The real position of point A and the position of point A' in the shared scan frame are shown in the accompanying Figure 4 As can be seen, the rotation distortion of the point cloud will cause the measured point cloud to become dense or sparse, and corresponding rotation compensation is required to restore the real point cloud coordinates. The relationship between the real angle and the measured angle can be represented by the following formula:

[0011]

[0012] As can be seen from the above, due to the inherent rotation scanning structure of the mechanical scanning laser radar, the motion of the carrier will inevitably cause the generation of point cloud distortion, which will affect the subsequent point cloud registration task and cause the decrease of the inter-frame odometer accuracy of the laser radar. SUMMARY

[0013] Technical problems to be solved

[0014] In order to avoid the shortcomings of the prior art, the present application proposes an unmanned ship combined navigation method based on IMU / DVL / magnetometer / laser radar, which is used under the hardware conditions of MEMS IMU, Doppler velocity log (DVL) and magnetometer sensors. Point cloud distortion correction refers to transferring the point cloud in different coordinate systems in the same data frame to the coordinate system of the starting point cloud of the frame. MEMS IMU, DVL and magnetometer all have high data update frequency, and can predict and estimate the motion of the carrier before the point cloud data is updated. MEMS IMU outputs the angular velocity and acceleration in the carrier coordinate system (b system), and uses the above information to update the state; the magnetometer outputs the magnetic field intensity in the b system, and the DVL outputs the velocity in the DVL coordinate system (d system), and uses the above information to update the measurement, to deduce a simplified 9-dimensional state Kalman filter, estimate the motion of the carrier, and then effectively correct the point cloud distortion.

[0015] Technical scheme

[0016] An unmanned ship combined navigation method based on IMU / DVL / magnetometer / LiDAR, characterized in that the steps are as follows:

[0017] Step 1: Simplified Kalman filter combined navigation algorithm

[0018] Let the 9-dimensional state variable of the system to be estimated be:

[0019] X=[Φ δv n δP T ] n (1)

[0020] Wherein, Φ is the attitude misalignment angle, δv g is the velocity error, and δP is the position error;

[0021] System state equation

[0022]

[0023] In the formula, represents the rotation matrix of the b system to the navigation coordinate system, i.e. the n system, is the projection of the specific force measured by the accelerometer in the navigation system, η a and η n are the white noise of the gyroscope and accelerometer;

[0024] System measurement equation Z=HX+V:

[0025]

[0026] In the formula, is the magnetic field measurement value, H n-1 is the geomagnetic vector in the navigation system, is the speed information calculated by the inertial navigation system, is the rotation matrix of the DVL coordinate system, i.e. the d system to the b system, is the DVL measurement value;

[0027] Step 2: Point cloud timestamp acquisition

[0028] The data frame generates n measurement points, let (x0, y0, z0) represent the starting point cloud of the frame, and (x n-1 , y n-1 , z s ) be the end point cloud of the frame; Since the emission interval time between the vertically arranged laser emitters of the LiDAR is very small and can be ignored, the vertical angle ω does not need to be considered, only the horizontal angle α needs to be considered;

[0029] According to the coordinate triangle relationship, the starting angle αs is:

[0030]

[0031] Similarly, the end angle of scanning α e is:

[0032]

[0033] In the same way, the scanning angle α i is determined for any point cloud in the data frame period T:

[0034]

[0035] In the case of uniform rotation of the laser radar, the relative generation time of any point cloud in the period is determined by the angle difference:

[0036]

[0037] After obtaining the scanning start time time_scan and the relative time time_rel, the real point cloud generation time time_cur is determined:

[0038] time_cur = time_scan + time_rel (8)

[0039] Step 3: Linear interpolation of integrated navigation data:

[0040] For any point cloud i, the time difference Δt between point cloud i and navigation information j is calculated based on the navigation information j and j+1 on both sides:

[0041] Δt = t i -t j (9)

[0042] In the formula, t i is the time of point cloud i, and t j is the time of navigation information j;

[0043] The proportion of time difference Δt to adjacent navigation state is:

[0044]

[0045] Using first-order linear interpolation, the navigation pose T i of any point cloud i is obtained:

[0046] T i = (1-r) × T j +r × T j+1 (11)

[0047] In the formula, T j represents the combined navigation pose at time j, T j+1 represents the combined navigation pose at time j+1.

[0048] Step 4: Point cloud distortion compensation:

[0049] Let the starting point cloud pose of the frame be T0, and the pose of any point cloud i be T i , the relative pose of the two point clouds is obtained by the following formula:

[0050] ΔT = (T0) T T i (12)

[0051] Through the relative pose, the measured point P i of point cloud i is converted to the starting point cloud coordinate system according to the following formula, and the distortion-corrected point cloud P i ' is obtained:

[0052] P i ' = ΔTP i (13)

[0053] Finally, by inputting the distortion-corrected point cloud P i ' into the laser radar system, the subsequent navigation task is completed.

[0054] In step 3, due to the difference between the point cloud data frequency and the combined navigation data frequency, when the navigation information of the j+1 frame is not output, it needs to be processed:

[0055] First, calculate the time difference Δt between point cloud i and the jth frame of navigation information:

[0056] Δt = t i -t j (14)

[0057] Then, the proportion of Δt and the time difference of two frames of navigation information needs to be calculated:

[0058]

[0059] Finally, the difference between T j-1 and T j is used to predict the navigation pose T i of point cloud i:

[0060] T i = T j +r×(T j -T j ) = (1+r)×T j -r×T j (16)

[0061] The navigation pose Ti Any point cloud i pose T as step 4 i .

[0062] Advantages

[0063] The application provides an unmanned ship combined navigation method based on an IMU / DVL / magnetometer / LiDAR, and is used under the hardware conditions of MEMS IMU, a Doppler velocity log (DVL) and a magnetometer and the like. Point cloud distortion correction refers to transferring point clouds in different coordinate systems in the same data frame to a coordinate system where starting point clouds of the frame are located. The MEMS IMU, the DVL and the magnetometer all have a high data update frequency, and can predict and estimate the motion of a carrier before point cloud data is updated. The MEMS IMU outputs angular velocity and acceleration in a carrier coordinate system (b system), and state updating is performed by using the above information; the magnetometer outputs magnetic field intensity in the b system, and the DVL outputs velocity in a DVL coordinate system (d system), and measurement updating is performed by using the above information, a simplified 9-dimensional state Kalman filter is derived, the motion of the carrier is estimated, and then point cloud distortion is effectively corrected.

[0064] (1) The application derives error equations of the magnetometer and the DVL, and provides a simplified MEMS IMU / magnetometer / DVL combined Kalman filter algorithm. The algorithm performs time updating by using the IMU and performs measurement updating by using the magnetometer / DVL, can be independently applied to a navigation task, and has universality and practicability.

[0065] (2) The application provides a linear interpolation method, which aligns the combined navigation result with a time stamp of the LiDAR, and lays a foundation for subsequent point cloud distortion correction. Meanwhile, the method provides a feasible idea for solving the problem of alignment of different time stamps.

[0066] (3) The application provides an unmanned ship combined navigation method based on an IMU / DVL / magnetometer / LiDAR, which ingeniously uses combined navigation results obtained by Kalman filtering, realizes transferring point clouds in the same data frame to a coordinate system of starting point clouds of the frame, completes the target of point cloud distortion correction, and improves the navigation precision of the LiDAR. BRIEF DESCRIPTION OF DRAWINGS

[0067] Figure 1 LiDAR rotation distortion of an implementation example

[0068] Figure 2 A point rotation distortion of an implementation example

[0069] Figure 3 LiDAR translation distortion of an implementation example

[0070] Figure 4 A point translation distortion for implementing an example

[0071] Figure 5 A point cloud timestamp retrieval flowchart for implementing an example

[0072] Figure 6 A time series alignment schematic for implementing an example

[0073] Figure 7 A point cloud distortion correction flow for implementing an example

[0074] Figure 8 A rotation experiment platform for implementing an example

[0075] Figure 9 A rotation experiment point cloud map comparison for implementing an example

[0076] Figure 10 A robot chassis for implementing an example

[0077] Figure 11 A translation experiment platform for implementing an example

[0078] Figure 12 A translation experiment point cloud map comparison for implementing an example

[0079] Figure 13 An unmanned ship underwater platform for implementing an example

[0080] Figure 14 A water surface experiment scene and point cloud map for implementing an example

[0081] Figure 15 A water surface experiment trajectory comparison chart for implementing an example DETAILED DESCRIPTION

[0082] The present application will be further described in conjunction with the embodiments, drawings:

[0083] The present application mainly includes the following steps:

[0084] Step one: Simplify the Kalman filter integrated navigation algorithm

[0085] The navigation information required for point cloud distortion motion compensation should meet the requirements of high frequency, high real-time and high precision as much as possible. Since the motion in the real world is complex and variable, only high-frequency navigation information can truly reflect the real-time motion state of the carrier, so as to better perform interpolation motion compensation; the SLAM system is a system with high real-time requirement. Taking a 16-line laser radar as an example, it generates nearly 300,000 point cloud data per second. In order to eliminate distortion, the de-distortion algorithm needs to traverse all point clouds and correct all point clouds through a unified timestamp, which also consumes a large amount of computing power. Therefore, the navigation solution should reduce the amount of calculation as much as possible to save computing resources for subsequent front-end and back-end processing; finally, the accuracy of the navigation information can directly affect the point cloud de-distortion effect, and the high-precision navigation solution result is the primary goal of the motion compensation algorithm.

[0086] In full consideration of the above three requirements of the navigation information required for motion compensation, the application designs and derives a simplified MEMS IMU / magnetometer / DVL combined Kalman filter algorithm for point cloud distortion correction.

[0087] Let the 9-dimensional state variable of the system to be estimated be:

[0088] X = [Φ δv n δP n ] T (17)

[0089] Wherein, Φ is the attitude misalignment angle, δv n is the velocity error, and δP is the position error.

[0090] (1) System state equation

[0091] Let the system state equation be:

[0092]

[0093] In the formula, F and G are time-varying matrices with respect to the time parameter t, and W b is a zero-mean Gaussian white noise vector.

[0094] Since the MEMS IMU cannot sense the earth angular velocity, the specific forms of F, G and W b are derived by using the simplified MEMS IMU error equation, and the simplified error equation is as follows

[0095]

[0096]

[0097]

[0098] In the formula: denotes the rotation matrix of b to navigation frame (n), η g , η a is the gyro and accelerometer white noise, is the projection of the specific force measured by the accelerometer in the navigation frame.

[0099] Substitute equation 5 into equation 4, we have:

[0100]

[0101] Therefore, we have:

[0102]

[0103]

[0104]

[0105] (2) System measurement equation

[0106] Let the system measurement equation be:

[0107] Z = HX + V

[0108] The error equation of the magnetometer and DVL is shown in the supplement. According to the error equation, the system measurement composed of the MEMS IMU and the difference between the magnetometer and DVL is defined as:

[0109]

[0110] In the formula, is the measured value of the magnetometer, H n is the geomagnetic vector in the navigation frame, is the speed information calculated by the inertial navigation system, is the rotation matrix of the coordinate system (d) of the DVL to the b frame, is the measured value of the DVL.

[0111] Substitute equation 8 into equation 7, we have:

[0112]

[0113]

[0114] In the formula, V H is the geomagnetic measurement white noise, V D is the DVL measurement white noise.

[0115] In summary, equation 4 and equation 7 can be combined to obtain the MEMS IMU / magnetometer / DVL combined Kalman filter system equation.

[0116] Step two: Time stamp calculation of point cloud

[0117] Since the scanning period of the laser radar is 100ms, the laser radar will only provide the time stamp time_scan of the starting moment of scanning, so the specific time of the generation of all point clouds needs to be calculated according to the position relationship of the point clouds, so as to perform linear interpolation subsequently. During the operation of the mechanical scanning laser radar, due to the high-speed mechanical movement, the starting angle of scanning is not exactly 0°, but near 0°. Similarly, the ending point of scanning is not exactly 360°, but near 360°. In order to accurately calculate the generation time of all point clouds of the frame, the starting angle and the ending angle of scanning need to be determined first.

[0118] We assume that n measurement points are generated in the data frame, let (x0, y0, z0) represent the starting point cloud of the frame, and let (x n-1 ,y n-1 ,z n-1 ) be the ending point cloud of the frame. Since the emission interval time between the vertically arranged laser emitters of the laser radar is very small and can be ignored, the vertical angle ω does not need to be considered, and only the horizontal angle a needs to be considered.

[0119] According to the coordinate triangle relationship, the starting angle a s is:

[0120]

[0121] Similarly, the ending angle a e of scanning is:

[0122]

[0123] In the same way, the scanning angle a i of any point cloud in the data frame can be determined as:

[0124]

[0125] Under the condition that the data frame period T is determined, since the laser radar rotates at a constant speed, the relative generation time of any point cloud in the period can be determined by the angle difference, and the specific formula is as follows:

[0126]

[0127] After obtaining the starting time of scanning time_scan and the relative time time_rel, the real generation time of the point cloud time_cur can be determined:

[0128] time_cur = time_scan + time_rel (24)

[0129] In summary, the point cloud timestamp acquisition process is shown in FIG. 8. Figure 5

[0130] Step three: linear interpolation of integrated navigation data

[0131] In order to find the same time pose information for each point cloud for distortion correction, it is also necessary to interpolate the high-frequency integrated navigation information obtained in step one. The laser radar data frame sequence, radar point cloud sequence, and integrated navigation sequence are shown in FIG. 9. Figure 6

[0132] For any point cloud i, the navigation information j and j+1 on both sides need to be found. Then the time difference At between the point cloud i and the navigation information j needs to be calculated:

[0133] At = t i -t j (25)

[0134] In the formula, t i is the time of the point cloud i, and t j is the time of the navigation information j.

[0135] The proportion of the time difference At and the adjacent navigation state is:

[0136]

[0137] Using first-order linear interpolation, the navigation pose T i of any point cloud i can be obtained:

[0138] T i = (1-r) x T j + r x T j+1 (27)

[0139] In the formula, T j represents the integrated navigation pose at time j, and T j+1 represents the integrated navigation pose at time j+1.

[0140] In particular, for the m points shown in FIG. 10, due to the difference between the point cloud data frequency and the integrated navigation data frequency, there is no n+1 frame of navigation information output, so special processing needs to be performed as follows. Figure 6 First, the time difference At between the m points and the n-th frame of navigation information is calculated:

[0141] At = t m -t n (28)

[0142] Then the proportion of At and the time difference of the two frames of navigation information needs to be calculated:

[0143] ​​​

[0144]

[0145] Finally, using T n-1 With T n The difference is used to predict the navigation pose T of point cloud m. m :

[0146] T m =T n +r×(T n -T n-1 )=(1+r)×T n -r×T n-1 (30)

[0147] Step 4: Point Cloud Distortion Compensation

[0148] Step 3 yields the poses of each point cloud in the same data frame under the corresponding navigation coordinates. Let the initial point cloud pose of the frame be T0, and the pose of any point cloud i be T0. i The relative pose of two point clouds can be obtained using the following formula:

[0149] ΔT=(T0) T T i (31)

[0150] Based on the relative pose, the measurement point P of point cloud i is determined according to the following formula. i Switching to the initial point cloud coordinate system, we obtain the distortion-corrected point cloud P. i ':

[0151] P i '=ΔTP i (32)

[0152] The point cloud distortion correction process, combining steps one through four, is shown in the attached figure. Figure 7 As shown. Finally, by correcting the distortion of the point cloud P... i 'It is sent into the lidar system to complete the subsequent navigation task.'

[0153] The implementation example includes two parts: first, verifying the effectiveness of the point cloud distortion correction method of the present invention; and second, verifying the impact of the present invention on navigation accuracy.

[0154] Example 1: Point Cloud Distortion Correction Experiment

[0155] LiDAR point cloud distortion can be divided into two categories: point cloud distortion caused by carrier rotation and point cloud distortion caused by carrier translation. The motion of the LiDAR can also be decomposed into rotational motion and translational motion. To verify the effectiveness of the point cloud distortion correction of this invention, the A-LOAM algorithm is used as an example to conduct experimental verification on rotational motion and translational motion respectively, and the point cloud maps before and after applying this invention are compared.

[0156] (1) Rotation Experiment

[0157] The rotating experimental platform is attached. Figure 8 As shown, a 16-line lidar, a MARG (Magnetic, Angular Rate, and Gravity) sensor, and a turntable are fixed together to form the main body of the rotating experimental platform. An industrial control computer, display screen, and lithium battery modules are also connected for data storage and power supply. The 16-line lidar adopts a mechanical rotating structure and can perform point cloud measurements within a vertical range of ±15° and a horizontal range of 360°. Its vertical resolution is 2°, its horizontal resolution is 0.2°, its scanning frequency is 10Hz, and it generates nearly 300,000 point cloud data points per second with a point cloud accuracy within ±3cm. The turntable parameters are shown in Table 1; the MARG sensor parameters are shown in Table 2.

[0158] Table 1 Main performance parameters of the turntable

[0159]

[0160] Table 2 Technical Parameters of MARG Sensor

[0161]

[0162] The turntable was rotated, including irregular shaking and sudden stops, during which LiDAR point cloud data and MARG sensor data were recorded. Point cloud maps were generated using both the original A-LOAM and the improved A-LOAM. The results are attached. Figure 9 As shown in the attached analysis. Figure 9 (a) It can be seen that after a long period of turntable rotation, due to the limited indoor geometric features, the original lidar mileage error accumulates significantly, leading to obvious errors in the point cloud map; analysis of the appendix... Figure 9 (b) It can be seen that, due to the effective correction of point cloud distortion, the overall cumulative error of the improved lidar odometer is small, and it can maintain a good rotation estimation effect.

[0163] (2) Translation Experiment

[0164] The experimental platform is an extension of the robot chassis, which includes a built-in 400-line odometer to provide odometer information for the platform. An additional acrylic sheet is added to the robot chassis; the middle layer houses the lithium battery and industrial control computer, and the top layer mounts the LiDAR and MARG sensors. The robot chassis is shown in the attached image. Figure 10 As shown in the attached diagram, the complete experimental platform is... Figure 11 As shown.

[0165] In the land-based experiments, an odometer was used instead of a DVL (Dual Volume Level). Both measure b-series velocities and have the same error model; the only difference lies in the form of the velocity output. The odometer outputs a velocity as v.d = [0 v 0], DVL velocity output is v d = [v x v y v z ].

[0166] In the indoor corridor with few geometric features and prone to translation error, the robot is controlled to repeatedly move back and forth, and the laser radar point cloud data, MARG sensor data and odometry data are recorded. Then, the original A-LOAM and the improved A-LOAM are used to estimate the saved data between frames, and then the corresponding point cloud map is generated. The results are shown in Figs. 16A and 16B. Figure 12 As shown in Fig. 16A, the translation estimation error of the original algorithm is large, resulting in multiple ghosting phenomena in the point cloud map. As shown in Fig. 16B, in the corridor scene where the translation feature is not obvious, the positioning accuracy of the improved algorithm is obviously improved. Figure 12 Figure 12

[0167] Example Two: Water Surface Navigation Experiment

[0168] In order to verify the influence of the point cloud distortion correction of the application on the navigation accuracy, the A-LOAM algorithm is taken as an example to compare the navigation accuracy before and after the application of the application.

[0169] Experimental platform: The unmanned ship experimental platform is shown in Fig. 17. The 16-line laser radar, MARG sensor, global positioning system (GPS) and other sensors are fixed on the box; the DVL is immersed in water through aluminum material to provide three-dimensional velocity measurement; the industrial computer, lithium battery and the like are placed in the box, and the box is fixed to the ship body through a belt. Figure 13 Experimental equipment: The 16-line laser radar adopts a mechanical rotating structure and can measure point clouds within a vertical range of ±15° and a horizontal range of 360°. The vertical resolution is 2°, the horizontal resolution is 0.2°, the scanning frequency is 10Hz, nearly 300,000 point cloud data are generated per second, and the point cloud accuracy is within ±3cm. The GPS positioning accuracy is centimeter level. The DVL speed measurement accuracy is 1%±1mm / s. The MARG sensor parameters are shown in Table 1.

[0170] Table 1 MARG sensor parameter table

[0171]

[0172]

[0173] ​​​Experimental steps: the unmanned ship experimental platform travels along the lake shore for about 220m, and its trajectory is shown in the following figure. During the operation, subscribe to the laser radar, MARG sensor, DVL, GPS topics, and save them as.bag files. The water surface experiment scene and point cloud map are shown in Figs. 15(a) and 15(b). Figure 14

[0174] Experimental results: the differential GPS trajectory, A-LOAM algorithm trajectory, and the algorithm trajectory of the present application are shown in Fig. 15(a). Figure 15 Figure 15 As can be seen from Fig. 15(b), in the water surface scene, the SLAM algorithm designed in the present application and the A-LOAM algorithm both maintain stable operation, and the positioning error of the algorithm in the present application is generally smaller than that of the A-LOAM algorithm, but at about 700s, due to the increase in the speed error of the DVL in the shallow water area, the error rises. The positioning errors of the two SLAM algorithms are shown in the following table:

[0175] Table 2: Water surface experiment error of two SLAM algorithms

[0176]

[0177] Through water surface experiments, due to the introduction of the point cloud distortion correction algorithm of the present application, the positioning accuracy of the SLAM algorithm in the present application is better than that of the A-LOAM algorithm, and the average positioning error relative to the total distance is reduced by 0.17%, and the standard deviation of the positioning error relative to the total distance is reduced by 0.09%.

[0178] In addition, the error model of the magnetometer is:

[0179] Since the running time of the SLAM system is relatively short, and only navigation and mapping are performed in a small range, during the operation of the SLAM system, the geomagnetic vector can be considered as a three-dimensional constant. Only before running the SLAM system in the present application, through the world geomagnetic field model (WMM), input the time and latitude and longitude position parameters, the geomagnetic vector H m at the magnetic field coordinate system m under this time and place can be obtained.

[0180]

[0181] The three measurement sensitive axes of the three-axis magnetometer are in the b system, and the measurement output is Neglecting the high-order small amount, the measurement error model of the magnetometer can be obtained

[0182]

[0183] In addition, the DVL error model is:

[0184] ​​For convenience, a DVL coordinate system fixed to the carrier is established, denoted as d-frame, and the DVL velocity output can be expressed as:

[0185] v d = [v x v y v z ] (35)

[0186] Since there is a small installation error angle a between d-frame and b-frame, the transformation matrix from d-frame to b-frame is:

[0187]

[0188] In addition, the DVL scale factor error δK D will affect its velocity output, and the output velocity and the theoretical velocity v d have the following relationship:

[0189]

[0190] To simplify the error equation, the error sources that can be compensated by accurate pre-measurement, such as scale factor error δK D and installation error angle a, are not considered here. The DVL velocity error can be simplified as:

[0191]

Claims

1. An unmanned ship combined navigation method based on IMU / DVL / magnetometer / lidar, characterized by The steps are as follows: Step 1: Simplified Kalman filter integrated navigation algorithm Let the 9-dimensional state variable of the system to be estimated be: X = [Φ δv n δP n ] T (1) where Φ is the attitude misalignment angle, δv n is the velocity error, and δP is the position error. System state equation wherein denotes the rotation matrix of b to the navigation frame n, is the projection of the specific force measured by the accelerometer in the navigation frame, η g , η a is the gyro and accelerometer white noise; System measurement equation Z = HX + V: wherein H is the magnetometer measurement value, n is the geomagnetic vector in the navigation system, is the velocity information calculated by the inertial navigation system, is the rotation matrix from the DVL coordinate system, i.e., d system, to b system, is the DVL measurement value; Step 2: Point cloud timestamp acquisition Data frame generates n measuring points, let (x0, y0, z0) represent the frame starting point cloud, let (x n-1 ,y n-1 ,z n-1 ) be the frame end point cloud; Since the emission interval time between each laser emitter of the laser radar is small, it is ignored, so the vertical angle ω does not need to be considered, only the horizontal angle α needs to be considered; According to the coordinate triangle relationship, the initial angle a s is: Similarly, the scan end angle a e is: In the same way, the scanning angle a of the cloud of any point of the data frame is determined i is: Under the condition of determining the data frame period T, since the laser radar rotates at a constant speed, the relative generation time of any point cloud in the period is determined by the angle difference value: After obtaining the scan start time time_scan and the relative time time_rel, the real point cloud generation time time_cur is determined: time_cur = time_scan + time_rel (8) Step 3: Linear interpolation of integrated navigation data: For any point cloud i, the time difference Δt between the point cloud i and the navigation information j is calculated based on the navigation information j and j+1 on both sides: Δt = t i -t j (9) In the formula, t i is the time of the point cloud i, t j is the time of the navigation information j; The proportion of the time difference Δt and the adjacent navigation state is: Using first order linear interpolation, the navigation pose T of any point cloud i is obtained i : T i = (1 - r) x T j + r x T j+1 (11) In the formula, T j represents the combined navigation position at time j, T j+1 represents the combined navigation position at time j+1; Step 4: Point cloud distortion compensation: Let the frame start point cloud pose be T0, and the arbitrary point cloud i pose be T i The relative pose of the two point clouds is obtained by the following formula: ΔT = (T0) T T i (12) By the relative pose, the measurement points P of the point cloud i are transformed according to the following formula i Turning to the starting point cloud coordinate system, the point cloud P after distortion correction is obtained i ' P i ' = ΔTP i (13) Finally, by correcting the distortion of the point cloud P i The input laser radar system completes the subsequent navigation task.

2. The unmanned ship integrated navigation method based on IMU / DVL / magnetometer / laser radar according to claim 1, characterized in that: In step 3, due to the difference between the point cloud data frequency and the integrated navigation data frequency, when the navigation information of j+1 frame is not output, it needs to be processed: First, calculate the time difference Δt between the point cloud i and the jth frame of navigation information: At = t i -t j (14) Then the proportion of Δt and the time difference of two frames of navigation information needs to be calculated: Finally, the navigation pose T j-1 of the point cloud i is predicted using T j and the difference between T i . T i = T j + r x (T j - T j ) = (1 + r) x T j - r x T j (16) with the navigation pose T i as an arbitrary point cloud i pose T i .

Images