Low-cost integrated navigation system

Abstract

The invention provides a low-cost integrated navigation system which is used for navigation of an AUV in still water, a single-chip microcomputer receives measurement data of a pressure sensor and an attitude sensor, and a depth observation value of the AUV in the still water is calculated according to a measurement value of the pressure sensor at the top of the AUV. Calculating a transverse speed observation value of the AUV in the still water according to the difference between the measured values of the pressure sensors on the left side and the right side of the AUV, and correcting the measured values of the attitude sensors by using Kalman filtering according to the depth observation value and the transverse speed observation value of the AUV in the still water to obtain accurate state information of the AUV; and finally, zero-speed detection is performed to prevent error accumulation of the attitude sensor. The method adopts a multi-sensor fusion navigation algorithm, and has the advantages of high data fusion capability, low delay state, high dynamic adaptability and the like. Compared with traditional acoustic navigation, the method has the advantages that the reaction is faster and the effect is more sensitive under the dynamic condition, acoustic beacons do not need to be distributed in advance, and a more ideal navigation effect can be provided for the low-cost AUV.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater navigation and positioning, and in particular relates to a low-cost integrated navigation system. Background Art

[0002] Underwater unmanned vehicles (UUVs) must possess highly sensitive navigation and perception systems to ensure safe navigation in dangerous, deep waters. These systems often rely on traditional acoustic equipment like sonar, which suffers from near-field blind spots, large and expensive equipment, and active acoustic emissions that can easily disturb aquatic life.

[0003] The patent application, published on May 4, 2017, with publication number US20170123070A1 and titled "Underwater Acoustic Navigation System and Method for Autonomous Underwater Vehicles (AUVs)," employs multiple low-cost acoustic beacons deployed throughout the operating area. The AUV navigates via triangulation, offering advantages such as high robustness and scalability. However, this method requires the pre-deployment and maintenance of beacons, which is costly. Furthermore, the method suffers from a significant near-field blind spot and significant multipath effects within 10 meters.

[0004] The patent application, published on April 3, 2020, with publication number CN110954098A and titled "Single-beacon-based underwater robot acoustic navigation method and system," requires only one acoustic beacon and uses an ultra-short baseline (USBL) receiver onboard the AUV to measure the beacon's position and range. This method is more cost-effective and suitable for small areas and dynamic environments. However, this method suffers from large cumulative errors and a conical blind spot. Summary of the Invention

[0005] The object of the present invention is to provide a low-cost integrated navigation system.

[0006] A low-cost integrated navigation system for navigating an autonomous underwater vehicle (AUV) in still water comprises a pressure sensor, an attitude sensor, and a single-chip microcomputer. The pressure sensors are mounted on the top and left and right sides of the AUV, while the attitude sensor and single-chip microcomputer are mounted in a waterproof compartment inside the AUV. The single-chip microcomputer receives measurement data from the pressure and attitude sensors, calculates the depth observation value of the AUV in still water based on the measurement value of the pressure sensor on the top of the AUV, calculates the lateral velocity observation value of the AUV in still water based on the difference between the measurement values of the pressure sensors on the left and right sides of the AUV, and uses Kalman filtering to correct the measurement value of the attitude sensor based on the depth observation value and the lateral velocity observation value of the AUV in still water to obtain accurate state information of the AUV. Finally, zero-speed detection is performed to prevent error accumulation in the attitude sensor.

[0007] Furthermore, the single chip computer receives the measurement data of the pressure sensor and the posture sensor, and obtains the current t k The pressure sensor measurement value on the top of the AUV at this moment Measurement values of pressure sensors on the left and right sides of the AUV and Measurement data from the attitude sensor

[0008] in, is the position measurement value of the attitude sensor for the AUV, is the velocity measurement value of the AUV by the attitude sensor, is the heading angle measurement value of the AUV by the attitude sensor.

[0009] Furthermore, the single chip microcomputer measures the value of the pressure sensor on the top of the AUV Calculate the current t k The depth observation value of AUV in still water at time h k ;

[0010]

[0011] Among them, p atm is the standard atmospheric pressure in still water; ρ is the density of the fluid in still water; g is the acceleration due to gravity in still water.

