CN104197927A - Real-time navigation system and real-time navigation method for underwater structure detection robot - Google Patents

Abstract

The invention discloses a real-time navigation system and a real-time navigation method for an underwater structure detection robot. The navigation system comprises a magnetic compass, a gyroscope, an accelerometer, a depth meter and a navigation microprocessor, wherein the magnetic compass, the gyroscope, the accelerometer and the depth meter are used for respectively collecting magnetic field intensity, an angular speed, a linear speed and submerged depth data and transmitting the magnetic field intensity, the angular speed, the linear speed and the submerged depth data to the navigation microprocessor; the navigation microprocessor is used for calculating attitude and position of the underwater robot according to the collected data. The navigation method comprises an attitude algorithm, a speed algorithm and a depth algorithm; according to the attitude algorithm, a complementary filtering method, a quaternion gradient descent method and a Kalman algorithm are combined for obtaining an attitude matrix and an attitude angle; the speed algorithm is used for calculating the speed and the position of the robot by using a three-order upwind scheme with rotary compensation; the depth algorithm is used for processing the data of the depth meter by using a moving average filter algorithm so as to obtain the submerged depth. By virtue of the real-time navigation system for the underwater structure detection robot and the method thereof, the navigation cost is reduced and a relatively good navigation precision is achieved.

Description

Submerged structure detects robot real-time navigation system and method

Technical field

The present invention relates to a kind of underwater robot real-time navigation technology, relate in particular to Algorithms of Robots Navigation System and method that a kind of submerged structure that is applied to oceanographic engineering detects.

Background technology

Remote underwater robot (Remotely Operated Vehicle, ROV) be a kind of underwater operation robot contacting by umbilical cable and water surface supporting platform, by water surface control system and under water submerge body two parts form, be widely used in the operations such as underwater observation, sea floor exploration, submerged structure maintenance, subsea pipeline detection and installation under water, become the critical equipment of oceanographic engineering submerged structure safety detection and maintenance.

Precision navigation is the important content that submerged structure detects robot, for underwater structure detects the support that provides the necessary technical.Existing underwater navigation technology, such as underwater sound navigation, inertial navigation, utilize the vision navigation method of sound wave image, optics etc., all can directly be applied on underwater vehicle, but have shortcoming separately.Integrated navigation is the more perfect navigational system of performance that many navigational system are constituted, and can learn from other's strong points to offset one's weaknesses, and improves navigation accuracy, is the developing direction of following airmanship.At present, underwater navigation is main mainly with inertial navigation greatly, is aided with other navigator as doppler velocity registering instrument (DVL), magnetic heading (MCP) and GPS etc., but these equipment or bulky, underwater navigation poor performance, or with high costs, obvious defect there is.

The navigator of employing based on MEMS (micro electro mechanical system) (MEMS), combination inertia assembly is as devices such as magnetic compass, depthometers, improve existing navigation algorithm, can reduce to the full extent accumulated error, suppress attitude error, obtain the good navigation effect that meets underwater detecting robot demand.The patent documentation that application number is " 2013100128123 " discloses a kind of " the line terrain match air navigation aid of underwater robot ", but reliability is lower, and navigation postpones large; The patent documentation that application number is " 2012103320229 " discloses a kind of " autonomous underwater vehicle combined navigation system and method ", and its navigational system complex structure is with high costs, and GPS is not suitable for underwater navigation.

Summary of the invention

The object of this invention is to provide a kind of submerged structure and detect robot real-time navigation system and method, for underwater structure detects ROV, design practical navigational system, carry and high-precisionly reducing costs simultaneously.

Object of the present invention is achieved by the following technical programs:

A kind of submerged structure detects Algorithms of Robots Navigation System, comprise magnetic compass 1, gyroscope 2, accelerometer 3, depthometer 4, navigation microprocessor 5, described magnetic compass 1, gyroscope 2, accelerometer 3 and depthometer 4 gather respectively underwater robot magnetic field intensity, angular velocity, linear velocity and submerged depth data, transfer to navigation microprocessor 5; The data that 5 pairs of magnetic compasses 1 of described navigation microprocessor, gyroscope 2, accelerometer 3, depthometer 4 gather are carried out A/D conversion; The data that described navigation microprocessor 5 gathers magnetic compass 1, gyroscope 2, accelerometer 3 in conjunction with hypercomplex number gradient descent method, complementary filter, Kalman Algorithm are carried out data fusion, obtain attitude matrix and attitude angle; The data acquisition that 5 pairs of gyroscopes 2 of described navigation microprocessor, accelerometer 3 gather is used with three rank of rotation compensation Algorithm for Solving speed and position windward; 5 pairs of depthometers of described navigation microprocessor adopt moving average filter to ask for underwater robot submerged depth, obtain integrated navigation information.

