Doppler speedometer residual error construction method for underwater robot positioning

Abstract

The invention provides a Doppler speedometer residual error construction method for underwater robot positioning. The method belongs to the field of underwater robot positioning. Comprising the following steps: firstly, measuring the three-dimensional speed of the underwater robot through a DVL (Doppler velocity meter); secondly, a reference coordinate system of DVL measurement data is converted into an IMU coordinate system through external parameters between the DVL and an IMU; measuring noise parameters and external parameters between the DVL and the IMU through a DVL sensor and an IMU sensor, and calculating a DVL factor covariance matrix; then calculating a DVL residual error through a system state estimator, an external parameter between the DVL and the IMU and measurement data of the DVL and the IMU; and finally, through the system state estimator and the external parameters between the DVL and the IMU, calculating the Jacobian matrix of the residual error of the DVL relative to the system state estimator. According to the novel DVL residual construction method suitable for nonlinear optimization provided by the invention, DVL measurement data are accurately utilized, so that the positioning precision and robustness of the underwater SLAM algorithm based on the DVL residual construction method are greatly improved compared with other underwater positioning algorithms.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater robot positioning, and in particular relates to a Doppler velocity meter residual construction method for underwater robot positioning. Background Art

[0002] At present, the use of DVL (Doppler velocimeter) measurement data by underwater multi-source fusion SLAM (simultaneous localization and mapping) algorithms can be roughly divided into two ways. One is to integrate the measurement velocity over time to obtain the relative position constraint between the measurement data, and use nonlinear optimization to obtain the pose estimation. The other is to associate the DVL measurement with the system state quantity as the observation equation of the Kalman filter (and its extended form). Algorithms based on nonlinear optimization usually have higher accuracy than those based on filtering, but the DVL modeling method used by the latter is more accurate. In order to combine the advantages of both, the DVL measurement data is more accurately associated with the state quantity. We propose a new DVL residual factor construction method suitable for nonlinear optimization fusion, which realizes the accurate use of DVL measurement data. Summary of the invention

[0003] Aiming at the insufficient and inaccurate use of DVL measurement data by existing SLAM algorithms based on nonlinear optimization, the present invention proposes a Doppler velocity meter residual construction method for underwater robot positioning.

[0004] In order to achieve the above purpose, the technical solution adopted by the present invention is:

[0005] This solution provides a Doppler velocity meter residual construction method for underwater robot positioning, which is characterized by comprising the following steps:

[0006] S1, measure the three-dimensional velocity of the underwater robot by DVL (Doppler velocimeter);

[0007] S2, converting the reference coordinate system of the DVL measurement data to the IMU coordinate system through the external reference between the DVL and the IMU (inertial measurement unit);

[0008] S3, calculating the DVL factor covariance matrix by measuring the noise parameters of the DVL and IMU sensors and the external parameters between the DVL and IMU;

[0009] S4, calculating the DVL residual through the system state estimation, the external parameters between the DVL and the IMU, and the DVL and IMU measurement data;

[0010] S5, calculating the Jacobian matrix of the DVL residual with respect to the system state estimate through the system state estimate and the external parameters between the DVL and the IMU;

[0011] Furthermore, the DVL measurement data in step S1 is as follows:

[0012] The velocity of the DVL in the direction of the corresponding beam is calculated by the frequency shift of the transmitted and received acoustic beam signals, and then the velocity of the DVL on the XYZ axis of its body coordinate system is converted by the velocity measurement results of multiple beams (usually 4). The superscript D represents the DVL body coordinate system (hereinafter referred to as the DVL coordinate system).

[0013] Furthermore, step S2 includes the following steps:

[0014] S201. In the underwater SLAM positioning system, we use IMU as the reference coordinate system of the underwater robot. We need to convert the speed of DVL into the IMU body coordinate system (hereinafter referred to as IMU coordinate system). is the rotation matrix from the DVL coordinate system to the IMU coordinate system (usually known), for Inverse transform, is the velocity of DVL in the IMU coordinate system, which is expressed as follows:

[0015]

