CN101832775A - 深海作业水下航行器组合导航***及水下初始对准方法 - Google Patents

Abstract

本发明公布了一种深海作业水下航行器组合导航***及水下初始对准方法，所述***包括卫星星座、差分卫星基准站、卫星浮标***、水下导航收发机和船基数据控制中心。所述方法水下航行器在水下工作之前，捷联式惯性导航***需要进行初始对准，或者经过长时间水下工作之后，捷联式惯性导航***需要重新进行初始对准，在航行器不浮出水面的情况下，利用水下卫星***将航行器位置、速度和姿态信息传递给捷联式惯性导航***，提高了初始对准的精度和速度，从而提高了组合导航***的可靠性和可用性。

Description

深海作业水下航行器组合导航***及水下初始对准方法

技术领域

本发明涉及一种深海作业水下航行器组合导航***及水下初始对准方法，特别是一种适合航行器工作之前捷联式惯性导航***初始对准或者长时间工作之后在不浮出水面的情况下进行再初始对准。

背景技术

水下导航定位是海洋开发活动与海洋高技术发展的基本前提，海洋开发需要获取大范围、精确的海洋环境数据，需要进行海底勘探、水下测量及水下工程等，现代海战也逐渐发展成为涉及太空、空中、陆地、海面、水下及海底多层空间的立体战争。所有这些都需要有海面与水下导航定位的支撑。

卫星导航定位***的发展使整个地球表面以及地球的大部分外部空间实现全天候导航定位，为太空探索和开发提供了方便的导航和定位手段，同时，也使传统的测绘技术发生了飞跃性的质的变化。由于无线电波无法穿透水，特别是海水，因此，对于广大的江河湖泊、海洋等水下导航定位的需求，卫星定位***无法满足。尽管无线电波不能在海水中传播，但是，声纳信号在海水中有很好的传播特性。通过声纳将无线电信号在水中的延续传播，可以实现水下航行器导航定位与探测。

水下声纳导航定位***测量的都市相对于基线阵的坐标，属于独立的局部相对坐标系，卫星定位***采用的是全球统一的空间直角坐标系，如果能够将两者集成到一个***中，就可以实现陆地、海洋水下立体导航定位。

通常可以采用卫星定位***和水声定位***和水声导航定位***的船级基集成实现全球坐标***下的水下组合导航，该方案成为松散耦合方式。即通过水面船只链接卫星定位***和水下声学***，卫星定位***测量船只的全球空间位置，水下声线定位***测量水下目标与船只的相对坐标，通过坐标变换，求解水下目标在全球范围内的空间坐标。所有的卫星定位***与水下声线***组合***都属于松散耦合***。与之相对应的就是将卫星导航定位***直接作为水声定位***的一部分进行设计，完全集成到水声定位***内部。卫星定位***不仅需要提供空间位置，还需要提供其他相关的信息，一旦卫星定位***不能工作，水声定位***也不能够独立工作，这种模式称为紧耦合集成。水下卫星***和卫星智能浮标***都是紧耦合集成***。

发明内容

本发明的目的在于提供一种实现水下组合导航捷联式惯性导航***，旨在解决现有技术在水下定位方面存在初始对准需要浮出水面对准的问题。

本发明的目的还在于提供一种实现水下组合导航捷联式惯性导航***的水下初始对准方法，以更好地解决现有技术中的上述问题。

本发明为实现上述目的，采用如下技术方案：

本发明深海作业水下航行器组合导航***，包括卫星星座、差分卫星基准站、卫星浮标***、水下导航收发机和船基数据控制中心，其中卫星星座与差分卫星基准站无线通信，差分卫星基准站和卫星浮标***分别通过海面无线电通信链路与船基数据控制中心连接，水下导航收发机通过水下声通信链路与卫星浮标***连接，水下导航收发机和船基数据控制中心通过水下声通信链路双向连接。

深海作业水下航行器组合导航***水下初始对准方法包括以下步骤：