[0012] Furthermore, the single chip microcomputer is based on the difference in the measured values of the pressure sensors on the left and right sides of the AUV. Calculate the current t k The observed lateral velocity of the AUV in still water at time

[0013]

[0014] Furthermore, the single chip microcomputer uses Kalman filtering to correct the measurement value of the attitude sensor to obtain accurate state information of the AUV;

[0015] Constructing attitude quaternion

[0016]

[0017] According to the previous moment t k-1 AUV state information, construct the state vector before correction

[0018]

[0019] in, is the displacement of AUV in three directions, is the acceleration of AUV in three directions,

[0020] Using Kalman filtering to filter the state vector Make corrections to obtain the precise state information x of the AUV k ;

[0021]

[0022] Among them, K k is the Kalman filter gain matrix; H k is the Jacobian matrix;

[0023] Accurate status information of AUVx k =[d x ,d y ,d z ,v xk ,v yk ,v zk ,q ωk ,q xk ,q yk ,q zk ,a x ,a y ,a z ,g].

[0024] Furthermore, the pressure sensor and attitude sensor are connected to the single-chip microcomputer through DuPont cables to realize data transmission and power supply. Two of the power supply DuPont cables are welded to form one male and four female cables. The two male cables are respectively connected to the 3V3 voltage output and GND of the single-chip microcomputer, and the female cables are connected to the VCC and GND of the pressure sensor and attitude sensor. The remaining DuPont cables are respectively connected to the SCL and SDA of the pressure sensor and the TX and RX of the attitude sensor to the soft serial port of the single-chip microcomputer; the waterproof cabin is connected to the AUV main control computer through an eight-core aviation plug to realize power supply and data transmission. The data transmission uses a stripped T568B network cable to connect to the single-chip microcomputer network port, and uses the TX+, TX-, RX+ and RX- therein to realize serial port data transmission; a power cord and a ground wire are used to power the single-chip microcomputer, and the three pressure sensors need to be waterproof packaged in advance and extend out of the waterproof cabin through the reserved openings.

[0025] The beneficial effects of the present invention are:

[0026] This invention utilizes a multi-sensor fusion navigation algorithm, offering advantages such as high data fusion capabilities, low latency, and strong dynamic adaptability. Compared to traditional acoustic navigation, this invention offers faster response and greater sensitivity in dynamic conditions (such as AUV obstacle avoidance), and eliminates the need for pre-deployed acoustic beacons, providing more optimal navigation for low-cost AUVs. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 Schematic diagram of the hardware connection method in the present invention.

[0028] Figure 2 It is a schematic diagram of the overall layout of the present invention.

[0029] Figure 3 Schematic diagram of the data transmission process in the present invention.

[0030] Figure 4 Schematic diagram of the data processing flow in the present invention. DETAILED DESCRIPTION

[0031] The present invention will be further described below with reference to the accompanying drawings.

[0032] The present invention provides a low-cost integrated navigation system for navigating an autonomous underwater vehicle (AUV) in still water, comprising a pressure sensor, a posture sensor, and a single-chip microcomputer. The pressure sensors are mounted on the top and left and right sides of the AUV, and the posture sensor and the single-chip microcomputer are mounted in a waterproof compartment inside the AUV. The single-chip microcomputer receives measurement data from the pressure sensor and the posture sensor, calculates a depth observation value of the AUV in still water based on the measurement value of the pressure sensor on the top of the AUV, calculates a lateral velocity observation value of the AUV in still water based on the difference between the measurement values of the pressure sensors on the left and right sides of the AUV, and uses a Kalman filter to correct the measurement value of the posture sensor based on the depth observation value and the lateral velocity observation value of the AUV in still water, thereby obtaining accurate state information of the AUV.

[0033] A navigation method for a low-cost integrated navigation system comprises the following steps:

[0034] Step 1: Get the current t k The pressure sensor measurement value on the top of the AUV at this moment Measurement values of pressure sensors on the left and right sides of the AUV and Measurement data from the attitude sensor in, is the position measurement value of the attitude sensor for the AUV, is the velocity measurement value of the AUV by the attitude sensor, is the heading angle measurement value of the AUV by the attitude sensor;

[0035] Step 2: Measure the pressure from the pressure sensor on top of the AUV Calculate the current t k The depth observation value of AUV in still water at time h k ;

