Latitude-free initial alignment method under swaying base based on gradient descent optimization

Abstract

The disclosure discloses a latitude-free initial alignment method under a swaying base based on gradient descent optimization. Firstly, swaying base latitude-free alignment is regarded as a Wahba attitude determination problem to inhibit device noise interference, and an objective function is established based on a gravitational acceleration vector under an earth system; then an exact solution of the objective function is obtained through a gradient descent optimization method, and inertial system conversion quaternion estimation is achieved under the latitude-free condition; and finally, an attitude quaternion is determined by only using information of an accelerometer and a gyroscope of a strapdown attitude heading reference system, and therefore latitude-free initial alignment under the swaying base is achieved. The disclosure can solve the problem that initial alignment cannot be accomplished with unknown latitude under the swaying base, and thus the application range of the strapdown attitude heading reference system is ensured.

Claims

1. A method comprising: establishing a first objective function using acceleration vectors obtained by an accelerometer and angular velocity vectors obtained by a gyroscope, wherein the accelerometer and the gyroscope are mounted on a vessel comprising a swaying base that has random perturbations relative to the earth, wherein the first objective function is a function of an orientation of a reference frame i.sub.b0 of the vessel at time to with respect to an inertial reference frame i, wherein the orientation is represented by a quaternion $q_i^{i_{b_0}};$ obtaining a first value of $q_i^{i_{b_0}}$ by optimizing the first objective function with respect to $q_i^{i_{b_0}};$ establishing a second objective function using a velocity vector or a position vector of the vessel, wherein the second objective function is a function of $q_i^{i_{b_0}},$ wherein the second objective function is $\zeta \left(q_i^{i_{b0}},\alpha \left(t_k\right),\beta \left(t_k\right)\right)=\frac{1}{2}\underset{t_k\in \left[t_{k_1,}{t}_{k_2}\right]}{.Math.}{.Math.\eta \left(q_i^{i_{b0}},\alpha \left(t_k\right),\beta \left(t_k\right)\right).Math.}^2,$ wherein $\alpha \left(t_k\right)={\tilde{v}}^i\left(t_k\right)=-{\int }_{t_0}^{t_k}{\tilde{g}}^i\left(t\right)dt\mathrm{and}\beta \left(t_k\right)={\tilde{v}}^{i_{b0}}\left(t_k\right)={\int }_{t_0}^{t_k}{\tilde{f}}^{i_{b_0}}\left(t\right)dt,$ or wherein $\alpha \left(t_k\right)={\tilde{p}}^i\left(t_k\right)={\int }_{t_0}^{t_k}{\tilde{v}}^i\left(t\right)dt\mathrm{and}\beta \left(t_k\right)={\tilde{p}}^{i_{b_0}}\left(t_k\right)={\int }_{t_0}^{t_k}{\tilde{v}}^{i_{b_0}}\left(t\right)dt,$ wherein {tilde over (v)}.sup.i(t.sub.k) is the velocity at time t.sub.k in the inertial reference frame i, {tilde over (p)}.sup.i(t.sub.k) is the position vector at time t.sub.k in the inertial reference frame i, {tilde over (v)}.sup.i.sup.bo(t.sub.k) is the velocity vector at time t.sub.k in the reference frame i.sub.b0, ${\stackrel{\_}{p}}^{i_{b_0}}\left(t\right)$ is the position vector at time t.sub.k in the reference frame i.sub.b0, {tilde over (g)}.sup.i(t) is a gravity acceleration vector at the time t in the inertial reference frame i, and ${\tilde{f}}^{i_{b_0}}\left(t\right)$ is an acceleration vector of the vessel at time t in the reference frame i.sub.b0, obtaining a second value of $q_i^{i_{b0}}$ by optimizing the second objective function with respect to $q_i^{i_{b0}}$ and by using the first value of $q_i^{i_{b0}}$ as an initial value; determining a position and orientation of the vessel in a reference system of the earth based the second value of $q_i^{i_{b_0}};$ and steering the vessel using the position and orientation of the vessel in the reference system of the earth.