(1)船基数据控制中心向水下航行器的水下导航收发机发送一个请求定位的声纳脉冲信号；

(2)水下导航收发机向卫星浮标***发射水声定位信号；

(3)卫星浮标***接收到水声定位信号后，将水声定位信号数据及接收到的卫星信号数据进行编码和调制得到无线电信号，利用海面无线电通信链路将无线电信号传送到船基数据控制中心；

(4)船基数据控制中心将各卫星浮标***的无线电信号进行计算得到水下航行器的位置；

(5)船基数据控制中心将计算出的水下航行器的位置和水声定位信号发射时刻进行编码和调制得到调制信号，利用水声发射器将调制信号发射到水下导航收发机；水下导航收发机根据调制信号计算出导航参数和当前时刻。

优选地，步骤(5)所述的水下导航收发机根据调制信号计算出导航参数和当前时刻的方法包含如下步骤：

(a)水下导航收发机定位数据采集：采用组合导航计算机以周期T_G持续读取水下导航收发机接收到的导航信息即调制信号，并进行解读，取得航行器的实时三维位置、三维速度和航向角ψ和时间t，其中，三维位置包括经度λ、维度L和高度H，三维速度包括当地地理坐标系下的东向速度V_E、北向速度V_N和天向速度V_U；

(b)卫星导航定位数据缓存：把步骤(a)中所述的航行器的三维位置、三维速度、航向角与时间信息一并保持在导航计算机的内存空间变量中，缓存后得到经度λ_k-1、纬度L_k-1、H_k-1高度，东向速度V_E，k-1、北向速度V_N，k-1和天向速度V_U，k-1，航向角ψ_k-1，时间信息t_k-1；

(c)惯性测量单元数据采集：以周期T_I读取惯性测量单元中陀螺和加速度计测量的三维角速率和三维比力，捷联式惯性测量单元测得的信息均为航行器教运动和线运动在载体坐标系b下的投影，三维角速率和三维比力分别为

和

其中T_G为T_I的整数倍，x，y，z表示载体系的三个坐标轴；

(d)航行器运动加速度的计算：经过至少2个周期T_G的时间后，在完成卫星信息解读时刻t_k，组合导航计算机中有经度λ_k、纬度L_k，高度H_k，东向速度V_E，k，北向速度V_N，k，天向速度V_U，k，航向角ψ_k，按如下的前后差分法计算航行器的东向、北向和天向运动加速度：

$a_E=\frac{V_{E,k}-V_{E,k-1}}{T_G},$

$a_N=\frac{V_{N,k}-V_{N,k-1}}{T_G},$

$a_U=\frac{V_{U,k}-V_{U,k-1}}{T_G};$

(e)将步骤(a)、步骤(c)和步骤(d)得到的实时位置、速度、运动加速度，代入捷联惯性导航***比力方程，计算导航系下的三维比力，其计算方法如下：

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=\begin{array}{}\end{array}\left[\begin{array}{c}{a}_E\\ a_N\\ a_U\end{array}\right]+\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{ieU}}^n+{\mathrm{\ω}}_{\mathrm{enU}}^n)& (2{\mathrm{\ω}}_{\mathrm{ieN}}^n+{\mathrm{\ω}}_{\mathrm{enN}}^n)\\ (2{\mathrm{\ω}}_{\mathrm{ieU}}^n+{\mathrm{\ω}}_{\mathrm{enU}}^n)& 0& -(2{\mathrm{\ω}}_{\mathrm{ieE}}^n+{\mathrm{\ω}}_{\mathrm{enE}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{ieN}}^n+{\mathrm{\ω}}_{\mathrm{enN}}^n)& (2{\mathrm{\ω}}_{\mathrm{ieE}}^n+{\mathrm{\ω}}_{\mathrm{enE}}^n)& 0\end{array}\right]\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ V_{U,k}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g_0\end{array}\right],$

式中，导航坐标系下地球自转角速度在导航坐标系的投影可以由地球自转角速度及当地纬度计算得到：

