CN-121994209-A - Positioning and orientation method, device, equipment and medium for water surface robot

CN121994209ACN 121994209 ACN121994209 ACN 121994209ACN-121994209-A

Abstract

The application relates to the technical field of navigation of a water surface robot, and particularly provides a positioning and orientation method, a device, equipment and a medium of the water surface robot, wherein the method comprises the following steps of S1, analyzing whether effective navigation data are acquired at the current moment, if so, executing a step S3, and if not, executing a step S2; S2, predicting a state vector and an error covariance matrix at the next moment according to the forward acceleration of the carrier, the vertical angular velocity of the carrier and the state vector at the current moment, which are acquired at the current moment, based on a state transfer equation of the extended Kalman filtering, and returning to the step S1, and S3, correcting the state vector and the error covariance matrix at the current moment according to the effective navigation data based on an observation equation of the extended Kalman filtering, wherein the method can continuously obtain positioning and orientation information with high precision and high robustness.

Inventors

ZENG XIAOBIN
LI XIAOLEI
YANG BO

Assignees

超滑科技(佛山)有限责任公司

Dates

Publication Date: 20260508
Application Date: 20260327

Claims (10)

1. The positioning and orientation method for the water surface robot is characterized by comprising the following steps of: S1, analyzing whether effective navigation data are acquired at the current moment, if yes, executing a step S3, and if not, executing a step S2; S2, predicting a state vector and an error covariance matrix at the next moment according to a state transfer equation based on extended Kalman filtering, wherein the state vector comprises a predicted east relative position, a predicted north relative position, a predicted course angle, a predicted forward speed, a predicted angular speed bias and a predicted acceleration bias; S3, correcting the state vector and the error covariance matrix at the current moment according to the effective navigation data by using an observation equation based on extended Kalman filtering.
2. The water surface robot positioning and orientation method according to claim 1, wherein step S2 includes: s21, acquiring a time interval, wherein the time interval is the time difference between the current time and the maximum value in the time node of the last prediction and the last correction; S22, predicting the predicted east relative position of the next moment according to the time interval, the predicted east relative position of the current moment, the predicted forward speed and the predicted course angle by using a state transition equation of extended Kalman filtering; s23, predicting the predicted north relative position of the next moment according to the time interval, the predicted north relative position of the current moment, the predicted forward speed and the predicted course angle by using a state transition equation of extended Kalman filtering; S24, predicting a predicted course angle at the next moment according to the time interval, the predicted course angle at the current moment, the vertical angular velocity of the carrier and the bias of the predicted angular velocity by using a state transition equation of extended Kalman filtering; S25, predicting the predicted forward speed at the next moment according to the time interval, the predicted forward speed at the current moment, the carrier forward acceleration and the predicted acceleration bias by using a state transition equation of extended Kalman filtering; S26, predicting the predicted angular velocity bias at the next moment according to the predicted angular velocity bias at the current moment by using the state transfer equation of the extended Kalman filter, and predicting the predicted acceleration bias at the next moment according to the acceleration bias at the current moment by using the state transfer equation of the extended Kalman filter; s27, integrating the predicted east relative position, the predicted north relative position, the predicted course angle, the predicted forward speed, the predicted angular speed bias and the predicted acceleration bias at the next moment into a state vector at the next moment; s28, generating a state transition Jacobian matrix at the next moment according to the state vector at the current moment; S29, generating an error covariance matrix of the next moment according to the error covariance matrix of the current moment, the noise covariance matrix of the motion model and the state transition Jacobian matrix of the next moment.
3. The positioning and orientation method of a water surface robot according to claim 1, wherein the calculation formula of the predicted east-direction relative position at the next moment is: ; Wherein x t+1y represents the predicted east relative position at the next time, x ty represents the predicted east relative position at the current time, v ty represents the predicted forward speed at the current time, θ ty represents the predicted heading angle at the current time, and Δt represents the time interval; the calculation formula of the predicted north relative position at the next moment is as follows: ; Wherein y t+1y represents the predicted relative north position at the next time, and y ty represents the predicted relative north position at the current time; The calculation formula of the predicted course angle at the next moment is as follows: ; Wherein θ t+1y represents the predicted heading angle at the next time, ω t represents the carrier vertical angular velocity at the current time, and ω bty represents the predicted angular velocity bias at the current time; The calculation formula of the predicted forward speed at the next moment is as follows: ; Wherein v t+1y represents the predicted forward speed at the next time, a t represents the carrier forward acceleration at the current time, and a bty represents the predicted acceleration bias at the current time; The calculation formula of the predicted angular velocity bias at the next moment is as follows: ; Wherein ω bt+1y represents the predicted angular velocity bias at the next time; the calculation formula of the predicted acceleration bias at the next moment is as follows: ; wherein a bt+1y represents the predicted acceleration bias at the next time; The calculation formula of the state transition Jacobian matrix at the next moment is as follows: ; wherein F represents the state transition Jacobian matrix at the next moment; the calculation formula of the error covariance matrix at the next moment is as follows: ; Wherein, P t+1 represents the error covariance matrix at the next time, P t represents the error covariance matrix at the current time, F T represents the inversion of the state transition jacobian matrix at the next time, and Q t represents the motion model noise covariance matrix at the current time.
4. The water surface robot positioning and orientation method according to claim 1, wherein the effective navigation data includes an effective water surface robot longitude, an effective water surface robot latitude, an effective heading angle, and an effective forward speed, and step S3 includes: S31, converting the longitude of the effective water surface robot and the latitude of the effective water surface robot into plane coordinates relative to a preset reference point to obtain an effective east-direction relative position and an effective north-direction relative position; S32, integrating the effective east relative position, the effective north relative position, the effective course angle and the effective forward speed into an observation vector; S33, generating a current state vector observation value according to a state vector at the current moment by using an observation equation of extended Kalman filtering, and generating an observation Jacobian matrix at the current moment according to the current state vector observation value; s34, generating Kalman gain at the current moment according to the error covariance matrix at the current moment, the observation model noise covariance matrix and the observation Jacobian matrix; s35, correcting the state vector at the current moment according to the Kalman gain and the difference between the observed vector and the current state vector observed value; S36, correcting the error covariance matrix at the current moment according to the six-order identity matrix, the Kalman gain and the observation Jacobian matrix.
5. The water surface robot positioning and orientation method according to claim 1, wherein the expression of the observation vector is: ; Wherein z t represents an observation vector at the current time, x tg represents an effective east-direction relative position at the current time, y tg represents an effective north-direction relative position at the current time, θ tg represents an effective heading angle at the current time, and v tg represents an effective forward speed at the current time; the calculation formula of the current state vector observation value is as follows: ; Wherein h (x t ) represents a current state vector observation value at the current time, x ty represents a predicted east-direction relative position at the current time, y ty represents a predicted north-direction relative position at the current time, θ ty represents a predicted heading angle at the current time, and v ty represents a predicted forward speed at the current time; the calculation formula of the Kalman gain is as follows: ; Wherein K t represents the Kalman gain at the current time, P t represents the error covariance matrix at the current time, H T represents the inversion of the observation Jacobian matrix, and R t represents the observation model noise covariance matrix at the current time; the process of correcting the state vector at the current time is as follows: ; Wherein x tjz represents the corrected state vector at the current time, and x t represents the state vector at the current time; the process of correcting the error covariance matrix at the current moment is shown as follows: ; wherein, P tjz represents the error covariance matrix of the corrected current moment, and I represents the six-order identity matrix.
6. The method for positioning and orienting a water surface robot according to claim 1, wherein the effective navigation data is acquired by a global navigation satellite system, and the carrier forward acceleration and the carrier vertical angular velocity are acquired by an inertial sensor.
7. The method according to claim 1, wherein the initial values of the predicted angular velocity bias and the predicted acceleration bias are both 0, the initial value of the predicted forward velocity is an effective forward velocity in the first piece of effective navigation data collected by the global navigation satellite system, the initial value of the predicted heading angle is an effective heading angle in the first piece of effective navigation data collected by the global navigation satellite system, the initial value of the predicted east relative position is an east relative position of longitude and latitude of the water surface robot in the first piece of effective navigation data collected by the global navigation satellite system relative to a preset reference point, and the initial value of the predicted north relative position is a north relative position of longitude and latitude of the water surface robot in the first piece of effective navigation data collected by the global navigation satellite system relative to the preset reference point.
8. The utility model provides a surface of water robot positioning orientation device which characterized in that, surface of water robot positioning orientation device includes: The analysis module is used for analyzing whether the effective navigation data are acquired at the current moment, if yes, the correction module is triggered, and if no, the prediction module is triggered; the prediction module is used for predicting a state vector and an error covariance matrix at the next moment according to the forward acceleration of the carrier, the vertical angular velocity of the carrier and the state vector at the current moment, which are acquired at the current moment, based on a state transfer equation of the extended Kalman filtering, and then triggering the analysis module, wherein the state vector comprises a predicted east relative position, a predicted north relative position, a predicted course angle, a predicted forward velocity, a predicted angular velocity bias and a predicted acceleration bias; and the correction module is used for correcting the state vector and the error covariance matrix at the current moment according to the effective navigation data based on the extended Kalman filtering observation equation.
9. An electronic device comprising a processor and a memory storing computer readable instructions that, when executed by the processor, perform the steps in the method of any of claims 1-7.
10. A computer readable storage medium, on which a computer program is stored, characterized in that the computer program, when being executed by a processor, performs the steps of the method according to any of claims 1-7.