Submerged structure detects a robot navigation method, take hypercomplex number and angle error as filtering estimation object, first carries out complementary filter, and gyroscope survey value is done to preliminary compensation; Next uses gradient descent method to upgrade hypercomplex number, effectively reduces attitude angle drift; Carry out again Kalman filtering, adopt the UD decomposition method in matrix, avoid filter divergence, obtain underwater robot attitude matrix and attitude angle; Then accelerometer and gyro data are used to three rank Algorithm for Solving speed and position windward.Meanwhile, depthometer image data being carried out to moving average filter, obtain underwater robot depth value, takes this as the standard in the vertical position of underwater robot.The method comprises the following steps:

1) attitude, speed, location compute, process is as follows:

(1) the discrete Karman equation that underwater vehicles navigation system adopts is:

X _k＝Φ _k，k-1X _k-1+W _k-1

Z _k＝H _kX _k+V _k

In formula, k represents system discrete sampling time point, k>=0; X _kfor k system estimation quantity of state constantly; Φ _{k, k-1}for X _k-1to X _kmatrix of shifting of a step; W _k-1for system incentive Gaussian sequence, average is E[w (k)]=0, variance is E[w (k) w (k) ^t]=Q _k, Q _kit is system noise variance; Z _kfor k sensor measuring value constantly; H _kfor measurement matrix, the mathematical relation between reacting dose measurement and estimator; V _kfor measuring value Gaussian sequence, average is E[v (k)]=0, variance is E[v (k) v (k) ^t]=R _k, R _kmeasure noise variance; W _k, V _kuncorrelated mutually;

(2) determine parameter original state, comprising:

Local gravitational acceleration g, systematic sampling time t, angular velocity error delta ω ₁, Δ ω ₂, Δ ω ₃, complementary filter parameter k _p, k _i, by estimated state amount initial value estimate square error matrix initial value P _0|0; K wherein _p, k _ivalue rule is: k _p+ k _i=1,0.95 < k _p< 1, by test of many times, determines optimal value; p _0|0press following value: setting navigation coordinate is sky, northeast coordinate system, is designated as n system; Setting body axis system is b system, and initial time overlaps with navigation coordinate system, and n system and b system are three axle orthogonal coordinate systems; Hypercomplex number denotation coordination rotation relationship Q=[q ₀, q ₁, q ₂, q ₃], q wherein ₀for rotation scalar, q ₁, q ₂, q ₃for rotation of coordinate vector, with hypercomplex number statement n, being tied to b is that transformational relation is expressed as:

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

And b is tied to n, be r _ijeach element value in (1≤i, j≤3) representing matrix;

(3) utilize the frequency domain complementary characteristic correction angle velocity of gyroscope and magnetic compass, accelerometer, process is as follows:

If magnetic compass measured value is m _b, acceleration measuring value is a, gyroscope survey value is ω, m _b, a, ω be 3 * 1 column matrix;

First magnetic field amount being transformed into navigation coordinate by body axis system is $m_n=C_b^n{m}_b={\left[\begin{array}{ccc}{m}_{\mathrm{nx}}& m_{\mathrm{ny}}& m_{\mathrm{nz}}\end{array}\right]}^T,$ And calculated level and vertical direction magnetic field: by=0, bz=m _nz;

Then magnetic field is transformed into body axis system: $w=C_n^b{\left[\begin{array}{ccc}\mathrm{bx}& \mathrm{by}& \mathrm{bz}\end{array}\right]}^T,$ And utilize the 3rd row are estimated acceleration of gravity vector:

Calculate again acceleration and field compensation error:

In formula, " * " represents vector multiplication;

Obtaining cumulative errors is: errInt=Ki ∑ err*t;

Finally obtain the angular velocity vector after proofreading and correct: ω _c=ω+k _p* err+errInt

Wherein, K _i, K _pit is complementary filter parameter;

(4) utilize gradient descent method to upgrade hypercomplex number:

By obtain new hypercomplex number q ₀, q ₁, q ₂, q ₃, wherein, μ is the step value of Gradient Descent, by determine; $\&dtri;f=\frac{\&PartialD;f}{\&PartialD;Q},f(Q,d,s)=Q*\&CircleTimes;d\&CircleTimes;Q-s,$ D is the measured value of sensor based on body axis system, and s is the actual value of sensor based on navigation coordinate system, and " * " represents adjoint matrix, " " be gradient multiplication, " ο " is hypercomplex number multiplication;

(5) Kalman filtering process decomposing based on UD is followed successively by:

State one-step prediction:

UD decomposes: H _kp _k/k-1h _k ^t+ R=U ∧ D;

One-step prediction square error: P _k/k-1=Φ _{k, k-1}p _k-1/k-1Φ _k/k-1 ^t+ Q _k-1;

Calculation of filtered gain: K _k=P _k/k-1h _k ^t(H _kp _k/k-1h _k ^t+ R) ^-1=P _k/k-1h _k ^t(UDU ^t) ^-1;

Calculate new breath (new breath is calculated and obtained by up-to-date measuring value):

State Estimation is calculated: ${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k{\mathrm{\&gamma;}}_k;$

Estimate square error: P _k/k=(I-K _kh _k) P _k/k-1;

(6) use three rank upstreame schemes to solve speed and position, and carry out rotation compensation, calculate according to the following procedure:

If accelerometer is respectively a at the k moment, the k-1 moment, k-2 measured value constantly _k, a _k-1, a _k-2, gyroscope is respectively ω at the k moment, the k-1 moment, k-2 measured value constantly _k, ω _k-1, ω _k-2, make T=2t, T _m, T _m-1, T _m-2speed is constantly made as respectively v _m, v _m-1, v _m-2, T _m, T _m-1position is constantly respectively P _m, P _m-1;

First calculate speed increment and the angle increment of two sampling instants:

Speed increment is $\mathrm{\&Delta;v}=T(\frac{1}{6}{a}_k+\frac{4}{6}{a}_{k-1}+\frac{1}{6}{a}_{k-2});$

Angle increment is $\mathrm{\&Delta;\&theta;}=T(\frac{1}{6}{\mathrm{\&omega;}}_k+\frac{4}{6}{\mathrm{\&omega;}}_{k-1}+\frac{1}{6}{\mathrm{\&omega;}}_{k-2});$

Then adopt three rank upstreame schemes to solve speed and position:

Speed is $v_m=v_{m-1}+C_b^n(\frac{T(5a_k+8a_{k-1}-a_{k-2})}{12}+\frac{1}{2}\mathrm{\&Delta;v}\&times;\mathrm{\&Delta;\&theta;})$

Position is $P_m=P_{m-1}+\frac{T(5v_m+8v_{m-1}{v}_{m-2})}{12}$

In formula, be rotation compensation part, " * " represents vector multiplication;

2) depthometer resolves

The power end points such as depthometer employing are smoothly made to moving average filter, calculate underwater robot submerged depth:

$\mathrm{depth}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_i$

Constantly reach slidably one by one m the adjacent data average computation that counts, in formula, d _ibe i the data that depthometer gathers, depth is filtered depth value.

Object of the present invention can also further be achieved by following technical measures:

Aforementioned submerged structure detects Algorithms of Robots Navigation System, and wherein magnetic compass 1 adopts the electronic compass IC HMC5883L of Honeywell company.

Aforementioned submerged structure detects Algorithms of Robots Navigation System, and wherein gyroscope 2, accelerometer 3 are micro electronmechanical (MEMS) products that InvenSense company produces.

Aforementioned submerged structure detects Algorithms of Robots Navigation System, and wherein depthometer 4 is diffuse si type pressure units, has good precision in 100m range ability.

Aforementioned submerged structure detects Algorithms of Robots Navigation System, and the microprocessor 5 that wherein navigates adopts based on ARMCortex ^tMthe STM32F103T8 of-M3 kernel.

Compared with prior art, the invention has the beneficial effects as follows: compare with existing navigational system and algorithm, adopt the hypercomplex number update algorithm based on gradient descent method, effectively reduce attitude drift; To expanded Kalman filtration algorithm, adopt the UD decomposition method in matrix, effectively suppression filter is dispersed.

Accompanying drawing explanation

Fig. 1 is navigational system structural drawing of the present invention;

Fig. 2 is that air navigation aid of the present invention is resolved process flow diagram;

Fig. 3 is hypercomplex number gradient descent method schematic diagram of the present invention;

Fig. 4 is Kalman filter theory figure of the present invention.

Embodiment

Below in conjunction with the drawings and specific embodiments, the invention will be further described.

Basic thought of the present invention is, based on micro-electromechanical device, utilize hypercomplex number gradient descent method, complementary filter, Kalman filtering to merge the attitude information that navigation information obtains underwater robot, use three rank upstreame schemes to solve speed and the position of underwater robot, adopt moving average filter to calculate underwater robot submerged depth, can reduce costs to a certain extent, improve precision.

