Amphibious robot combined navigation method

Abstract

The application belongs to the field of navigation and positioning, and particularly relates to a combined navigation method for an amphibious robot. The method comprises the following steps: (1) navigation initialization: using the IMU, DVL and GNSS arranged on the amphibious robot to estimate the initial noise and state of the system; (2) nominal state prediction: using the median integral result of the IMU as the nominal state of the amphibious robot during navigation; (3) error state prediction: using error state Kalman filtering to calculate the discrete kinematics equation and the prediction covariance matrix of the error state; (4) observation update: using the DVL, pressure depth gauge and GNSS to construct the observation equation and the observation covariance matrix for correcting the nominal state. The application has faster response speed, smaller operation burden and more accurate pose estimation during the navigation of the amphibious robot.

Description

Technical Field

[0001] This invention belongs to the field of navigation and positioning, and specifically relates to a combined navigation method for amphibious robots. Background Technology

[0002] With the development of technology, amphibious robots are being used more and more widely in fields such as military, environmental monitoring, and disaster relief. To enable amphibious robots to achieve high-precision and high-stability navigation in complex environments, researchers have conducted in-depth studies on integrated navigation technology. Integrated navigation technology mainly improves navigation accuracy and stability by fusing information from multiple sensors, such as inertial navigation systems (INS), global positioning systems (GPS), and geomagnetic sensors.

[0003] Existing amphibious robot integrated navigation technologies are mainly based on the Extended Kalman Filter (EKF) method. These methods have improved navigation accuracy to some extent, but some problems still exist. EKF is time-consuming because it calculates the Jacobian every time the filter is optimized, and it also suffers from a loss of accuracy due to the neglect of second-order terms.

[0004] Error State Kalman Filtering (ESKF) is based on EKF and is generally used in navigation and positioning systems for unmanned vehicles. Land environments usually have GNSS sensors to prevent excessive accumulation of positioning errors. However, in water, GNSS signals are ineffective. Traditional ESKF only considers the state of the current frame and the previous frame. Without global observation constraints, errors accumulate continuously, causing the positioning error of amphibious robots to increase as the range increases. Summary of the Invention

[0005] The purpose of this invention is to provide a combined navigation method for amphibious robots, which enables pose prediction of amphibious robots and improves the accuracy and efficiency of the combined navigation system.

[0006] The technical solution to achieve the objective of this invention is: a combined navigation method for amphibious robots, comprising the following steps:

[0007] Step (1): Navigation static initialization: The initial noise and state of the system are estimated using the IMU, DVL and GNSS set on the amphibious robot;

[0008] Step (2): Nominal state prediction: Use the median integral result of the IMU as the nominal state of the amphibious robot during navigation;

[0009] Step (3): Error state prediction: Using error state Kalman filtering, calculate the discrete kinematic equations of the error state and the prediction covariance matrix;

[0010] Step (4): Observation update: Use DVL, pressure depth gauge and GNSS to construct observation equations and observation covariance matrix to correct the nominal state.

[0011] Furthermore, step (1) of navigation static initialization specifically includes the following steps:

[0012] Step (11): Estimate the initial noise of the system using an IMU;

[0013] Step (12): Use GNSS to estimate the position of the integrated navigation system and convert the coordinate information measured by GNSS into the ENU coordinate system used for navigation;

[0014] Step (13): The magnetometer and attitude sensor integrated inside the DVL measure the rotation vector of the robot and obtain the initial rotation matrix of the amphibious robot.

[0015] Furthermore, step (11) specifically involves:

[0016] The amphibious robot was placed stationary on the water surface. The angular velocity and acceleration outputs of the IMU were received during the stationary period, and the average values ​​were recorded as follows: and As the initial gyroscope and accelerometer zero bias of the IMU g and b a The measurement equation for the accelerometer is:

[0017]

[0018] Where g represents the acceleration due to gravity, η a For the noise term of the accelerometer, when the actual acceleration 'a' of the amphibious robot is zero, the rotation matrix R = I from the body coordinate system to the world coordinate system, and the actual acceleration measured by the accelerometer is b. a -g, ignore small amounts of b a The modulus of g is considered a fixed value, and the direction is taken as... A vector of size 9.81 is used as the gravity vector to initialize the gravity term;

[0019] Step (12) is as follows:

[0020] The system's position is estimated using GNSS. The coordinates measured by GNSS in the latitude, longitude and height coordinate system are converted to the coordinate system used for navigation - the northeast-sky coordinate system. The real-time coordinates of the amphibious robot measured by GNSS are denoted as P1 = [lat, lon, height], and the initial coordinates are denoted as P0 = [lat0, lon0, height0], where lat, lon, and height represent the longitude, latitude, and height of the target point, respectively.

[0021] First, transform to the Earth-centered Earth-fixed coordinate system ECEF, where the ECEF coordinates are P2 = [x, y, z]. The transformation relationship is as follows:

[0022]

[0023]

[0024]

[0025] In this model, the Earth is considered an ellipsoid, where 'a' is the major axis and 'b' is the minor axis.

[0026] Transform the ECEF coordinate system to the Northeast-East-South coordinate system. Let the target point in the Northeast-East-South coordinate system be P3 = [E, N, U]. The rotation matrix R1 at P1 is:

[0027]

[0028]

[0029] Based on the above calculations, the initial GNSS coordinates are set as the origin of the northeast celestial coordinate system to obtain subsequent position observations;

[0030] Step (13) is as follows:

[0031] The rotation vector of the amphibious robot was measured by integrating a magnetometer and attitude sensor within the DVL. According to Rodriguez formula θ is the magnitude of the rotation vector. Let be the direction of the rotation vector, and obtain the initial rotation matrix of the amphibious robot, where

[0032]

[0033] Furthermore, step (2) specifically includes the following steps:

[0034] Step (21): Set the state variables of the integrated navigation system as a 15-dimensional column vector:

[0035] X = [p, v, R, b] g ,b a ,g] · (36)

[0036] Where p represents displacement in the three-axis direction, v represents velocity in the three-axis direction, and R represents rotation matrix from the amphibious robot system to the world system;

[0037] Step (22): The equation of motion for continuous time is calculated as follows:

[0038]

[0039] The calculation formula for the IMU measurement equation is:

[0040]

[0041] in and η represents the acceleration and angular velocity measured by the IMU. a and η g Representing velocity random walk and angle walk respectively; the zero-bias model is as follows:

[0042]

[0043] Where t represents the current time and t′ represents the previous time;

[0044] Step (23): Rearranging equations (9)(10)(11), we obtain the motion equations of the nominal values ​​of the IMU's state variables in continuous time as follows:

[0045]

[0046] Discretizing the nominal state yields the following prediction equation for the nominal state:

[0047]

[0048] Where Δt represents the time interval between each sampling, the IMU measurement uses median integration, and the average value of the angular velocity and acceleration at the current moment and the next moment is taken as the measurement value at the current moment.

[0049] Furthermore, step (3) specifically includes the following steps:

[0050] Step (31): Using error state Kalman filtering, calculate the transfer equations for each state error and define the true state X of ESKF. t =[p t ,v t ,R t ,b at ,b gt ,g t ] · According to equation (12), the relationship between the derivatives of the state variables and the observations can be obtained:

[0051]

[0052] The calculation of the error state variable is defined as follows:

[0053]

[0054] Here, δ represents the error between the nominal state and the true value. The rotation part δR can be represented by its Lie algebra exp(δθ∧). The rotation matrix is ​​updated using a left perturbation model. The derivative of the error state with respect to time is expressed as follows:

[0055]

[0056] Differentiating both sides of equation (15) and substituting equations (12), (14), and (16) into the equations on both sides after differentiation, we can obtain the kinematic equations of the error state variables as follows:

[0057]

[0058] Similarly, discretizing equation (17) yields the discrete kinematic equations for the error state:

[0059]

[0060] Step (32): Calculate the covariance matrix of the prediction process. Based on the Kalman filter model, the state transition matrix is ​​obtained as follows:

[0061]

[0062] The state noise covariance matrix Q is calculated as follows:

[0063] Q = diag(03, cov(η) v ),cov(η θ ),cov(η g ),cov(η a ),03) (48)

[0064] According to equations (18), (19), and (20), the error state prediction process can be obtained as follows:

[0065]

[0066] Where, δx pred P represents the error state before the update at this moment. pred Let P represent the state process covariance matrix before the update at this moment, and let P be the state process covariance matrix at the previous moment. The error state δx will be set to 0 after each update.

[0067] Furthermore, step (4) includes the following steps:

[0068] Step (41): Construct the observation equation using DVL. The basic form of the observation equation is as follows:

[0069] Z=h(x)+υ,υ~N(0,V) (50)

[0070] Where Z represents the observed data, υ represents the observed noise, and V represents the covariance matrix of the observed noise;

[0071] Calculate the Jacobian matrix of the observation equation with respect to the error state:

[0072]

[0073] in,

[0074]

[0075] The rotational observation equation for DVL is calculated as follows:

[0076] R DVL =R·exp(δθ∧) (53)

[0077] Where R DVL It is a direct observation of δθ, therefore:

[0078] Z δθ =h(δθ)=Log(R) · R DVL (54)

[0079] The Jacobian of the observation equation with respect to δθ is calculated as follows:

[0080]

[0081] Step (42) uses GNSS to construct the observation equation, observes the position state, and calculates as follows:

[0082] Z δp =h(δp)=p gnss -p (56)

[0083] In this equation, the Jacobian of the state variables is the identity matrix;

[0084] Step (43): Use a pressure depth gauge to construct an observation equation and observe the displacement of the z-axis. When the amphibious robot dives to the bottom of the water, the GNSS signal fails. At this time, the observation sensors are DVL and pressure depth gauge. State constraints are applied to the amphibious robot to ensure the accuracy of the trajectory.

[0085] Step (44): Use the updated error state to correct the predicted nominal state, and obtain the final nominal state as the system output:

[0086]

[0087] Compared with the prior art, the significant advantages of this invention are:

[0088] The Jacobian matrix of this invention has a simpler form and calculation. At the same time, since the error state is small, the small second-order terms are ignored, which has little impact on the system accuracy, thereby improving the system accuracy and operating efficiency. In addition, this method improves on the traditional ESKF. The attitude variables are represented by rotation matrices. The attitude error state is multiplied by the nominal state on the left. Through derivation, it is found that the attitude error state update equation does not contain the measured value of angular velocity, avoiding the positioning error caused by excessive angular velocity noise, thereby further suppressing the cumulative error of amphibious robot combined navigation. Attached Figure Description

[0089] Figure 1 This is a flowchart of the integrated navigation process in a specific embodiment of the present invention.

[0090] Figure 2 This is a comparison diagram of positional errors in a specific embodiment of the present invention. Detailed Implementation

[0091] The present invention will now be described in further detail with reference to the accompanying drawings.

[0092] according to Figure 1 This invention is based on an error-state Kalman filter. It utilizes an IMU, GNSS, DVL, and pressure depth gauge mounted on an amphibious robot to initialize and estimate filter parameters, determining the initial zero bias, position, and attitude parameters for integrated navigation. During amphibious robot navigation, the median integral result of the IMU is used as the nominal state of the filter. In aquatic environments, GNSS is ineffective, and the DVL and pressure depth gauge are used for observation. In terrestrial environments, both DVL and pressure depth gauge are ineffective, and GNSS is used for observation. When navigating on the water surface, all sensors are effective. Simultaneously, their observation information is used to correct the error state, and the corrected error state is used to update the nominal state, thus constraining the robot's integrated navigation pose and suppressing its divergence.

[0093] A method for integrated navigation of an amphibious robot, the method comprising the following steps:

[0094] Navigation static initialization steps: Use the IMU (Inertial Measurement Unit), DVL (Doppler Velocimeter), and GNSS (Global Navigation Satellite System) installed on the amphibious robot to estimate the initial noise and state of the system;

[0095] Nominal state prediction step: Use the median integral result of the IMU as the nominal state of the amphibious robot's navigation process;

[0096] Error state prediction steps: Use error state Kalman filtering to calculate the discrete kinematic equations and prediction covariance matrix of the error state;

[0097] Observation update steps: Use DVL, pressure depth gauge and GNSS to construct observation equations and observation covariance matrix to correct nominal state.

[0098] The navigation static initialization step includes the following steps:

[0099] Step 1) Use an IMU to estimate the initial noise of the system. Place the amphibious robot stationary on the water surface and receive the angular velocity and acceleration outputs of the IMU during this period. Take the average values ​​and record them as follows: and As the initial gyroscope and accelerometer zero bias of the IMU g and b a The measurement equation for the accelerometer is:

[0100]