[0016] S202, because the speed of DVL in the IMU coordinate system Affected by the two modes of motion, rotation and translation, it can be decomposed into the rotational velocity component and translational velocity components Make For a specific underwater robot, the coordinates of the DVL in the IMU coordinate system are fixed and known, so the translational velocity component of the DVL in the IMU coordinate system is is equal to the speed of the IMU in the IMU coordinate system, that is, Similarly, the angular velocities of DVL and IMU in the IMU coordinate system are also equal. Assume that at the corresponding DVL measurement time, the angular velocity of IMU rotation in the IMU coordinate system is The coordinates of DVL in the IMU coordinate system are: And because the conversion relationship between angular velocity and linear velocity is So you can Rewrite as

[0017]

[0018] In order to facilitate derivation and subsequent implementation, we rewrite the vector cross multiplication in the above formula into the form of matrix multiplication

[0019]

[0020] Where K is

[0021]

[0022] Furthermore, the step S3 of calculating the DVL factor covariance matrix includes the following steps:

[0023] S301, calculate the relationship between the DVL sensor measurement noise and the DVL speed measurement value. Since the DVL measurement value is only affected by zero-mean Gaussian white noise n DVL N(0,Σ DVL ), the measurement model of DVL is:

[0024]

[0025] S302, calculate the equation relationship between the IMU angular velocity measurement noise and the velocity measurement value of the DVL in the IMU coordinate system. will be subject to zero bias b gyr and measurement noise n gyr (Assuming that it follows a zero-mean Gaussian distribution ), and its measurement model is:

[0026]

[0027] in is the true value of IMU angular velocity, we can get the relationship between the IMU angular velocity measurement value and the true value as follows:

[0028]

[0029] Substituting this into the equation in matrix multiplication form in step S2:

[0030]

[0031] S303, with the previous equation relationship, calculate the measurement speed of DVL in the IMU coordinate system The relationship between each variable is:

[0032]

[0033] Will Substitute the equation into the above formula to get:

[0034]

[0035] set up The measurement noise n in this equation DVL and measurement noise n gyrBoth obey zero-mean Gaussian distribution and remain zero-mean Gaussian distribution after linear transformation. In addition, the two measurement noises are usually independent of each other, so we can get the covariance matrix of Gaussian noise n as

[0036]

[0037] Furthermore, step S4 includes the following steps:

[0038] Each position state of the underwater robot contains the quaternion estimate in the world coordinate system (Corresponding to the rotation matrix estimation from the IMU coordinate system to the world coordinate system ), velocity estimator And IMU angular velocity bias estimate We can use these three state estimators through equation Get a velocity estimate of the DVL in the IMU coordinate system

[0039]

[0040] in yes Inverse transform of . So we can estimate the speed and the speed value actually measured by DVL Constructing DVL velocity residuals

[0041]

[0042] Furthermore, step S5 includes the following steps:

[0043] Through the residual formula of step S4, we can get the DVL velocity residual e DVL about and The Jacobian matrix

[0044]

[0045] in The definition is

[0046]

[0047] The beneficial effects of the present invention are: BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 The present invention is a flow chart of the method.

[0049] Figure 2 Trajectory positioning diagram of different algorithms under the Caves dataset.

[0050] Figure 3 This is a diagram of the Holooocean underwater data collection scene in this embodiment.

[0051] FIG. 4 is a diagram showing the positioning trajectories of various positioning algorithms obtained by testing a simulation data set in this embodiment. DETAILED DESCRIPTION

[0052] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0053] Example

[0054] The present invention provides a Doppler velocity meter residual construction method for underwater robot positioning. First, the three-dimensional velocity of the underwater robot is measured by DVL. Secondly, the reference coordinate system of the DVL measurement data is converted to the IMU coordinate system through the external parameters between the DVL and the IMU. The noise parameters and the external parameters between the DVL and the IMU are measured by the DVL and IMU sensors again, and the DVL factor covariance matrix is ​​calculated. Then, the DVL residual is calculated by the system state estimate, the external parameters between the DVL and the IMU, and the DVL and IMU measurement data. Finally, the Jacobian matrix of the DVL residual with respect to the system state estimate is calculated by the system state estimate and the external parameters between the DVL and the IMU. Figure 1 As shown, the present invention proposes a Doppler velocity meter residual construction method for underwater robot positioning, and its implementation method is as follows:

[0055] S1. Measuring the three-dimensional velocity of the underwater robot by DVL:

[0056] The velocity of the DVL in the direction of the corresponding beam is calculated by the frequency shift of the transmitted and received acoustic beam signals, and then the velocity of the DVL on the XYZ axis of its body coordinate system is converted by the velocity measurement results of multiple beams. The superscript D indicates the DVL coordinate system.