[0036]

[0037] Among them, p atm is the standard atmospheric pressure in still water; ρ is the density of the fluid in still water; g is the acceleration due to gravity in still water;

[0038] Step 3: Based on the difference in the measured values of the pressure sensors on the left and right sides of the AUV Calculate the current t k The observed lateral velocity of the AUV in still water at time

[0039]

[0040] Step 4: Use Kalman filtering to correct the measurement value of the attitude sensor to obtain the accurate state information of the AUV;

[0041] Step 4.1: Constructing the attitude quaternion

[0042]

[0043] Step 4.2: According to the previous moment t k-1 AUV state information, construct the state vector before correction

[0044]

[0045] in, is the displacement of AUV in three directions, is the acceleration of AUV in three directions,

[0046] Step 4.3: Use Kalman filter to filter the state vector Make corrections to obtain the precise state information x of the AUV k ;

[0047]

[0048] Among them, K k is the Kalman filter gain matrix; H k is the Jacobian matrix;

[0049] Accurate status information of AUVx k=[d x ,d y ,d z ,v xk ,v yk ,v zk ,q ωk ,q xk ,q yk ,q zk ,a x ,a y ,a z ,g].

[0050] Example 1:

[0051] This paper addresses the shortcomings of existing acoustic navigation systems and proposes a hardware solution for a low-cost integrated navigation system. This system is equipped on an AUV (Automated Underwater Vehicle) as a carrier platform. The low-cost integrated navigation system primarily includes a single-chip microcontroller (MCU), a pressure sensor, and an attitude sensor. The pressure sensor is connected to the MCU via IIC communication, the attitude sensor is connected to the MCU via a serial port, and the MCU is connected to the AUV's main control computer via a serial port.

[0052] Combine Figure 1 and Figure 2 The hardware components of the proposed low-cost integrated navigation system are described below: a cylindrical, waterproof, pressure-resistant capsule 9, 10 cm long and 7 cm in diameter, containing a microcontroller 3, three pressure sensors 7, and an attitude sensor 4. The pressure and attitude sensors are connected to the microcontroller via DuPont cables for data transmission and power. Two of the power cables are soldered together, with one male connector and four female connectors. The two male connectors connect to the 3V3 voltage output and GND of the microcontroller, respectively, while the female connectors connect to the VCC and GND of the pressure and attitude sensors. The remaining DuPont cables connect the SCL and SDA pins of the pressure sensor, and the TX and RX pins of the attitude sensor, respectively, to the soft serial port 5 of the microcontroller. The capsule is connected to the AUV's main control computer via an eight-core aviation plug 8 for power and data transmission. A stripped T568B Ethernet cable is used to connect to the microcontroller's Ethernet port 1, with the TX+, TX-, RX+, and RX- pins used for serial data transmission. A 5V power cable and a ground wire 2 power the microcontroller. The three pressure sensors need to be waterproof packaged in advance and extend out of the waterproof compartment through the reserved opening 6.

[0053] Software: A data collection and packaging program, burned into the microcontroller, collects IIC signals from the three pressure sensors and the serial port signal from the attitude sensor via the soft serial port and outputs the collected data as a hexadecimal big-endian data packet. A data parsing and pressure processing program, pre-installed in the AUV's main control computer, restores the data packets to their original data strings and assists the AUV in navigation by performing pressure calculations.

[0054] Combine Figure 3 This low-cost integrated navigation system describes its workflow: Three pressure sensors, Sensor 1, Sensor 2, and Sensor 3, are placed above and on either side of the AUV. An attitude sensor, housed within the waterproof chamber, measures displacement and velocity in the X, Y, and Z directions (since the last time stamp). After acquiring the data, the microcontroller, through a programmed program, processes it into a hexadecimal data packet formatted as follows: x-displacement - y-displacement - z-displacement - x-velocity - y-velocity - z-velocity - measurement interval - pressure from Sensor 1 - pressure from Sensor 2 - pressure from Sensor 3 - flow rate - total time stamp since time stamp - checksum. This data is then output to the AUV's main control computer via the serial port.