2. The method according to claim 1, wherein the first objective function is $\zeta \left(q_i^{i_{b0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)=\frac{1}{2}\underset{t_k\in \left[t_{k_1},t_{k_2}\right]}{.Math.}{.Math.\eta \left(q_i^{i_{b0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right).Math.}^2;$ $\mathrm{wherein}\eta \left(q_i^{i_{b0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)=\left(\begin{array}{c}{q}_i^{i_{b0}}.Math.{\tilde{g}}^i\left(t_{k_1}\right).Math.q_i^{i_{b0}^{\star }}+{\tilde{f}}^{i_{b_0}}\left(t_{k_1}\right)\\ q_i^{i_{b0}}.Math.{\tilde{g}}^i\left(t_{k_2}\right).Math.q_i^{i_{b0}^{\star }}+{\tilde{f}}^{i_{b_0}}\left(t_{k_2}\right)\end{array}\right).$

Description

BRIEF DESCRIPTION OF FIGURES

(1) FIG. 1 is reference attitude motion curves under a swaying condition.

(2) FIG. 2 is alignment error curves of Three-axis Attitude Determination (TRIAD), Optimization-Based Alignment (OBA), Three Vectors Self-Alignment (TVSA) and Gravitational Apparent Motion (GAM). FIG. 3 schematically shows a vessel with an accelerometer and a gyroscope.

DETAILED DESCRIPTION

(3) The disclosure is further described in combination with accompanying drawings below.

(4) In order to verify the effectiveness of the disclosure, a method of the disclosure is simulated through Matlab®.

(5) First, according to the definition of a coordinate system, at the moment of t=t.sub.0, a carrier coordinate system b coincides with a solidification coordinate system

(6) $i_{b_0},$
and then

(7) $q_b^{i_{b_0}}\left(t_0\right)={\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right]}^T.$
Thus, a

(8) $q_b^{i_{b_0}}$
(t.sub.k) updating equation is obtained:

(9) ${\stackrel{.}{q}}_b^{i_{b_0}}\left(t_k\right)=\frac{1}{2}{q}_b^{i_{b_0}}\left(t_{k-1}\right).Math.{\tilde{\omega }}_{i_{b_0}}^b\left(t_k\right)$
where

(10) $q_b^{i_{b_0}}$
(t.sub.k) is a carrier swaying quaternion, representing coordinate change of a relative inertia solidification coordinate system caused by carrier swaying motion; and

(11) ${\tilde{\omega }}_{i_{b_{0}b}}^b\left(t_k\right)$
is information of gyroscope output angular velocity at the moment t.sub.k.

(12) Likewise, according to the definition of the coordinate system, a formula q.sub.e.sup.i(t.sub.0)=[1 0 0 0].sup.T is established, and a q.sub.e.sup.i(t.sub.k) updating equation is obtained:
{dot over (q)}.sub.e.sup.i(t.sub.k)=½q.sub.e.sup.i(t.sub.k-1).Math.ω.sub.ie.sup.e
where q.sub.e.sup.i(t.sub.k) is an earth rotation quaternion, representing coordinate change of a relative inertia system caused by the earth rotation; and
ω.sub.ie.sup.e is a projection of a rotation angular velocity vector of the earth in an earth coordinate system, and ω.sub.ie.sup.e=[0 0 0 ω.sub.ie].sup.T.

(13) Besides, if local latitude information is known, through rotation of two times, a position quaternion q.sub.e.sup.n can be obtained:
q.sub.e.sup.n=q.sub.n′.sup.n.Math.q.sub.e.sup.n′,
where

(14) $\{\begin{array}{c}{q}_e^{n^{\prime }}={\left[\cos \left(\frac{90°-L}{2}\right)0-\sin \left(\frac{90°-L}{2}\right)0\right]}^T\\ q_{n^{\prime }}^n={\left[\cos \left(45°\right)00-\sin \left(45°\right)\right]}^T\end{array}.$

(15) According to a quaternion multiplication chain rule, an attitude quaternion q.sub.b.sup.n(t.sub.k) can be factorized into a product of four portions including the position quaternion q.sub.e.sup.n, the earth rotation quaternion q.sub.e.sup.i(t.sub.k), an inertial system conversion quaternion

(16) $q_{i_{b_0}}^i$
and the carrier swaying quaternion

(17) 0$q_b^{i_{b_0}}$
(t.sub.k);

(18) $q_b^n\left(t_k\right)=q_e^n.Math.q_i^e\left(t_k\right).Math.q_{i_{b_0}}^i.Math.q_b^{i_{b_0}}\left(t_k\right).$

(19) Moreover, a gravitational acceleration vector g.sup.n and a normalization vector g.sup.n thereof under the system are written as follows:
g.sup.n=[0 0 −g].sup.T
g.sup.n=g.sup.n/∥g.sup.n∥[0 0−1].sup.T.

(20) Likewise, a gravitational acceleration vector normalization form g.sup.e under the system is written as follows:
g.sup.e=[−cos L0−sin L].sup.T.

(21) Then the gravitational acceleration vector is converted from an e system to an i system, as shown in a formula:
g.sup.i(t.sub.k)=q.sub.e.sup.i(t.sub.k).Math.g.sup.e.Math.q.sub.e.sup.i*(t.sub.k).

(22) Further, the gravitational acceleration vector is converted from the e system to an i.sub.b.sub.0 system, as shown in a formula:

(23) $\begin{array}{c}{\overline{g}}^{i_{b_0}}\left(t_k\right)=q_i^{i_{b_0}}.Math.{\overline{g}}^i\left(t_k\right).Math.q_i^{i^{{*}_{b_0}}}\\ =q_l^{i_{b_0}}.Math.q_e^i\left(t_k\right).Math.{\overline{g}}^e.Math.q_e^{i^{*}}\left(t_k\right).Math.q_i^{i^{{*}_{b_0}}}.\end{array}$

(24) In a traditional inertial system alignment method, after the gravitational acceleration vector pair

(25) ${\overline{g}}^{i_{b_0}}$
(t.sub.k) and g.sup.i(t.sub.k) is obtained, an objective function can be established, and then the inertia system conversion quaternion

(26) $q_i^{i_{b_0}}$
is determined. However, under the condition of no latitude, g.sup.e and g.sup.i(t.sub.k) cannot be obtained due to lack of the local latitude information L, and thus g.sup.e needs to be reconstructed. The objective function is established by using output information of an accelerometer in fixed-length sliding window, and a reconstructed gravitational acceleration is obtained by using a gradient descent optimization method to solve the objective function:

(27) ${\tilde{g}}^e={\left[-\sqrt{1-{\left(\frac{1}{m}\underset{j=j_1}{\overset{j_m}{.Math.}}\widetilde{f_z^{″}}\left(t_j\right)\right)}^2}0-\frac{1}{m}\underset{j=j_1}{\overset{j_m}{.Math.}}{\tilde{f}}_z^{i^{\prime }}\left(t_j\right)\right]}^T.$

(28) Under the inertial coordinate system i, a gravitational acceleration vector projection {tilde over (g)}.sup.i(t.sub.k) is:
{tilde over (g)}.sup.i(t.sub.k)=q.sub.e.sup.i(t.sub.k).Math.q.sub.e.sup.i*(t.sub.k).

(29) Besides, at two different moments t=t.sub.k1 and t=t.sub.k2, a formula is established:

(30) $\{\begin{array}{c}{\tilde{f}}^{i_{b_0}}\left(t_{k_1}\right)=-q_i^{i_0}.Math.{\tilde{g}}^i\left(t_{k_1}\right).Math.q_i^{i_{b_0}^{*}}\\ {\tilde{f}}^{i_{b_0}}\left(t_{k_2}\right)=-q_i^{i_0}.Math.{\tilde{g}}^i\left(t_{k_2}\right).Math.q_i^{i_{b_0}^{*}}\end{array}.$

(31) Likewise, inhibiting interference caused by inertial device noise by using measured data in a period of time window and establishing an objective function

(32) $\zeta \left(q_i^{i_{b_0}},{\stackrel{\sim }{g}}^i\left(t_k\right),{\stackrel{\sim }{f}}^{i_{b_0}}\left(t_k\right)\right)$
are as follows:

(33) $\underset{q_i^{i_{b_0}}}{\min }\zeta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)=\frac{1}{2}\underset{k_1,k_2}{.Math.}{.Math.\eta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right).Math.}^2$$\eta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right),\mathrm{and}=\left(\begin{array}{c}{q}_i^{i_{b_0}}.Math.{\tilde{g}}^i\left(t_{k_1}\right).Math.q_i^{i_{b_0}^{*}}+{\tilde{f}}^{i_{b_0}}\left(t_{k_1}\right)\\ q_i^{i_{b_0}}.Math.{\tilde{g}}^i\left(t_{k_2}\right).Math.q_i^{i_{b_0}^{*}}+{\tilde{f}}^{i_{b_0}}\left(t_{k_2}\right)\end{array}\right)$
the above formula is solved by using the gradient descent optimization method, and its iteration process is shown as follows:

(34) $q_i^{i_{b_0}}\left(k\right)=q_i^{i_{b_0}}\left(k-1\right)-\lambda \left(k\right)\frac{\nabla \zeta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)}{.Math.\nabla \zeta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right).Math.}$$\nabla \zeta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)=\frac{\partial {\eta }^T\left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)}{\partial q_i^{i_{b_0}}}\eta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\tilde{f}}^{i_{b_0}}\left(t_k\right)\right)$
where