[0057] S2. Convert the reference coordinate system of the DVL measurement data to the IMU coordinate system through the external reference between the DVL and the IMU. The implementation method is as follows:

[0058] S201. In the underwater SLAM positioning system, we use IMU as the reference coordinate system of the underwater robot. We need to convert the speed of DVL into the IMU coordinate system. is the rotation matrix from the DVL coordinate system to the IMU coordinate system, for Inverse transform, is the velocity of DVL in the IMU coordinate system, which is expressed as follows:

[0059]

[0060] S202, because the speed of DVL in the IMU coordinate system Affected by the two modes of motion, rotation and translation, it can be decomposed into the rotational velocity component and translational velocity components Make For a specific underwater robot, the coordinates of the DVL in the IMU coordinate system are fixed and known, so the translational velocity component of the DVL in the IMU coordinate system is is equal to the speed of the IMU in the IMU coordinate system, that is, Similarly, the angular velocities of DVL and IMU in the IMU coordinate system are also equal. Assume that at the corresponding DVL measurement time, the angular velocity of IMU rotation in the IMU coordinate system is The coordinates of DVL in the IMU coordinate system are: And because the conversion relationship between angular velocity and linear velocity is So you can Rewrite as

[0061]

[0062] In order to facilitate derivation and subsequent implementation, we rewrite the vector cross multiplication in the above formula into the form of matrix multiplication

[0063]

[0064] Where K is

[0065]

[0066] S3, through the DVL and IMU sensor measurement noise parameters and the external parameters between DVL and IMU, calculate the DVL factor covariance matrix, the implementation method is as follows:

[0067] S301, calculate the relationship between the DVL sensor measurement noise and the DVL speed measurement value. Since the DVL measurement value is only affected by zero-mean Gaussian white noise n DVL N(0,Σ DVL ), the DVL measurement model is:

[0068]

[0069] S302, calculate the equation relationship between the IMU angular velocity measurement noise and the velocity measurement value of the DVL in the IMU coordinate system. will be subject to zero bias b gyr and measurement noise n gyr (Assuming that it follows a zero-mean Gaussian distribution ), and its measurement model is:

[0070]

[0071] in is the true value of IMU angular velocity, we can get the relationship between the IMU angular velocity measurement value and the true value as follows:

[0072]

[0073] Substituting this into the equation in matrix multiplication form in step S2:

[0074]

[0075] S303, with the previous equation relationship, calculate the measurement speed of DVL in the IMU coordinate system The relationship between each variable is:

[0076]

[0077] Will Substitute the equation into the above formula to get:

[0078]

[0079] set up The measurement noise n in this equation DVL and measurement noise n gyr Both obey zero-mean Gaussian distribution and remain zero-mean Gaussian distribution after linear transformation. In addition, the two measurement noises are usually independent of each other, so we can get the covariance matrix of Gaussian noise n as

[0080]

[0081] S4. Calculate the DVL residual by using the system state estimate, the external parameters between the DVL and the IMU, and the DVL and IMU measurement data. The implementation method is as follows:

[0082] Each position state of the underwater robot contains the quaternion estimate in the world coordinate system (Corresponding to the rotation matrix estimation from the IMU coordinate system to the world coordinate system ), velocity estimator And IMU angular velocity bias estimate We can use these three state estimators through equation Get a velocity estimate of the DVL in the IMU coordinate system

[0083]

[0084] in yes Inverse transform of . So we can estimate the speed and the speed value actually measured by DVL Constructing DVL velocity residuals

[0085]

[0086] S5. Calculate the Jacobian matrix of the DVL residual with respect to the system state estimate through the system state estimate and the external parameters between the DVL and the IMU. The implementation method is as follows:

[0087] Through the residual formula of step S4, we can get the DVL velocity residual e DVL about and The Jacobian matrix

[0088]

[0089] in The definition is

[0090]

[0091] In this embodiment

[0092] In order to verify the performance of a Doppler velocimeter residual construction method for underwater robot positioning, we tested it on the underwater dataset Caves and the underwater simulator Holoocean. The experiment tested the performance of the SOTA (currently best performance) algorithms VINS-mono and ORB-SLAM3 (both of which are visual inertial measurement unit fusion SLAM algorithms) in the visual inertial measurement unit fusion SLAM algorithm, as well as the INEKF (invariant Kalman filter) and EKF (extended Kalman filter) positioning algorithms fused by IMU-DVL, as a comparison baseline for the performance evaluation of the visual inertial measurement unit Doppler velocimeter fusion SLAM algorithm (VID-SLAM) implemented based on the present invention.