${\mathrm{\ω}}_{\mathrm{ie}}^N=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ieE}}^n\\ {\mathrm{\ω}}_{\mathrm{ieN}}^n\\ {\mathrm{\ω}}_{\mathrm{ieU}}^n\end{array}\right]=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],$

导航坐标系相对于地球转动角速度可由航行器的航行速度和当地纬度、地球半径计算得到：

${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{enE}}^n\\ {\mathrm{\ω}}_{\mathrm{enN}}^n\\ {\mathrm{\ω}}_{\mathrm{enU}}^n\end{array}\right]=\left[\begin{array}{c}-\frac{V_{N,k}}{R}\\ \frac{V_{E,k}}{R}\\ \frac{V_{E,k}}{R}\mathrm{tgL}\end{array}\right],$

导航坐标系下计算得到的三维比力[f_E f_N f_U]^T和步骤(c)得到的载体坐标系下的比力

之间有如下变换关系：

${\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T=C_n^b{\left[\begin{array}{ccc}{f}_E& f_N& f_U\end{array}\right]}^T,$

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$

ψ，θ，γ分别为航向角、纵摇角和横摇角；

(f)将步骤(e)中求得的横摇角和纵摇角，与已获得的经度、纬度、高度、东向速度、北向速度、天向速度、航向角组成初始状态，初始化捷联惯性导航***的导航参数，实现对载体动态航行中的捷联惯性导航***动基座初始对准。

本发明与现有技术相比的优点在于：

本发明针对深海作业水下航行器工作之前动基座初始对准和长时间作业后再对准的的问题，结合卫星导航***和水声导航***的优势，对水下初始对准进行改进，实现了在水下利用卫星导航***的资源的优势，提高了***的可靠性和可用性。

附图说明

图1为本发明硬件***总体构成图。

图中标号：卫星星座1、差分卫星基准站2、卫星浮标***3、水下导航收发机4、船基数据控制中心5、海面无线电通信链路6、水下声通信链路7。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

如图1所示，深海作业水下航行器组合导航***，由导航卫星星座、卫星浮标***、水下导航收发装置、控制中心以及捷联式惯性导航***组成。

深海作业水下航行器组合导航***水下初始对准方法如下：

(1)水下航行器工作之前需要进行初始对准或者工作长时间后需要重新初始对准，数据控制中心向水下航行器的水下导航收发机发送一个请求定位的声纳脉冲信号，水下导航收发机接收到该脉冲后，再向卫星浮标发生水声定位信号；

(2)水下导航收发机向卫星浮标定位水听器发射水声定位信号；

(3)卫星浮标***接收到水声定位信号后，将水声定位信号数据及卫星信号数据进行编码和调制，利用无线电技术传送到船基数据控制中心；

(4)数据控制中心将各卫星浮标***的无线电信号进行综合处理，计算出水下航行器的位置。

(5)数据控制中心将计算出的水下航行器和水声信号发射时刻进行编码和调制，利用水声发射器将该调制信号发射到水下导航收发机；以及

水下导航收发机得到位置和时刻数据后，进一步计算出导航参数和当前时刻。

所述卫星浮标利用卫星天线接收卫星信号，利用定位水听器接受水声定位信号，利用卫星接收机时钟接口进行水声定位信号的延时测量，建立水下定位的时间基准，在将所述卫星信号、水听器延时测量信号及浮标状态数据编码、调制后，通过无线电天线发射至数据控制中心；

所述水下导航收发机响应请求水声定位信号生成或者通过用户接口直接生成水声定位信号，并通过发射换能器就爱那个该水声定位信号发射到所述卫星浮标，通过通讯换能器接收所述数据控制中心下传的定位结果数据，进一步计算出导航参数。

所属数据控制中心接收到卫星浮标和差分基站的无线电信号后，利用各浮标的卫星信号数据实时测定卫星浮标的运动状态参数，并归算到浮标定位水听器上，构成水下定位的海面动态大地测量基准；根据水听器延时测量的时间数据，采用时间或香味差分法确定水下导航收发机的三维位置并发射到水下导航收发机；

所述的海面无线电通信链路执行卫星浮标到数据控制中心的浮标各种信号、差分基准站到数据控制中心的卫星差分信号的信号实时传输；

所述的水下水声通信链路执行数据控制中心与水下导航收发机之间的水声数据通信；

***采用大地测量坐标系，并保持与陆地、空间信息的参考坐标系一致。

卫星接收机接收信号后，经过一系列的解算对捷联式惯性导航***进行初始对准，其特征在于包含如下步骤：

(a)卫星导航接收机定位数据采集：组合导航计算机以周期T_G持续从卫星导航接收机相连的接口读取卫星导航接收机输出的特定格式的导航信息，并进行解读，取得航行器的实时三维位置、三维速度和航向角，其中，三维位置包括经度λ，维度L和高度H，当地地理坐标系下的东向速度V_E、北向速度V_N和天向速度V_U，航向角ψ；

(b)卫星导航定位数据缓存：把步骤(1)中读入的组合的哦啊航计算机的卫星导航接收机输出的三维位置、三维速度和方位角与时间信息一并保持在导航计算机的内存空间变量中，缓存后的变量以此以符号λ_k-1表示经度、L_k-1表示纬度，H_k-1表示高度，V_E，k-1、V_N，k-1、V_U，k-1分别表示东向、北向和天向速度，ψ_k-1表示方位角，t_k-1表示时间信息；

(c)惯性测量单元数据采集：紧跟步骤(2)，通过模数转换或串行口，以周期T_I读取惯性测量单元中陀螺和加速度计测量的三维角速率和三维比力，捷联式惯性测量单元测得的信息均为航行器教运动和线运动在载体坐标系b下的投影，三维角速率和三维比力分别为

和

其中T_G为T_I的整数倍，x，y，z表示载体系的三个坐标轴；

(d)航行器运动加速度的计算：在初次获得卫星导航数据，并经过至少2个周期T_G的时间后，在完成卫星信息解读时刻t_k，组合导航计算机中有经度λ_k、纬度L_k，高度H_k，东向速度V_E，k，北向速度V_N，k，天向速度V_U，k，航向角ψ_k，按如下的前后差分法计算航行器的运动加速度：

$a_E=\frac{V_{E,k}-V_{E,k-1}}{T_G},$

$a_N=\frac{V_{N,k}-V_{N,k-1}}{T_G},$

$a_U=\frac{V_{U,k}-V_{U,k-1}}{T_G};$

(e)将步骤(a)、步骤(c)和步骤(d)得到的实时位置、速度、运动加速度，代入捷联惯性导航***比力方程，计算导航系下的三维比力，其计算方法如下：

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=\begin{array}{}\end{array}\left[\begin{array}{c}{a}_E\\ a_N\\ a_U\end{array}\right]+\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{ieU}}^n+{\mathrm{\ω}}_{\mathrm{enU}}^n)& (2{\mathrm{\ω}}_{\mathrm{ieN}}^n+{\mathrm{\ω}}_{\mathrm{enN}}^n)\\ (2{\mathrm{\ω}}_{\mathrm{ieU}}^n+{\mathrm{\ω}}_{\mathrm{enU}}^n)& 0& -(2{\mathrm{\ω}}_{\mathrm{ieE}}^n+{\mathrm{\ω}}_{\mathrm{enE}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{ieN}}^n+{\mathrm{\ω}}_{\mathrm{enN}}^n)& (2{\mathrm{\ω}}_{\mathrm{ieE}}^n+{\mathrm{\ω}}_{\mathrm{enE}}^n)& 0\end{array}\right]\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ V_{U,k}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g_0\end{array}\right],$