[0055] Combine Figure 4 The navigation algorithm process is as follows: After receiving the data packet, the AUV's main control computer uses a pre-programmed procedure to convert it into standard data. First, the attitude sensor data is used to determine the AUV's current acceleration and angular velocity, thereby estimating its position. Pressure data is then used to determine the AUV's current depth and estimate the flow field velocity. Finally, a Kalman filter is used to estimate the optimal state, while zero-speed detection is performed to prevent the accumulation of inertial navigation errors. This allows the AUV to obtain its precise current position and begin navigation.

[0056] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be within the scope of protection of the present invention.

Claims

1. A low-cost integrated navigation system for AUV navigation in still water, characterized by: The system comprises a pressure sensor, an attitude sensor and a single-chip microcomputer; the pressure sensor is installed on the top and left and right sides of the AUV, and the attitude sensor and the single-chip microcomputer are installed in a waterproof cabin inside the AUV; the single-chip microcomputer receives measurement data from the pressure sensor and the attitude sensor, calculates the depth observation value of the AUV in still water based on the measurement value of the pressure sensor on the top of the AUV, calculates the lateral speed observation value of the AUV in still water based on the difference between the measurement values of the pressure sensors on the left and right sides of the AUV, and uses Kalman filtering to correct the measurement value of the attitude sensor based on the depth observation value and the lateral speed observation value of the AUV in still water to obtain accurate state information of the AUV; and finally performs zero-speed detection to prevent error accumulation in the attitude sensor.

2. The low-cost integrated navigation system according to claim 1, characterized in that: The single chip receives the measurement data of the pressure sensor and the posture sensor, and obtains the current t k The pressure sensor measurement value on the top of the AUV at this moment Measurement values of pressure sensors on the left and right sides of the AUV and Measurement data from the attitude sensor in, is the position measurement value of the attitude sensor for the AUV, is the velocity measurement value of the AUV by the attitude sensor, is the heading angle measurement value of the AUV by the attitude sensor.

3. The low-cost integrated navigation system according to claim 2, characterized in that: The single chip microcomputer measures the value of the pressure sensor on the top of the AUV Calculate the current t k The depth observation value of AUV in still water at time h k ; Among them, p atm is the standard atmospheric pressure in still water; ρ is the density of the fluid in still water; g is the acceleration due to gravity in still water.

4. The low-cost integrated navigation system according to claim 3, characterized in that: The single chip microcomputer is based on the difference in the measured values of the pressure sensors on the left and right sides of the AUV Calculate the current t k The observed lateral velocity of the AUV in still water at time 5. The low-cost integrated navigation system according to claim 4, characterized in that: The single chip microcomputer uses Kalman filtering to correct the measurement value of the attitude sensor to obtain accurate state information of the AUV; Constructing attitude quaternion According to the previous moment t k-1 AUV state information, construct the state vector before correction in, is the displacement of AUV in three directions, is the acceleration of AUV in three directions, Using Kalman filtering to filter the state vector Make corrections to obtain the precise state information x of the AUV k ; Among them, K k is the Kalman filter gain matrix; H k is the Jacobian matrix; Accurate status information of AUVx k =[d x ,d y ,d z ,v xk ,v yk ,v zk ,q ωk ,q xk ,q yk ,q zk ,a x ,a y ,a z ,g].

6. The low-cost integrated navigation system according to claim 1, characterized in that: The pressure sensor and attitude sensor are connected to the single-chip microcomputer through DuPont cables to realize data transmission and power supply. Two of the power supply DuPont cables are welded to form one male and four female cables. The two male cables are respectively connected to the 3V3 voltage output and GND of the single-chip microcomputer, and the female cables are connected to the VCC and GND of the pressure sensor and attitude sensor. The remaining DuPont cables are respectively connected to the SCL and SDA of the pressure sensor and the TX and RX of the attitude sensor to the soft serial port of the single-chip microcomputer; the waterproof cabin is connected to the AUV main control computer through an eight-core aviation plug to realize power supply and data transmission. The data transmission uses a stripped T568B network cable to connect to the single-chip microcomputer network port, and uses the TX+, TX-, RX+ and RX- therein to realize serial port data transmission; a power cord and a ground wire are used to power the single-chip microcomputer. The three pressure sensors need to be waterproof packaged in advance and extend out of the waterproof cabin through the reserved openings.

Images