[0093] The underwater dataset Caves was collected in a group of underwater caves on the seabed, and the exploration of the caves was completed with the help of AUVs (autonomous underwater robots) equipped with sensors such as cameras, IMUs, DVLs, and sonars. The size of the test site is about 100m×100m, and the AUV's driving route includes typical underwater scenes such as water plants, coral reefs, and sand.

[0094] Table 1. Cone spacing in the positioning results of each positioning algorithm in the Caves dataset (unit / meter)

[0095]

[0096] Since it is very difficult to calibrate the path truth when making underwater datasets, the Caves dataset does not contain the path truth. The dataset author recommends using the distance between the cones pre-arranged in the AUV driving path as a reference. Figure 2 , the quantified cone spacing is shown in Table 1. Due to the scarcity of underwater features and dim lighting, the ORB-SLAM3 algorithm cannot complete initialization and cannot run at all, while the VINS-mono algorithm cannot continue to run after the AUV enters the sandy environment due to tracking loss, and the algorithm also has the problem of small scale. Both algorithms show insufficient robustness in underwater environments. Although InEKF and EKF can run completely, the scale of InEKF positioning trajectory is small, and both InEKF and EKF are affected by abnormal DVL data, and burrs appear in the trajectory. VID-SLAM has the highest positioning accuracy and the smoothest trajectory among all compared algorithms. In addition, VID-SLAM can run normally on sections with visual loss, and has stronger robustness than VINS and ORB-SLAM3.

[0097] The Holoocean simulator simulates two underwater scenes, Dam and Openwater. The Dam scene is built around a dam. The size of the entire scene is 650m×650m. The scene lighting conditions are poor and the visual features are not significant. We carry out underwater robot inspections of dams and underwater pipelines in this scene. The task trajectory length is about 680m and is named Dam_seq. The Openwater scene is built around a seabed covered with aquatic plants. The size of the entire scene is 2km×2km. The scene lighting conditions are good and the visual features of the aquatic plant area are rich, but the visual features in some sandy areas are not significant. In this scene, we simulate the underwater robot's detection of a submarine wreck and the surrounding environment on the seabed. The task trajectory length is about 490m and is named Openwater_seq. See the data collection scene. Figure 3 .

[0098] Table 2. ATE indicators of positioning results of various positioning algorithms in Holoocean simulation

[0099]

[0100] The absolute path error (ATE, in meters) of each positioning algorithm obtained from the simulation data set test is shown in Table 2, and the positioning trajectory is shown in Figure 4. ORB-SLAM3 cannot complete initialization in the Dam-seq sequence with dim light and sparse features. Although it can complete initialization in Openwater-seq, it is still easy to lose tracking in sections with sparse visual features. The positioning accuracy of VINS-mono in the Openwater_seq sequence is greatly improved compared with the Dam-seq sequence, which is due to the improvement of the underwater visual environment. This shows that the algorithm requires good visual conditions (sufficient lighting and rich visual features) to run stably in the underwater environment. Since the IMU in the simulator cannot provide the initial value of the pose, the EKF cannot run. The positioning accuracy of InEKF in the two test sequences is comparable and has good consistency. This is because the IMU and DVL sensors it uses are not affected by changes in the visual environment, and its positioning accuracy depends on the performance of the sensor hardware. VID-SLAM has the highest positioning accuracy among the compared algorithms. Thanks to the present invention, VID-SLAM has a 45.8% performance improvement in Dam-seq sequence and a 22.1% performance improvement in Openwater_seq sequence compared to the basic architecture VINS-mono. Tests on the data set have verified that the VID-SLAM algorithm implemented based on the present invention has good positioning accuracy and robustness.

Claims

1. A Doppler velocity meter residual construction method for underwater robot positioning, characterized in that: The following steps are involved: S1, measure the three-dimensional velocity of the underwater robot by DVL (Doppler velocimeter); S2, converting the reference coordinate system of the DVL measurement data to the IMU coordinate system through the external reference between the DVL and the IMU (inertial measurement unit); S3, calculating the DVL factor covariance matrix by measuring the noise parameters of the DVL and IMU sensors and the external parameters between the DVL and IMU; S4, calculating the DVL residual through the system state estimation, the external parameters between the DVL and the IMU, and the DVL and IMU measurement data; S5. Calculate the Jacobian matrix of the DVL residual with respect to the system state estimate through the system state estimate and the external parameters between the DVL and the IMU.