式中，导航坐标系下地球自转角速度在导航坐标系的投影可以由地球自转角速度及当地纬度计算得到：

${\mathrm{\ω}}_{\mathrm{ie}}^N=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ieE}}^n\\ {\mathrm{\ω}}_{\mathrm{ieN}}^n\\ {\mathrm{\ω}}_{\mathrm{ieU}}^n\end{array}\right]=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],$

导航坐标系相对于地球转动角速度可由航行器的航行速度和当地纬度、地球半径计算得到：

${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{enE}}^n\\ {\mathrm{\ω}}_{\mathrm{enN}}^n\\ {\mathrm{\ω}}_{\mathrm{enU}}^n\end{array}\right]=\left[\begin{array}{c}-\frac{V_{N,k}}{R}\\ \frac{V_{E,k}}{R}\\ \frac{V_{E,k}}{R}\mathrm{tgL}\end{array}\right],$

导航坐标系下计算得到的三维比力[f_E f_N f_U]^T和步骤(c)得到的载体坐标系下的比力

之间有如下变换关系：

${\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T=C_n^b{\left[\begin{array}{ccc}{f}_E& f_N& f_U\end{array}\right]}^T,$

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$

ψ，θ，γ分别为航向角、纵摇角和横摇角，通过方程可以求得横摇角和纵摇角。

(f)将步骤(e)中求得的横摇角和纵摇角，与已获得的经度、纬度、高度、东向速度、北向速度、天向速度、航向角组成初始状态，初始化捷联惯性导航***的导航参数，实现对载体动态航行中的捷联惯性导航***动基座初始对准。

Claims (3)

1.一种深海作业水下航行器组合导航***，其特征在于包括卫星星座(1)、差分卫星基准站(2)、卫星浮标***(3)、水下导航收发机(4)和船基数据控制中心(5)其中卫星星座(1)与差分卫星基准站(2)无线通信，差分卫星基准站(2)和卫星浮标***(3)分别通过海面无线电通信链路(6)与船基数据控制中心(5)连接，水下导航收发机(4)通过水下声通信链路(7)与卫星浮标***(3)连接，水下导航收发机(4)和船基数据控制中心(5)通过水下声通信链路(7)双向连接。

2.一种深海作业水下航行器组合导航***水下初始对准方法，其特征在于，包括以下步骤：

(1)船基数据控制中心(5)向水下航行器的水下导航收发机(4)发送一个请求定位的声纳脉冲信号；

(2)水下导航收发机(4)向卫星浮标***(3)发射水声定位信号；

(3)卫星浮标***(3)接收到水声定位信号后，将水声定位信号数据及接收到的卫星信号数据进行编码和调制得到无线电信号，利用海面无线电通信链路(6)将无线电信号传送到船基数据控制中心(5)；

(4)船基数据控制中心(5)将各卫星浮标***(3)的无线电信号进行计算得到水下航行器的位置；

(5)船基数据控制中心(5)将计算出的水下航行器的位置和水声定位信号发射时刻进行编码和调制得到调制信号，利用水声发射器将调制信号发射到水下导航收发机(4)；水下导航收发机(4)根据调制信号计算出导航参数和当前时刻。

3.根据权利要求2所述的深海作业水下航行器组合导航***水下初始对准方法，其特征在于步骤(5)所述的水下导航收发机(4)根据调制信号计算出导航参数和当前时刻的方法包含如下步骤：

(a)水下导航收发机(4)定位数据采集：采用组合导航计算机以周期TG持续读取水下导航收发机(4)接收到的导航信息即调制信号，并进行解读，取得航行器的实时三维位置、三维速度和航向角ψ和时间t，其中，三维位置包括经度λ、维度L和高度H，三维速度包括当地地理坐标系下的东向速度V_E、北向速度V_N和天向速度V_U；

(b)卫星导航定位数据缓存：把步骤(a)中所述的航行器的三维位置、三维速度、航向角与时间信息一并保持在导航计算机的内存空间变量中，缓存后得到经度λ_k-1、纬度L_k-1、H_k-1高度，东向速度V_E，k-1、北向速度V_N，k-1和天向速度V_U，k-1，航向角ψ_k-1，时间信息t_k-1；

(c)惯性测量单元数据采集：以周期T_I读取惯性测量单元中陀螺和加速度计测量的三维角速率和三维比力，捷联式惯性测量单元测得的信息均为航行器教运动和线运动在载体坐标系b下的投影，三维角速率和三维比力分别为

和

其中T_G为T_I的整数倍，x，y，z表示载体系的三个坐标轴；

(d)航行器运动加速度的计算：经过至少2个周期T_G的时间后，在完成卫星信息解读时刻t_k，组合导航计算机中有经度λ_k、纬度L_k，高度H_k，东向速度V_E，k，北向速度V_N，k，天向速度V_U，k，航向角ψ_k，按如下的前后差分法计算航行器的东向、北向和天向运动加速度：

$a_E=\frac{V_{E,k}-V_{E,k-1}}{T_G},$

$a_N=\frac{V_{N,k}-V_{N,k-1}}{T_G},$

$a_U=\frac{V_{U,k}-V_{U,k-1}}{T_G};$

(e)将步骤(a)、步骤(c)和步骤(d)得到的实时位置、速度、运动加速度，代入捷联惯性导航***比力方程，计算导航系下的三维比力，其计算方法如下：

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=\left[\begin{array}{c}{a}_E\\ a_N\\ a_U\end{array}\right]+\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{ieU}}^n+{\mathrm{\ω}}_{\mathrm{enU}}^n)& (2{\mathrm{\ω}}_{\mathrm{ieN}}^n+{\mathrm{\ω}}_{\mathrm{enN}}^n)\\ (2{\mathrm{\ω}}_{\mathrm{ieU}}^n+{\mathrm{\ω}}_{\mathrm{enU}}^n)& 0& -(2{\mathrm{\ω}}_{\mathrm{ieE}}^n+{\mathrm{\ω}}_{\mathrm{enE}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{ieN}}^n+{\mathrm{\ω}}_{\mathrm{enN}}^n)& (2{\mathrm{\ω}}_{\mathrm{ieE}}^n+{\mathrm{\ω}}_{\mathrm{enE}}^n)& 0\end{array}\right]\left[\begin{array}{c}{V}_{E,k}\\ V_{N,k}\\ V_{U,k}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ g_0\end{array}\right],$