[0101] Where g represents the acceleration due to gravity, η a For the noise term of the accelerometer, when the actual acceleration 'a' of the amphibious robot is zero, the rotation matrix R = I from the machine system to the world coordinate system, and the actual acceleration measured by the accelerometer is b. a -g, ignore small amounts of b a The modulus of g can be considered a fixed value, and the direction is taken as... A vector of size 9.81 is used as the gravity vector to initialize the gravity term.

[0102] Step 2) Use GNSS to estimate the position of the integrated navigation system. The coordinates measured by GNSS are in the LLA coordinate system. Convert them to the coordinate system used for navigation - the East-North-Upper-Sky coordinate system (ENU). Let the real-time coordinates of the amphibious robot measured by GNSS be P1 = [lat,lon,height] and the initial coordinates be P0 = [lat0,lon0,height0], where lat,lon,height represent the longitude, latitude, and height of the target point, respectively.

[0103] First, transform it to the Earth-centered Earth-fixed coordinate system ECEF, where the ECEF coordinates are P2 = [x, y, z]. The transformation relationship is as follows:

[0104]

[0105]

[0106]

[0107] In this model, the Earth is considered an ellipsoid, where 'a' is the major axis and 'b' is the minor axis.

[0108] Transform the ECEF coordinates to the N-H coordinate system, and denote the target point under the East ENU coordinate system as P3 = [E, N, U]. The rotation matrix R1 at P1 is:

[0109]

[0110]

[0111] Based on the above calculations, the initial GNSS coordinates are set as the origin of the northeast celestial coordinate system, and subsequent position observations can be obtained.

[0112] Step 3) The DVL integrates a magnetometer and attitude sensor, which can measure the rotation vector of the amphibious robot. According to Rodriguez formula θ is the magnitude of the rotation vector. Let be the direction of the rotation vector, from which the initial rotation matrix of the amphibious robot can be obtained, where

[0113]

[0114] Furthermore, the nominal state prediction step includes the following steps:

[0115] Step 1) Set the state variables of the integrated navigation system as a 15-dimensional column vector:

[0116] X = [p, v, R, b] g ,b a ,g] · (8)

[0117] Where p represents the displacement in the three-axis direction, v represents the three-axis velocity, and R represents the rotation matrix from the amphibious robot system to the world system. Unlike the general method, the attitude of the filter is handled by the expression method of Lie group and Lie algebra. The high-dimensional column vector x falls on the high-dimensional manifold and is called the Kalman filter on the manifold. This approach makes the calculation form more concise.

[0118] Step 2) The equation of motion for continuous time is:

[0119]

[0120] The calculation formula for the IMU measurement equation is:

[0121]

[0122] in and η represents the acceleration and angular velocity measured by the IMU. a and η gLet these represent velocity random walk and angle walk, respectively; on the other hand, the zero bias in the normal case can be modeled as:

[0123]

[0124] Where t represents the current time and t′ represents the previous time.

[0125] Step 3) Simplifying equations (9), (10), and (11), we can obtain the equations of motion for the nominal values ​​of the IMU's state variables in continuous time:

[0126]

[0127] Discretizing the nominal state yields the following prediction equation for the nominal state:

[0128]

[0129] Where Δt represents the sampling time interval, the IMU measurement value is obtained by median integration, the angular velocity and acceleration at the current moment and the next moment are respectively averaged to obtain the measurement value at the current moment, so that the estimation result is more accurate;

[0130] Furthermore, the error state prediction step includes the following steps:

[0131] Step 1) Employ error state Kalman filtering to calculate the transfer equations for each state error and define the true state X of the ESKF. t =[p t ,v t ,R t ,b at ,b gt ,g t ] · According to equation (12), the relationship between the derivatives of the state variables and the observations can be obtained:

[0132]

[0133] The calculation of the error state variable is defined as follows:

[0134]

[0135] Where δ represents the error between the nominal state and the true value, the rotation part δR can be represented by its Lie algebra exp(δθ∧). The rotation matrix is ​​updated using a left perturbation model, and the derivative of the error state with respect to time can be expressed as follows:

[0136]

[0137] Differentiating both sides of equation (15) and substituting equations (12), (14), and (16) into the equations on both sides after differentiation, we can obtain the kinematic equations of the error state variables as follows:

[0138]

[0139] Similarly, discretizing equation (17) yields the discrete kinematic equations for the error state:

[0140]

[0141] Step 2) Calculate the covariance matrix of the prediction process. Based on the Kalman filter model, the state transition matrix can be obtained as follows:

[0142]

[0143] The state noise covariance matrix Q is calculated as follows:

[0144] Q = diag(03, cov(η) v ),cov(η θ ),cov(η g ),cov(η a ),03) (20)

[0145] According to equations (18), (19), and (20), the error state prediction process can be obtained as follows:

[0146]

[0147] Where, δx pred P represents the error state before the update at this moment. pred Let P represent the state process covariance matrix before the update at this moment, and let P be the state process covariance matrix at the previous moment. The error state δx will be set to 0 after each update.

[0148] Furthermore, the observation update step includes the following steps:

[0149] Step 1) Construct the observation equation using DVL. The basic form of the observation equation is as follows:

[0150] Z=h(x)+υ,υ~N(0,V) (22)

[0151] Where Z represents the observed data, υ represents the observed noise, and V represents the covariance matrix of the observed noise;

[0152] Calculate the Jacobian matrix of the observation equation with respect to the error state:

[0153]

[0154] in,

[0155]

[0156] The rotational observation equation for DVL is calculated as follows:

[0157] R DVL =R·exp(δθ∧) (25)

[0158] Where R DVL Since it is a direct observation of δθ, we have:

[0159] Z δθ =h(δθ)=Log(R) · R DVL (26)

[0160] The Jacobian of the observation equation with respect to δθ is calculated as follows:

[0161]

[0162] Step 2) Construct observation equations using GNSS, observe the position state, and calculate as follows:

[0163] Z δp =h(δp)=p gnss -p (28)

[0164] In this equation, the Jacobian of the state variables is the identity matrix;

[0165] Step 3) Use a pressure depth gauge to construct an observation equation and observe the displacement along the z-axis. When the amphibious robot dives to the bottom of the water, the GNSS signal becomes invalid. At this time, the observation sensors include DVL and pressure depth gauge. State constraints are applied to the amphibious robot to ensure the accuracy of the trajectory.

[0166] Step 4) Use the updated error state from the observations to correct the predicted nominal state, obtaining the final nominal state as the system output:

[0167] p t =p+δp,

[0168] v t =v+δv,

[0169] R t =δR·R

[0170] b gt =b g +δb g ,

[0171] b at =b a +δb a,

[0172] g t =g + δg.

[0173] according to Figure 2 Data collected by sensors is imported into a filter. After filter optimization, the estimated pose values ​​of each axis are output and compared with the true values ​​to create a trajectory error comparison chart.

[0174] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. For those skilled in the art, the present invention can have various forms. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for combined navigation of an amphibious robot, characterized in that, Includes the following steps: Step (1): Navigation static initialization: The initial noise and state of the system are estimated using the IMU, DVL and GNSS set on the amphibious robot; Step (2): Nominal state prediction: Use the median integral result of the IMU as the nominal state of the amphibious robot during navigation; Step (3): Error state prediction: Using error state Kalman filtering, calculate the discrete kinematic equations and prediction covariance matrix of the error state; Step (4): Observation update: Use DVL, pressure depth gauge and GNSS to construct observation equations and observation covariance matrix to correct nominal state; Step (2) specifically includes the following steps: Step (21): Set the state variables of the integrated navigation system as a 15-dimensional column vector: , in, Indicates displacement in three axes. Indicates the three-axis velocity. The rotation matrix representing the amphibious robot system to the world system; Step (22): The equation of motion for continuous time is calculated as follows: , The calculation formula for the IMU measurement equation is: , in and This indicates the acceleration and angular velocity measured by the IMU. and Representing velocity random walk and angle walk respectively; the zero-bias model is as follows: , in, This indicates the current time. Indicates the time of the previous moment; Step (23): By rearranging equations (9), (10), and (11), the equations of motion for the nominal values ​​of the IMU's state variables in continuous time are obtained as follows: , Discretizing the nominal state yields the following prediction equation for the nominal state: , in, This indicates the time interval between each sampling. The IMU measurement uses median integration, which averages the angular velocity and acceleration at the current and next time moments to obtain the measurement value at the current moment.

2. The method according to claim 1, characterized in that, Step (1) Navigation static initialization specifically includes the following steps: Step (11): Estimate the initial noise of the system using an IMU; Step (12): Use GNSS to estimate the position of the integrated navigation system and convert the coordinate information measured by GNSS into the ENU coordinate system used for navigation; Step (13): The magnetometer and attitude sensor integrated inside the DVL measure the rotation vector of the robot and obtain the initial rotation matrix of the amphibious robot.