(35) 0$\nabla \zeta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\stackrel{\sim }{f}}^{i_{b_0}}\left(t_k\right)\right)$
represents a gradient vector of the objective function

(36) $\zeta \left(q_i^{i_{b_0}},{\tilde{g}}^i\left(t_k\right),{\stackrel{\sim }{f}}^{i_{b_0}}\left(t_k\right)\right),$
and λ(k) represents the k.sup.th iteration step length.

(37) Likewise,

(38) $q_i^{i_{b_0}}$
can be determined as long as a vector point pair meeting the following formula can be found:

(39) $\{\begin{array}{c}\beta \left(t_{k_1}\right)=q_i^{i_{b_0}}.Math.\alpha \left(t_{k_1}\right).Math.q_i^{i_{b_0}^{*}}\\ \beta \left(t_{k_2}\right)=q_i^{i_{b_0}}.Math.\alpha \left(t_{k_2}\right).Math.q_i^{i_{b_0}^{*}}\end{array},$
where β(t.sub.k) and α(t.sub.k) respectively represent projections of the vector pair under the inertial solidification coordinate system

(40) $i_{b_0}$
and the inertial coordinate system i.

(41) A specific establishing mode for further establishing an objective function by using a velocity vector and a position vector respectively in order to improve the inhibiting capability of an algorithm to inertial device noise and linear vibration interference is as follows:

(42) ${\tilde{v}}^{{\mathrm{ib}}_0}\left(t_k\right)={\int }_{t_0}^{t_k}{\tilde{f}}^{i_{b_0}}\left(t\right)\mathrm{dt}{\tilde{v}}^i\left(t_k\right)=-{\int }_{t_0}^{t_k}{\tilde{g}}^i\left(t\right)\mathrm{dt}$${\tilde{p}}^{i_{b_0}}\left(t_k\right)={\int }_{t_0}^{t_k}{\tilde{v}}^{i_{b_0}}\left(t\right)\mathrm{dt}{\tilde{p}}^i\left(t_k\right)={\int }_{t_0}^{t_k}{\tilde{v}}^i\left(t\right)\mathrm{dt}$$\underset{q_i^{i_{b_0}}}{\min }\zeta \left(q_i^{i_{b_0}},\alpha \left(t_k\right),\beta \left(t_k\right)\right)=\frac{1}{2}\underset{k_1,k_2}{.Math.}{.Math.\eta \left(q_i^{i_{b_0}},\alpha \left(t_k\right),\beta \left(t_k\right)\right).Math.}^2$$\eta \left(q_i^{i_{b_0}},\alpha \left(t_k\right),\beta \left(t_k\right)\right)=\left(\begin{array}{c}{q}_i^{i_{b_0}}.Math.\alpha \left(t_{k_1}\right).Math.q_i^{i_{b_0}^{*}}-\beta \left(t_{k_1}\right)\\ q_i^{i_{b_0}}.Math.\alpha \left(t_{k_2}\right).Math.q_i^{i_{b_0}^{*}}-\beta \left(t_{k_2}\right)\end{array}\right).$

(43) Likewise, a determination result of the inertial system conversion quaternion

(44) $q_i^{i_{b_0}}$
is determined by using gradient descent optimization to solve the objective function.

(45) A local latitude L is calculated by using the gravitational acceleration vector {tilde over (g)}.sup.e:

(46) $L={\sin }^1\left({\tilde{g}}_z^e\right)={\sin }^{-1}\left(\frac{1}{m}\underset{j=j_1}{\overset{j_m}{.Math.}}{\tilde{f}}_z^{i^{\prime }}\left(t_j\right)\right).$

(47) Then after the local latitude information L is obtained through estimation, the position quaternion q.sub.e.sup.n can be obtained:

(48) $q_e^n={\frac{\sqrt{2}}{2}\left[\cos \left(\frac{90°-L}{2}\right)-\sin \left(\frac{90°-L}{2}\right)-\sin \left(\frac{90°-L}{2}\right)-\cos \left(\frac{90°-L}{2}\right)\right]}^T,$
and thus the attitude quaternion q.sub.b.sup.n(t) is obtained through updating:

(49) $q_b^n\left(t_k\right)=q_e^n.Math.q_i^e\left(t_k\right).Math.q_{i_{b_0}}^i.Math.q_b^{i_{b_0}}\left(t_k\right)=q_e^n.Math.q_e^{i^{*}}\left(t_k\right).Math.q_i^{i_{b_0}^{*}}.Math.q_b^{i_{b_0}}\left(t_k\right).$

(50) In order to verify the effectiveness of the swaying base latitude-free self-alignment method provided by the disclosure, a simulation experiment is specially designed, and comparison analysis with a traditional swaying base alignment method depending on latitude information and an existing latitude-free swaying base alignment method is performed.

(51) Simulation conditions are set as follows.

(52) The simulation experiment adopts an inertial device with gyroscopic drift being 0.01°/h and accelerometer null bias being 10.sup.−4 g, and a sampling frequency is 100 Hz. Gyroscope random walk is 0.001°/√{square root over (h)}, and accelerometer measurement noise is 10.sup.−5 g/√{square root over (Hz)}, all treated as white noise. As a reference value of an attitude can be obtained in real time under a simulation environment, as long as a contrast experiment is performed under the same swaying condition, alignment performance of all algorithms can be evaluated by using obtained alignment errors. Thus, local latitude is set by simulation to be 45.7796° (Harbin), the simulation time is 200 s, initial alignment is performed from 0 s to 200 s, and a difference value between an alignment result at an alignment completing moment of the 200.sup.th s and a set reference value is used as an alignment error of this experiment. Specifically, in simulation, a swaying balance position is set to be: ρ=0° (rolling), κ=0° (pitching) and Ψ=45° (heading), an initial phase is (0°,0°,0°), and swaying motion meets the following formula:

(53) 0$\{\begin{array}{c}\rho =10°\sin \left(\frac{2\pi }{5}t+{\varphi }_i\right)\\ \theta =10°\sin \left(\frac{\pi }{4}t+{\varphi }_i\right)\\ \psi =45°+5°\sin \left(\frac{2\pi }{7}t+{\varphi }_i\right)\end{array}.$

(54) Corresponding attitude motion curves of the simulation experiment of the first time are as shown in FIG. 1. FIG. 2 shows alignment error curves of traditional TRIAD and OBA methods depending on external latitude information and latitude-free alignment methods TVSA and GAM. It shows in FIG. 2 that through an alignment process of 200 s, TRIAD, OBA, TVSA and GAM are smaller than 0.01° in horizontal error and smaller than 0.1° in heading error. The GAM latitude-free alignment method provided by the disclosure and the traditional TRIAD method depending on the latitude information are both based on a double-vector alignment concept, thereby being similar in alignment convergence process, and better inhibiting noise interference at the 150.sup.th s to enter a convergence state. However, the existing latitude-free alignment method TVSA needs threes groups of vectors to build an objective function and depends on a gravity vector (single frame measured information) under the inertial system at the current moment to determine space orientation information, leading to large noise interference influence within the alignment time (200 s), as shown in FIG. 2, the heading error of TVSA reaches −0.0967°, while the heading error of GAM provided by the disclosure is −0.0604°. Thus, compared with the existing swaying base latitude-free alignment method TVSA, the GAM method provided by the disclosure is faster in alignment convergence speed and better in noise inhibiting capability.