Description

Positioning and orientation method, device, equipment and medium for water surface robot Technical Field The application relates to the technical field of navigation of water robots, in particular to a positioning and orientation method, a device, equipment and a medium for the water robots. Background The surface robot requires accurate positioning and orientation capabilities when performing tasks. Conventional positioning techniques rely primarily on a single sensor of the Global Navigation Satellite System (GNSS) or Inertial Measurement Unit (IMU). GNSS provides absolute position information with high accuracy, but is susceptible to interference such as signal shielding and multipath effects in a water environment, so that positioning failure or accuracy is significantly reduced. Especially in complex water areas such as bridges and ports, the quality of GNSS signals is more difficult to ensure. Although the IMU system is not interfered by external signals and can autonomously calculate pose information, the intrinsic sensor drift can cause error accumulation along with time, and the position deviation can exponentially increase under long-time work or dynamic water surface environment. The accumulated errors are particularly obvious in dynamic environments such as water surface wave disturbance and the like, and seriously influence the positioning accuracy and reliability of the water surface robot. In the aspect of orientation technology, the existing method mainly relies on an IMU system, and the initial heading is set to be the true north direction, and the angular velocity in the vertical direction is integrated to obtain the heading angle. The method has two main defects that firstly, the obtained course angle is a relative angle relative to a starting point, absolute direction reference cannot be provided, and secondly, the calculation error of the course angle can be accumulated continuously along with time due to the inherent drift characteristic of the inertial sensor, and especially, the course deviation can reach an unacceptable degree in the long-time task execution process. Such heading errors can further affect the accuracy of the position estimate, forming a positive feedback loop of error. The prior art lacks a robust positioning and orientation method capable of effectively fusing information of multisource sensors and adapting to the characteristics of the dynamic environment of the water surface. Especially when the GNSS signal fails or the precision is reduced, how to maintain the continuity and accuracy of positioning and orientation becomes a key technical bottleneck for restricting the application of the surface robot. Meanwhile, the existing method has insufficient on-line compensation capability for sensor errors, and is difficult to cope with the problem of error accumulation in the long-time task execution process. In view of the above problems, no effective technical solution is currently available. Disclosure of Invention The application aims to provide a positioning and orientation method, a device, equipment and a medium for a water surface robot, which can continuously obtain positioning and orientation information with high precision and high robustness. In a first aspect, the present application provides a positioning and orientation method for a water surface robot, comprising the steps of: S1, analyzing whether effective navigation data are acquired at the current moment, if yes, executing a step S3, and if not, executing a step S2; S2, predicting a state vector and an error covariance matrix at the next moment according to a carrier forward acceleration acquired at the current moment, a carrier vertical angular velocity acquired at the current moment and a state vector at the current moment based on a state transfer equation of extended Kalman filtering, and returning to the step S1, wherein the state vector comprises a predicted east relative position, a predicted north relative position, a predicted course angle, a predicted forward velocity, a predicted angular velocity bias and a predicted acceleration bias; S3, correcting the state vector and the error covariance matrix at the current moment according to the effective navigation data by using an observation equation based on the extended Kalman filtering. According to the positioning and orientation method for the water surface robot, provided by the application, the defects of a single sensor are effectively overcome by the strategy of correcting when effective navigation data are available and predicting when the effective navigation data are not available, and the positioning and orientation information with high accuracy and high robustness can be continuously obtained in a complex and changeable water area environment by the dynamic adaptability of the water surface robot, so that the autonomous operation capacity and the task execution efficiency of the water surface robot are improved. Optionally, step S