3. The method according to claim 2, characterized in that, Step (11) is as follows: The amphibious robot was placed stationary on the water surface. The angular velocity and acceleration outputs of the IMU were received during the stationary period, and the average values ​​were recorded as follows: and As the initial gyroscope and accelerometer zero bias of the IMU and The measurement equation for the accelerometer is: , in Represents gravitational acceleration. For the noise term of the accelerometer, when the amphibious robot's actual acceleration... When the value is zero, the rotation matrix from the body coordinate system to the world coordinate system. The accelerometer actually measured Ignore small amounts , The modulus is considered a fixed value, and the direction is taken as... A vector of size 9.81 is used as the gravity vector to initialize the gravity term.

4. The method according to claim 3, characterized in that, Step (12) is as follows: The system's position is estimated using GNSS. The coordinates measured by GNSS in the latitude, longitude, and altitude coordinate system are converted to the navigation coordinate system—the northeast-central coordinate system. The real-time coordinates of the amphibious robot measured by GNSS are recorded. initial coordinates ,in These represent the longitude, latitude, and altitude of the target point, respectively. First, convert to the Earth-centered Earth-fixed (ECEF) coordinate system. The conversion relationship is as follows: , , , In this model, the Earth is considered an ellipsoid, where 'a' is the major axis and 'b' is the minor axis. ; Transform the ECEF coordinates to the Northeast-East-South coordinate system, and denote the target point in the Northeast-East-South coordinate system as... ,exist Rotation matrix at position for: , , Based on the above calculations, the initial GNSS coordinates are set as the origin of the northeast celestial coordinate system to obtain subsequent position observations.

5. The method according to claim 4, characterized in that, Step (13) is as follows: The rotation vector of the amphibious robot was measured by integrating a magnetometer and attitude sensor within the DVL. According to Rodriguez formula , Let be the magnitude of the rotation vector. Let be the direction of the rotation vector, and obtain the initial rotation matrix of the amphibious robot, where 。 6. The method according to claim 5, characterized in that, Step (3) specifically includes the following steps: Step (31): Using error state Kalman filtering, calculate the transfer equations for each state error and define the true state of ESKF. According to equation (12), the relationship between the derivatives of the state variables and the observations is obtained: , The calculation of the error state variable is defined as follows: , The symbol represents the error between the nominal state and the truth value, where the rotation part... Using its Lie algebra To represent this, the rotation matrix is ​​updated using a left perturbation model, and the derivative of the error state with respect to time is expressed as follows: , Differentiating both sides of equation (15) and substituting equations (12), (14), and (16) into the equations on both sides after differentiation, we obtain the kinematic equations of the error state variables as follows: , Similarly, discretizing equation (17) yields the discrete kinematic equations for the error state: , Step (32): Calculate the covariance matrix of the prediction process. Based on the Kalman filter model, the state transition matrix is ​​obtained as follows: , The state noise covariance matrix Q is calculated as follows: , According to equations (18), (19), and (20), the error state prediction process is obtained as follows: , in, This indicates the error state before the update at this moment. Let P be the state process covariance matrix before the current update, and P be the state process covariance matrix of the previous time step, representing the error state. It will be set to 0 after each update.

7. The method according to claim 6, characterized in that, Step (4) includes the following steps: Step (41): Construct the observation equation using DVL. The basic form of the observation equation is as follows: , in, For observation data, V represents the observation noise, and V is the covariance matrix of the observation noise. Calculate the Jacobian matrix of the observation equation with respect to the error state: , in, , The rotational observation equation for DVL is calculated as follows: , in Yes Therefore, through direct observation: , The observation equation for Jacobian is calculated as follows: , Step (42) Use GNSS to construct the observation equation, observe the position state, and calculate as follows: , In this equation, the Jacobian of the state variables is the identity matrix; Step (43): Use a pressure depth gauge to construct an observation equation and observe the displacement of the z-axis. When the amphibious robot dives to the bottom of the water, the GNSS signal fails. At this time, the observation sensors are DVL and pressure depth gauge. State constraints are applied to the amphibious robot to ensure the accuracy of the trajectory. Step (44): Use the updated error state from the observations to correct the predicted nominal state, and obtain the final nominal state as the system output: 。

Images