2. A Doppler velocity meter residual construction method for underwater robot positioning according to claim 1, characterized in that: The measurement data of DVL in step S1 is as follows: The DVL calculates the velocity of the sensor in the direction of the corresponding beam by calculating the frequency shift of the transmitted and received acoustic beam signals, and then converts the velocity measurement results of multiple beams (usually 4) to obtain the velocity of the DVL on the XYZ axis of its body coordinate system. The superscript D represents the DVL body coordinate system (hereinafter referred to as the DVL coordinate system).

3. A Doppler velocity meter residual construction method for underwater robot positioning according to claim 2, characterized in that: The steps of step S2 are as follows: S201. In the underwater SLAM (simultaneous localization and mapping) positioning system, we use IMU as the reference coordinate system of the underwater robot. We need to convert the speed of DVL into the IMU body coordinate system (hereinafter referred to as IMU coordinate system). is the rotation matrix from the DVL coordinate system to the IMU coordinate system (usually known), for Inverse transform, is the velocity of DVL in the IMU coordinate system, which is expressed as follows: S202, because the speed of DVL in the IMU coordinate system Affected by the two modes of motion, rotation and translation, it can be decomposed into the rotational velocity component and translational velocity components Make For a specific underwater robot, the coordinates of the DVL in the IMU coordinate system are fixed and known, so the translational velocity component of the DVL in the IMU coordinate system is is equal to the speed of the IMU in the IMU coordinate system, that is, Similarly, the angular velocities of DVL and IMU in the IMU coordinate system are also equal. Assume that at the corresponding DVL measurement time, the angular velocity of IMU rotation in the IMU coordinate system is The coordinates of DVL in the IMU coordinate system are: And because the conversion relationship between angular velocity and linear velocity is So you can Rewrite as In order to facilitate derivation and subsequent implementation, we rewrite the vector cross multiplication in the above formula into the form of matrix multiplication Where K is 4. A Doppler velocity meter residual construction method for underwater robot positioning according to claim 3, characterized in that: The steps of step S3 are as follows: S301, calculate the relationship between the DVL sensor measurement noise and the DVL speed measurement value. Since the DVL measurement value is only affected by zero-mean Gaussian white noise n DVL ~N(0,Σ DVL ), the measurement model of DVL is: S302, calculate the equation relationship between the IMU angular velocity measurement noise and the velocity measurement value of the DVL in the IMU coordinate system. will be affected by zero bias b gyr and measurement noise n gyr (Assuming that it follows a zero-mean Gaussian distribution ), and its measurement model is: in is the true value of IMU angular velocity, we can get the relationship between the IMU angular velocity measurement value and the true value as follows: Substituting this into the equation in matrix multiplication form in step S2: S303, with the above equation relationship, calculate the equation relationship between the IMU angular velocity measurement noise and the velocity measurement value of the DVL in the IMU coordinate system: Will Substitute the equation into the above formula to get: set up The measurement noise n in this equation DVL and measurement noise n gyr Both obey zero-mean Gaussian distribution and remain zero-mean Gaussian distribution after linear transformation. In addition, the two measurement noises are usually independent of each other, so we can get the covariance matrix of Gaussian noise n as 5. A Doppler velocity meter residual construction method for underwater robot positioning according to claim 4, characterized in that: The steps of step S4 are as follows: Each position state of the underwater robot contains the quaternion estimate in the world coordinate system (Corresponding to the rotation matrix estimation from the IMU coordinate system to the world coordinate system ), velocity estimator And IMU angular velocity bias estimate We can use these three state estimators through equation Get a velocity estimate of the DVL in the IMU coordinate system in yes Inverse transform of . So we can estimate the speed and the actual speed value measured by DVL Constructing DVL velocity residuals 6. A Doppler velocity meter residual construction method for underwater robot positioning according to claim 5, characterized in that: The steps of step S5 are as follows: Through the residual formula of step S4, we can get the DVL velocity residual e DVL about and The Jacobian matrix in The definition is

Images