As shown in Figure 1, a kind of submerged structure detects Algorithms of Robots Navigation System, by magnetic compass 1, gyroscope 2, accelerometer 3, depthometer 4, navigation microprocessor 5, formed, described magnetic compass 1, gyroscope 2, accelerometer 3 and depthometer 4 gather respectively underwater robot magnetic field intensity, angular velocity, linear velocity and submerged depth data, transfer to navigation microprocessor 5, wherein magnetic compass 1, gyroscope 2, accelerometer 3 are three-axis sensors, and depthometer 4 is the devices based on the water-pressure survey degree of depth; 5 pairs of magnetic compasses 1 of described navigation microprocessor, gyroscope 2, accelerometer 3, depthometer 4 image data are carried out 16 A/D conversions, carry out data fusion, filtering, calculate attitude angle, speed and the position of underwater robot, and transfer to the microcontroller under water 6 in Fig. 1.That wherein magnetic compass 1 adopts is Honeywell electronic compass IC HMC5883L, gyroscope 2, accelerometer 3 are micro electronmechanical (MEMS) products that InvenSense company produces, depthometer 4 is diffuse si type pressure units, in 100m range ability, have good precision, navigation microprocessor 5 adopts based on ARM Cortex ^tMthe STM32F103T8 of-M3 kernel.

As shown in Figure 2, a kind of submerged structure detects robot navigation method, and magnetic compass, gyroscope, accelerometer, depthometer image data are carried out to data fusion.First to Kalman initial, to hypercomplex number initialization, to sampling time, acceleration of gravity initialization.Then carry out complementary filter, gyroscope survey value angular acceleration is done to preliminary compensation; Use gradient descent method to upgrade hypercomplex number, effectively reduce attitude angle drift; Carry out Kalman filtering, adopt the UD decomposition method in matrix, dispersing of suppression filter, obtains underwater robot attitude and position; To accelerometer and gyro data, use three rank upstreame schemes to solve speed and position.Meanwhile, depthometer image data being carried out to moving average filter, obtain underwater robot depth value, takes this as the standard in the vertical position of underwater robot.As shown in Figure 2, in conjunction with specific embodiments, performing step is as follows:

1. parameter initialization: local gravitational acceleration g=9.79973, sampling time t=0.02s, hypercomplex number initial value q ₀=1, q ₁=0, q ₂=0, q ₃=0, angular velocity error delta ω ₁=0, Δ ω ₂=0, Δ ω ₃=0; According to complementary filter principle, determine filtering parameter k _p=0.0132, k _i=0.9868; System noise variance Q _kwith measurement noise variance R _kfor:

Q＝diag([0.0001?0.0001?0.0001?0.0001?0.005?0.005?0.005])

R＝diag([0.005?0.005?0.005?0.003?0.003?0.003])

Estimate square error matrix initial value:

P _0/0＝diag([0.001?0.001?0.001?0.001?0.005?0.005?0.005])

By estimated vector initial value:

Setting navigation coordinate is sky, northeast coordinate system, is designated as n system; Setting body axis system is b system, initially overlaps with navigation coordinate system, and it is that transformational relation is expressed as that hypercomplex number statement n is tied to b:

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

$C_b^n={\left(C_n^b\right)}^T$

In formula, expression is tied to the conversion of body axis system from navigation coordinate, the conversion that expression is from body axis system to navigation coordinate.

2. utilize the thought of gyroscope and accelerometer, the complementation of magnetic compass frequency domain, angular acceleration compensation:

If magnetic compass measured value is m _b, acceleration measuring value is a, gyroscope survey value is ω, m _b, a, ω be 3 * 1 column matrix.

1) magnetic field amount being transformed into navigation coordinate by body axis system is $m_n=C_b^n{m}_b={\left[\begin{array}{ccc}{m}_{\mathrm{nx}}& m_{\mathrm{ny}}& m_{\mathrm{nz}}\end{array}\right]}^T,$ Then

Calculated level and vertical direction magnetic field: by=0; Bz=m _nz;

2) magnetic field is transformed into body axis system: $w=C_n^b{\left[\begin{array}{ccc}\mathrm{bx}& \mathrm{by}& \mathrm{bz}\end{array}\right]}^T;$

3) utilize the 3rd row are estimated acceleration of gravity vector:

4) calculate acceleration and field compensation error: obtaining thus cumulative errors is: errInt=Ki* ∑ err*t;

5) finally obtain proofreading and correct angular velocity vector afterwards: ω _c=ω+k _p* err+errInt.

3. as shown in Figure 3, adopt gradient descent method to upgrade hypercomplex number:

1) definition correlated variables

$\stackrel{\&RightArrow;}{G}={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$ : acceleration of gravity, represents in navigation coordinate system;

$\stackrel{\&RightArrow;}{H}={\left[\begin{array}{ccc}0& h_2& h_3\end{array}\right]}^T$ : geomagnetic field intensity constant, represents in navigation coordinate system;

acceleration measurement, represents in body axis system;

magnetic field intensity measured value, represents in body axis system;

2) error function