式中，导航坐标系下地球自转角速度在导航坐标系的投影可以由地球自转角速度及当地纬度计算得到：

${\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ieE}}^n\\ {\mathrm{\ω}}_{\mathrm{ieN}}^n\\ {\mathrm{\ω}}_{\mathrm{ieU}}^n\end{array}\right]=\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right],$

导航坐标系相对于地球转动角速度可由航行器的航行速度和当地纬度、地球半径计算得到：

${\mathrm{\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{enE}}^n\\ {\mathrm{\ω}}_{\mathrm{enN}}^n\\ {\mathrm{\ω}}_{\mathrm{enU}}^n\end{array}\right]=\left[\begin{array}{c}-\frac{V_{N,k}}{R}\\ \frac{V_{E,k}}{R}\\ \frac{V_{E,k}}{R}\mathrm{tgL}\end{array}\right],$

导航坐标系下计算得到的三维比力[f_E f_N f_U]^T和步骤(c)得到的载体坐标系下的比力

之间有如下变换关系：

${\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T=C_n^b{\left[\begin{array}{ccc}{f}_E& f_N& f_U\end{array}\right]}^T,$

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& -\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\sin \mathrm{\ψ}& -\sin \mathrm{\γ}\sin \mathrm{\ψ}-\cos \mathrm{\γ}\sin \mathrm{\θ}\cos \mathrm{\ψ}& \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right],$

ψ，θ，γ分别为航向角、纵摇角和横摇角；

(f)将步骤(e)中求得的横摇角和纵摇角，与已获得的经度、纬度、高度、东向速度、北向速度、天向速度、航向角组成初始状态，初始化捷联惯性导航***的导航参数，实现对载体动态航行中的捷联惯性导航***动基座初始对准。

Images