According to formula $f(Q,d,s)=Q*\&CircleTimes;d\&CircleTimes;Q-s,$ Can obtain

$\mathrm{\&Delta;}\stackrel{\&RightArrow;}{a}=C_n^b*\stackrel{\&RightArrow;}{G}-\stackrel{\&RightArrow;}{a}=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)g-a_x\\ 2(q_2{q}_3+q_0{q}_1)g-a_y\\ (q_0^2-q_1^2-q_2^2+q_3^2)g-a_z\end{array}\right]=\left[\begin{array}{c}\mathrm{\&Delta;}{a}_x\\ \mathrm{\&Delta;}{a}_y\\ \mathrm{\&Delta;}{a}_z\end{array}\right]$

$\mathrm{\&Delta;}\stackrel{\&RightArrow;}{h}=C_n^b*\stackrel{\&RightArrow;}{h}=\left[\begin{array}{c}2(q_1{q}_2+q_0{q}_3)h_2+2(q_1{q}_3-q_0{q}_2)h_3-h_x\\ (q_0^2-q_1^2+q_2^2-q_3^2)h_2+2(q_2{q}_3+q_0{q}_1)h_3-h_y\\ 2(q_2{q}_3-q_0{q}_1)h_2+(q_0^2-q_1^2-q_1^2+q_3^2)h_3-h_z\end{array}\right]=\left[\begin{array}{c}\mathrm{\&Delta;}{h}_x\\ \mathrm{\&Delta;}{h}_y\\ \mathrm{\&Delta;}{h}_z\end{array}\right]$

3) target error function

By synthetic one of above-mentioned two error functions, use quadratic sum label taking value here, obtain

F (a, h)=| a| ²+ | h| ², now functional value is more than or equal to 0.

4) gradient

$\&dtri;f(a,h)=\frac{\&PartialD;f(a,h)}{\&PartialD;Q}={\left[\begin{array}{cccc}\frac{\&PartialD;f}{\&PartialD;q_0}& \frac{\&PartialD;f}{\&PartialD;q_1}& \frac{\&PartialD;f}{\&PartialD;q_2}& \frac{\&PartialD;f}{\&PartialD;q_3}\end{array}\right]}^T,\frac{\&PartialD;f}{\&PartialD;q_0},\frac{\&PartialD;f}{\&PartialD;q_1},\frac{\&PartialD;f}{\&PartialD;q_2},\frac{\&PartialD;f}{\&PartialD;q_3}$ Be followed successively by

$\left\{\begin{array}{c}-4\mathrm{\&Delta;}{a}_x{q}_2+4\mathrm{\&Delta;}{a}_y{q}_1+4\mathrm{\&Delta;}{h}_x(h_2{q}_3-h_3{q}_2)+4\mathrm{\&Delta;}{h}_y{h}_3{q}_1-\mathrm{\&Delta;}{h}_z{h}_2{q}_1\\ 4\mathrm{\&Delta;}{a}_x{q}_3+4\mathrm{\&Delta;}{a}_y{q}_0-4\mathrm{\&Delta;}{a}_z{q}_1+4\mathrm{\&Delta;}{h}_x(h_2{q}_2+h_3{q}_3)+4\mathrm{\&Delta;}{h}_y(h_3{q}_0-h_2{q}_1)-4\mathrm{\&Delta;}{h}_z(h_2{q}_0+h_{{3q}_1})\\ -4\mathrm{\&Delta;}{a}_x{q}_0+4\mathrm{\&Delta;}{a}_y{q}_3-4\mathrm{\&Delta;}{a}_z{q}_2+4\mathrm{\&Delta;}{h}_x(h_2{q}_1-h_3{q}_0)+4\mathrm{\&Delta;}{h}_y{h}_3{q}_3+4\mathrm{\&Delta;}{h}_z(h_2{q}_3-h_3{q}_2)\\ 4\mathrm{\&Delta;}{a}_x{q}_1+4\mathrm{\&Delta;}{a}_y{q}_2+4\mathrm{\&Delta;}{h}_x(h_2{q}_0+h_3{q}_1)+4\mathrm{\&Delta;}{h}_y(h_3{q}_2-h_2{q}_3)+4\mathrm{\&Delta;}{h}_z{h}_2{q}_2\end{array}\right.$

5) hypercomplex number is upgraded

Step size mu is set to 0.041 times of gradient length, and renewal equation is

6) finally to hypercomplex number normalization, obtain new hypercomplex number:

4. the EKF of decomposing based on UD, as shown in Figure 4, filtering is followed successively by:

State one-step prediction:

UD decomposes: H _kp _k/k-1h _k ^t+ R=U ∧ D;

One-step prediction square error: P _k/k-1=Φ _{k, k-1}p _k-1/k-1Φ _k/k-1 ^t+ Q _k-1;

Calculation of filtered gain: K _k=P _k/k-1h _k ^t(H _kp _k/k-1h _k ^t+ R) ^-1=P _k/k-1h _k ^t(UDU ^t) ^-1;

Calculate new breath: ${\mathrm{\&gamma;}}_k=Z_k-H_k{\hat{X}}_{k/k-1};$

State Estimation is calculated: ${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k{\mathrm{\&gamma;}}_k;$

Estimate square error: P _k/k=(I-K _kh _k) P _k/k-1.

Iterative cycles upgrades.

In formula,

$H_k=\left[\begin{array}{ccccccc}-q_{2}g& q_{3}g& -q_{0}g& q_{1}g& 0& 0& 0\\ q_{1}g& q_{0}g& q_{3}g& q_{2}g& 0& 0& 0\\ q_{0}g& -q_{1}g& {-q}_{2}g& q_{3}g& 0& 0& 0\\ -\mathrm{bx}{q}_0-\mathrm{bz}{q}_2& \mathrm{bx}{q}_1+\mathrm{bz}{q}_3& -\mathrm{bx}{q}_2-{\mathrm{bzq}}_0& -\mathrm{bx}{q}_3+\mathrm{bz}{q}_1& 0& 0& 0\\ -\mathrm{bx}{q}_3+\mathrm{bz}{q}_1& \mathrm{bx}{q}_2+\mathrm{bz}{q}_0& \mathrm{bx}{q}_1+\mathrm{bz}{q}_3& -\mathrm{bx}{q}_0-\mathrm{bz}{q}_2& 0& 0& 0\\ \mathrm{bx}{q}_2+\mathrm{bz}{q}_0& \mathrm{bx}{q}_3-\mathrm{bz}{q}_1& \mathrm{bx}{q}_0-\mathrm{bz}{q}_2& \mathrm{bx}{q}_1+\mathrm{bz}{q}_3& 0& 0& 0\end{array}\right]$

5. often complete data fusion one time, calculate one time attitude matrix:

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

By above formula, can calculate underwater robot attitude angle;

ψ＝arctan(r ₁₂/r ₂₂)

θ＝arcsinr ₃₂

γ＝arctan(-r ₃₁/r ₃₃)

6. use three rank upstreame schemes to solve speed and position, and carry out rotation compensation, calculate according to the following procedure:

If accelerometer is respectively a at the k moment, the k-1 moment, k-2 measured value constantly _k, a _k-1, a _k-2, gyroscope is respectively ω at the k moment, the k-1 moment, k-2 measured value constantly _k, ω _k-1, ω _k-2, make T=2t, T _m, T _m-1, T _m-2speed is constantly made as respectively v _m, v _m-1, v _m-2, T _m, T _m-1position is constantly respectively P _m, P _m-1.

First calculate speed increment and the angle increment of two sampling instants:

Speed increment is $\mathrm{\&Delta;v}=T(\frac{1}{6}{a}_k+\frac{4}{6}{a}_{k-1}+\frac{1}{6}{a}_{k-2})$

Angle increment is $\mathrm{\&Delta;\&theta;}=T(\frac{1}{6}{\mathrm{\&omega;}}_k+\frac{4}{6}{\mathrm{\&omega;}}_{k-1}+\frac{1}{6}{\mathrm{\&omega;}}_{k-2})$

Then adopt three rank upstreame schemes to solve speed and position:

Speed is $v_m=v_{m-1}+C_b^n(\frac{T(5a_k+8a_{k-1}-a_{k-2})}{12}+\frac{1}{2}\mathrm{\&Delta;v}\&times;\mathrm{\&Delta;\&theta;})$

Position is $P_m=P_{m-1}+\frac{T(5v_m+8v_{m-1}{v}_{m-2})}{12}$

In formula, be rotation compensation part, " * " represents vector multiplication.

7. the power end points such as pair depthometer employing is smoothly made moving average filter, collapse dept:

$\mathrm{depth}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_i$

Constantly reach slidably one by one m the adjacent data average computation that counts.In formula, d _ibe i the data that depthometer gathers, depth is filtered depth value.

In addition to the implementation, the present invention can also have other embodiments, and all employings are equal to the technical scheme of replacement or equivalent transformation formation, all drop in the protection domain of requirement of the present invention.

Claims (6)

1. a submerged structure detects Algorithms of Robots Navigation System, comprise magnetic compass (1), gyroscope (2), accelerometer (3), depthometer (4), navigation microprocessor (5), it is characterized in that, described magnetic compass (1), gyroscope (2), accelerometer (3) and depthometer (4) gather respectively underwater robot magnetic field intensity, angular velocity, linear velocity and submerged depth data, transfer to navigation microprocessor (5); The data that described navigation microprocessor (5) gathers magnetic compass (1), gyroscope (2), accelerometer (3), depthometer (4) are carried out A/D conversion; The data that described navigation microprocessor (5) gathers magnetic compass (1), gyroscope (2), accelerometer (3) in conjunction with hypercomplex number gradient descent method, complementary filter, Kalman Algorithm are carried out data fusion, obtain attitude matrix and attitude angle; The data acquisition that described navigation microprocessor (5) gathers gyroscope (2), accelerometer (3) is used with three rank of rotation compensation Algorithm for Solving speed and position windward; Described navigation microprocessor (5) adopts moving average filter to ask for underwater robot submerged depth to depthometer, obtains integrated navigation information.

2. submerged structure detects Algorithms of Robots Navigation System as claimed in claim 1, it is characterized in that, described magnetic compass (1) adopts the electronic compass IC HMC5883L of Honeywell company.

3. submerged structure detects Algorithms of Robots Navigation System as claimed in claim 1, it is characterized in that, described gyroscope (2), accelerometer (3) are the micro electronmechanical products that InvenSense company produces.

4. submerged structure detects Algorithms of Robots Navigation System as claimed in claim 1, it is characterized in that, described depthometer (4) is diffuse si type pressure unit, has good precision in 100m range ability.

5. submerged structure detects Algorithms of Robots Navigation System as claimed in claim 1, it is characterized in that, described navigation microprocessor (5) adopts based on ARM Cortex ^tMthe STM32F103T8 of-M3 kernel.

6. submerged structure detects the air navigation aid of Algorithms of Robots Navigation System as claimed in claim 1, it is characterized in that, take hypercomplex number and angle error to estimate object as filtering, first carries out complementary filter, and gyroscope survey value is done to preliminary compensation; Next uses gradient descent method to upgrade hypercomplex number, effectively reduces attitude angle drift; Carry out again Kalman filtering, adopt the UD decomposition method in matrix, avoid filter divergence, obtain underwater robot attitude matrix and attitude angle; Then accelerometer and gyro data are used to three rank Algorithm for Solving speed and position windward.Meanwhile, depthometer image data being carried out to moving average filter, obtain underwater robot depth value, takes this as the standard in the vertical position of underwater robot.The method comprises the following steps:

1) attitude, speed, location compute, process is as follows:

(1) the discrete Karman equation that underwater vehicles navigation system adopts is:

X _k＝Φ _k，k-1X _k-1+W _k-1

Z _k＝H _kX _k+V _k

In formula, k represents system discrete sampling time point, k>=0; X _kfor k system estimation quantity of state constantly; Φ _{k, k-1}for X _k-1to X _kmatrix of shifting of a step; W _k-1for system incentive Gaussian sequence, average is E[w (k)]=0, variance is E[w (k) w (k) ^t]=Q _k, Q _kit is system noise variance; Z _kfor k sensor measuring value constantly; H _kfor measurement matrix, the mathematical relation between reacting dose measurement and estimator; V _kfor measuring value Gaussian sequence, average is E[v (k)]=0, variance is E[v (k) v (k) ^t]=R _k, R _kmeasure noise variance; W _k, V _kuncorrelated mutually;

(2) determine parameter original state, comprising:

Local gravitational acceleration g, systematic sampling time t, angular velocity error delta ω ₁, Δ ω ₂, Δ ω ₃, complementary filter parameter k _p, k _i, by estimated state amount initial value estimate square error matrix initial value P _0|0; K wherein _p, k _ivalue rule is: k _p+ k _i=1,0.95 < k _p< 1, by test of many times, determines optimal value; p _0|0press following value: setting navigation coordinate is sky, northeast coordinate system, is designated as n system; Setting body axis system is b system, and initial time overlaps with navigation coordinate system, and n system and b system are three axle orthogonal coordinate systems; Hypercomplex number denotation coordination rotation relationship Q=[q ₀, q ₁, q ₂, q ₃], q wherein ₀for rotation scalar, q ₁, q ₂, q ₃for rotation of coordinate vector, with hypercomplex number statement n, being tied to b is that transformational relation is expressed as:

$C_n^b=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]=\left[\begin{array}{ccc}{r}_{11}& r_{21}& r_{31}\\ r_{12}& r_{22}& r_{32}\\ r_{13}& r_{23}& r_{33}\end{array}\right]$

And b is tied to n, be r _ijeach element value in (1≤i, j≤3) representing matrix;

(3) utilize the frequency domain complementary characteristic correction angle velocity of gyroscope and magnetic compass, accelerometer, process is as follows:

If magnetic compass measured value is m _b, acceleration measuring value is a, gyroscope survey value is ω, m _b, a, ω be 3 * 1 column matrix;

First magnetic field amount being transformed into navigation coordinate by body axis system is $m_n=C_b^n{m}_b={\left[\begin{array}{ccc}{m}_{\mathrm{nx}}& m_{\mathrm{ny}}& m_{\mathrm{nz}}\end{array}\right]}^T,$ And calculated level and vertical direction magnetic field: by=0, bz=m _nz;

Then magnetic field is transformed into body axis system: $w=C_n^b{\left[\begin{array}{ccc}\mathrm{bx}& \mathrm{by}& \mathrm{bz}\end{array}\right]}^T,$ And utilize the 3rd row are estimated acceleration of gravity vector:

Calculate again acceleration and field compensation error:

In formula, " * " represents vector multiplication;

Obtaining cumulative errors is: errInt=Ki ∑ err*t;

Finally obtain the angular velocity vector after proofreading and correct: ω _c=ω+k _p* err+errInt

Wherein, K _i, K _pit is complementary filter parameter;

(4) utilize gradient descent method to upgrade hypercomplex number:

By obtain new hypercomplex number q ₀, q ₁, q ₂, q ₃, wherein, μ is the step value of Gradient Descent, by determine; $\&dtri;f=\frac{\&PartialD;f}{\&PartialD;Q},f(Q,d,s)=Q*\&CircleTimes;d\&CircleTimes;Q-s,$ D is the measured value of sensor based on body axis system, and s is the actual value of sensor based on navigation coordinate system, and " * " represents adjoint matrix, " " be gradient multiplication, " ο " is hypercomplex number multiplication;

(5) Kalman filtering process decomposing based on UD is followed successively by:

State one-step prediction:

UD decomposes: H _kp _k/k-1h _k ^t+ R=U ∧ D;

One-step prediction square error: P _k/k-1=Φ _{k, k-1}p _k-1/k-1Φ _k/k-1 ^t+ Q _k-1;

Calculation of filtered gain: K _k=P _k/k-1h _k ^t(H _kp _k/k-1h _k ^t+ R) ^-1=P _k/k-1h _k ^t(UDU ^t) ^-1;

Calculate new breath (new breath is calculated and obtained by up-to-date measuring value):

State Estimation is calculated: ${\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k{\mathrm{\&gamma;}}_k;$

Estimate square error: P _k/k=(I-K _kh _k) P _k/k-1;

(6) use three rank upstreame schemes to solve speed and position, and carry out rotation compensation, calculate according to the following procedure:

If accelerometer is respectively a at the k moment, the k-1 moment, k-2 measured value constantly _k, a _k-1, a _k-2, gyroscope is respectively ω at the k moment, the k-1 moment, k-2 measured value constantly _k, ω _k-1, ω _k-2, make T=2t, T _m, T _m-1, T _m-2speed is constantly made as respectively v _m, v _m-1, v _m-2, T _m, T _m-1position is constantly respectively P _m, P _m-1;

First calculate speed increment and the angle increment of two sampling instants:

Speed increment is $\mathrm{\&Delta;v}=T(\frac{1}{6}{a}_k+\frac{4}{6}{a}_{k-1}+\frac{1}{6}{a}_{k-2});$

Angle increment is $\mathrm{\&Delta;\&theta;}=T(\frac{1}{6}{\mathrm{\&omega;}}_k+\frac{4}{6}{\mathrm{\&omega;}}_{k-1}+\frac{1}{6}{\mathrm{\&omega;}}_{k-2});$

Then adopt three rank upstreame schemes to solve speed and position:

Speed is $v_m=v_{m-1}+C_b^n(\frac{T(5a_k+8a_{k-1}-a_{k-2})}{12}+\frac{1}{2}\mathrm{\&Delta;v}\&times;\mathrm{\&Delta;\&theta;})$

Position is $P_m=P_{m-1}+\frac{T(5v_m+8v_{m-1}{v}_{m-2})}{12}$

In formula, be rotation compensation part, " * " represents vector multiplication;

2) depthometer resolves

The power end points such as depthometer employing are smoothly made to moving average filter, calculate underwater robot submerged depth:

$\mathrm{depth}=\frac{1}{m}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}{d}_i$

Constantly reach slidably one by one m the adjacent data average computation that counts, in formula, d _ibe i the data that depthometer gathers, depth is filtered depth value.