CN102269594B - 基于路径积分的空间导航方法 - Google Patents

Abstract

本发明涉及一种基于路径积分的空间导航方法，其特征是根据沙蚁利用偏振光按照路径积分方法进行导航的思想，在建立空间导航所需的地理坐标系和运载体坐标系的基础上，首先通过运载体坐标系和地理坐标系之间的旋转矩阵映射关系实时得出运载体当前运动局部矢量与地理坐标系三轴向的夹角，并根据运载体当前位置与前一位置之间的局部运动距离，结合路径积分整合运算实时精准的获得初始位置相对当前位置的全局运动矢量。本发明提供了一种空间运载体导航的路径积分方法，可以用于空间运载体的导航。

Description

基于路径积分的空间导航方法

技术领域

本发明涉及导航与定位技术领域，是一种基于路径积分的空间导航方法，特别涉及一种根据运载体局部运动距离、运载体姿态信息实时获得运载体当前位置相对初始位置全局运动矢量的方法，可以用于空间运载体的导航与定位。

背景技术

导航是一种为运载体运动时提供连续、安全和可靠服务的技术，无论是在军事还是民用领域，都有着极为广泛的应用，随着科技进步与社会发展，出现了越来越多的导航方式，其中，自主导航成为一个新的研究热点。以20世纪70年代以来信息技术的发展为基础，出现了一系列新的导航技术，其中包括卫星导航、惯性导航、地磁导航、偏振光导航以及组合导航等，这些系统更能满足军事和民用对运载体航行引导的要求，然而，这些新型方法主要是在新时期各种军事操作需求的推动下发展起来的，每个方法都具有很强的针对性，其导航方法和传感器均有其特性和适用范围，特别是在无通信、弱/无卫星等恶劣条件下，很多导航误差非常大，甚至不能完成导航任务。

路径积分特别适合陌生环境下的导航，是沙蚁、蜜蜂等生物利用偏振光导航过程的主要策略，沙蚁等在外出觅食的过程中不断地通过天空中的偏振光获得运动的角度(罗盘信息)，通过本体感受器记录在每个方向上的运动距离，并将这些角度和距离进行积分(矢量叠加)，便可以得到任意时刻相对于出发点的位置和行进距离，从而始终维持一个指向出发点的全局矢量，这个矢量联系着当前位置和出发点，因此能在结束漫游后以直线路径返回出发点，这个过程称为路径积分。路径积分原理简单，实现方便，导航精度高，可以与其他导航方法结合，为运载体实时提供精确、连续、有效的导航信息。

近年来，对于路径积分乃至偏振光导航的研究大都致力于平面内应用，对于将路径积分方法用于空间，实现空间运载体导航方面的研究还未见相关研究论文与报告。

发明内容

本发明是针对上述技术存在的不足之处，提供一种基于路径积分的空间导航方法，将沙蚁利用偏振光按照路径积分导航的方法推广到三维空间，以达到三维空间中的运载体利用自然特性进行导航的目的，为三维空间中运载体的自主导航提供一种新的方法，以使得在无通信、弱/无卫星等恶劣条件下，同样能够获得准确导航。

本发明解决技术问题采用如下技术方案：

本发明基于路径积分的空间导航方法的特点是按如下过程进行：

首先以运载体初始位置为原点O、以运载体初始位置所在纬线指向东的方向为X轴方向，以运载体初始位置所在子午线指向北的方向为Y轴方向，以运载体初始位置所在地理垂线指向上方的方向为Z轴方向，建立描述三维导航所需的东北天地理坐标系OXYZ；所述东北天地理坐标系OXYZ中的XOY平面为大地水准面，YOZ平面为子午面；以所述运载体质心为原点O_i、以垂直于运载体运动方向水平向右的方向为X_i轴方向、以运载体的运动方向为Y_i轴方向，以垂直于X_iO_iY_i面指向上的方向为Z_i轴方向，建立运载体坐标系O_iX_iY_iZ_i，i＝1，L n，n为运载体从初始位置O出发，在空间中运动时路径积分整合运算的步数；运载体从初始位置O运动到空间中第i个位置O_i，初始位置O相对于第i个位置O_i的全局运动矢量表示为：

式(1)中，α_i为初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的X轴之间的夹角；β_i为初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的Y轴之间的夹角；γ_i为初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的Z轴之间的夹角，所述α_i、β_i和γ_i的取值范围均为[0°，180°]；l_i为初始位置O与第i个位置O_i之间的距离；第i个位置O_i相对于第i-1个位置O_i-1的运动距离用l_i-1，i表征；

n步路径积分过程是：

第1步：运载体从初始位置O运动到第一个位置O₁，通过坐标系映射矩阵得出全局运动矢量

与地理坐标系三轴之间的夹角为α′₁、β′₁、γ′₁，全局运动矢量为：

式(2)中，α₁、β₁和γ₁与α′₁、β′₁和γ′₁分别对应相等；

第2步：运载体从第一个位置O₁运动到第二个位置O₂，第一个位置O₁与第二个位置O₂之间的距离为l_1，2，通过坐标系映射矩阵得出运载体局部运动矢量与东北天地理坐标系的三个轴的夹角为α’₂、β’₂、γ’₂，则此时运载体局部运动矢量

和第1步中全局运动矢量

之间的夹角余弦为：

$\cos {\mathrm{\φ}}_2=\frac{\cos {\mathrm{\α}}_1\cos {\mathrm{\α}}_2^{\′}+\cos {\mathrm{\β}}_1\cos {\mathrm{\β}}_2^{\′}+\cos {\mathrm{\γ}}_1\cos {\mathrm{\γ}}_2^{\′}}{\sqrt{{\cos }^2{\mathrm{\α}}_1+{\cos }^2{\mathrm{\β}}_1+{\cos }^2{\mathrm{\γ}}_1}\sqrt{{\cos }^2{\mathrm{\α}}_2^{\′}+{\cos }^2{\mathrm{\β}}_2^{\′}+{\cos }^2{\mathrm{\γ}}_2^{\′}}}---\left(3\right)$

由式(3)、l₁和l_1，2，根据三角形几何运算得：

$l_2=\sqrt{l_1^2+l_{\mathrm{1,2}}^2+2l_1{l}_{\mathrm{1,2}}\cos {\mathrm{\φ}}_2}---\left(4\right)$

在XOY面上投影

的长度为：

l_1XY＝l₁sinγ₁    (5)

在XOY面上投影

的长度为：

l_1，2XY＝l_1，2sinγ′₂    (6)

在XOY面上的投影

和

在XOY面上的投影

之间的夹角余弦为：

$\cos {\mathrm{\φ}}_{2\mathrm{XY}}=\frac{\cos {\mathrm{\α}}_1}{\sin {\mathrm{\γ}}_1}\frac{\cos {\mathrm{\α}}_2^{\′}}{\sin {\mathrm{\γ}}_2^{\′}}+\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_1}{\sin {\mathrm{\γ}}_1}\right)}^2}\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_2^{\′}}{\sin {\mathrm{\γ}}_2^{\′}}\right)}^2}---\left(7\right)$

由式(7)、l_1XY以及l_1，2XY，根据三角运算得出在XOY面上投影矢量

和

之和的长度为：

$l_{2\mathrm{XY}}=\sqrt{l_{1\mathrm{XY}}^2+l_{\mathrm{1,2}\mathrm{XY}}^2+2l_{1\mathrm{XY}}{l}_{\mathrm{1,2}\mathrm{XY}}\cos {\mathrm{\φ}}_{2\mathrm{XY}}}---\left(8\right)$

故得全局运动矢量

与地理坐标系Z轴的夹角为：

${\mathrm{\γ}}_2=\arcsin \frac{l_{2\mathrm{XY}}}{l_2}---\left(9\right)$

将

分别在YOZ面和XOZ面上投影，并通过式(5)～式(9)的计算，分别获得全局运动矢量

与地理坐标系X轴和Y轴的夹角为α₂、β₂；则初始位置O与第2个位置O₂的全局运动矢量为：

第n步：运载体从第n-1个位置O_n-1运动到第n个位置O_n，O_n-1与O_n之间的距离为l_n-1，n，通过坐标系映射矩阵得出运载体局部运动矢量

与地理坐标系的三轴的三个夹角分别为α′_n、β′_n、γ′_n，此时运载体局部运动矢量和第n-1步中全局运动矢量

之间的夹角余弦为：

$\cos {\mathrm{\φ}}_n=\frac{\cos {\mathrm{\α}}_{n-1}\cos {\mathrm{\α}}_n+\cos {\mathrm{\β}}_{n-1}\cos {\mathrm{\β}}_n+\cos {\mathrm{\γ}}_{n-1}\cos {\mathrm{\γ}}_n}{\sqrt{{\cos }^2{\mathrm{\α}}_{n-1}+{\cos }^2{\mathrm{\β}}_{n-1}+{\cos }^2{\mathrm{\γ}}_{n-1}}\sqrt{{\cos }^2{\mathrm{\α}}_n+{\cos }^2{\mathrm{\β}}_n+{\cos }^2{\mathrm{\γ}}_n}}---\left(11\right)$

由式(11)、l_n和l_n-1，n，根据三角形几何运算得：

$l_n=\sqrt{l_{n-1}^2+l_{n-1,n}^2+2l_{n-1}{l}_{n-1,n}\cos {\mathrm{\φ}}_n}---\left(12\right)$

在XOY面投影

的长度为：

l_n-1XY＝l_n-1sinγ_n-1    (13)

在XOY面投影的长度为：

l_n-1，nXY＝l_n-1，nsinγ′_n    (14)

投影

和

之间的夹角余弦为：

$\cos {\mathrm{\φ}}_{\mathrm{nXY}}=\frac{\cos {\mathrm{\α}}_{n-1}}{\sin {\mathrm{\γ}}_{n-1}}\frac{\cos {\mathrm{\α}}_n^{\′}}{\sin {\mathrm{\γ}}_n^{\′}}+\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_{n-1}}{\sin {\mathrm{\γ}}_{n-1}}\right)}^2}\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_n^{\′}}{\sin {\mathrm{\γ}}_n^{\′}}\right)}^2}---\left(15\right)$

由式(15)、l_n-1XY以及l_n-1，nXY，根据三角运算得出XOY面上的投影矢量

和之和

的长度为：

$l_{\mathrm{nXY}}=\sqrt{l_{n-1\mathrm{XY}}^2+s_{n-1,\mathrm{nXY}}^2+2l_{n-1\mathrm{XY}}{l}_{n-1,\mathrm{nXY}}\cos {\mathrm{\φ}}_{\mathrm{nXY}}}---\left(16\right)$

故得全局运动矢量与地理坐标系Z轴的夹角为：

${\mathrm{\γ}}_n=\arcsin \frac{l_{\mathrm{nXY}}}{l_n}---\left(17\right)$

将

分别在YOZ面和XOZ面上投影，并通过式(13)～式(17)进行相同方法的计算，分别获得运载体全局运动矢量

与地理坐标系X轴和Y轴的夹角为α_n、β_n；故得初始位置O与第n个位置O_n的全局运动矢量为

本发明基于路径积分的空间导航方法的特点也在于当前运载体局部运动矢量与地理坐标系三轴之间的夹角α′_i、β′_i、γ′_i是按如下过程进行：

运载体运动到第i个位置O_i时，运载体坐标系为O_iX_iY_iZ_i，根据相对运动原理，寻找映射关系C_i使得运载体坐标系O_iX_iY_iZ_i与导航所需的地理坐标系OXYZ的三个坐标轴方向一致；假设此时运载体坐标系为0系，先绕自身O_iX_i轴正向顺时针旋转α′_i角得到a系，而后绕自身O_iY_i轴正向顺时针旋转β′_i角得到b系，最后绕自身O_iZ_i轴正向顺时针旋转γ′_i角得到c系，此时O_iX_iY_iZ_i与OXYZ轴向重合；

坐标系O_iX_iY_iZ_i绕自身O_iX_i轴正向旋转α′_i角后的旋转矩阵为：

$C_{{\mathrm{\α}}_i^{\′}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_i^{\′}& \sin {\mathrm{\α}}_i^{\′}\\ 0& -\sin {\mathrm{\α}}_i^{\′}& \cos {\mathrm{\α}}_i^{\′}\end{array}\right]---\left(19\right)$

坐标系O_iX_iY_iZ_i绕自身O_iY_i轴正向旋转β′_i角后的旋转矩阵为：

$C_{{\mathrm{\β}}_i^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i^{\′}& 0& -\sin {\mathrm{\β}}_i^{\′}\\ 0& 1& 0\\ \sin {\mathrm{\β}}_i^{\′}& 0& \cos {\mathrm{\β}}_i^{\′}\end{array}\right]---\left(20\right)$

坐标系O_iX_iY_iZ_i绕自身O_iZ_i轴正向旋转γ′_i角后的旋转矩阵为：

$C_{{\mathrm{\γ}}_i^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_i^{\′}& \sin {\mathrm{\γ}}_i^{\′}& 0\\ -\sin {\mathrm{\γ}}_i^{\′}& \cos {\mathrm{\γ}}_i^{\′}& 0\\ 0& 0& 1\end{array}\right]---\left(21\right)$

则O_iX_iY_iZ_i系与绕自身O_iX_i轴正向旋转α′_i角、绕自身O_iY_i轴正向旋转β′_i角、再绕自身O_iZ_i轴正向旋转γ′_i角后得到的坐标系之间的变换矩阵为：

$C_i=C_{{\mathrm{\α}}_i^{\′}}{C}_{{\mathrm{\β}}_i^{\′}}{C}_{{\mathrm{\γ}}_i^{\′}}---\left(22\right)$

$C_i=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}& \sin {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}+\cos {\mathrm{\α}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}& -\cos {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}+\sin {\mathrm{\α}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}\\ -\cos {\mathrm{\β}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}& -\sin {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}+\cos {\mathrm{\α}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}& \cos {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}+\sin {\mathrm{\α}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}\\ \sin {\mathrm{\β}}_i^{\′}& -\sin {\mathrm{\α}}_i^{\′}\cos {\mathrm{\β}}_i^{\′}& \cos {\mathrm{\α}}_i^{\′}\cos {\mathrm{\β}}_i^{\′}\end{array}\right]---\left(23\right)$

通过求解变换矩阵C_i得出运载体当前局部运动矢量

与地理坐标系OXYZ三个坐标轴之间的夹角为α′_i、β′_i、γ′_i。

本发明将路径积分用于三维空间用于实现空间内运载体的导航，本发明根据沙蚁利用偏振光按照路径积分方法进行导航的思想，在建立空间导航所需的地理坐标系和运载体坐标系的基础上，首先通过运载体坐标系和地理坐标系之间的旋转矩阵映射关系实时得出运载体当前局部运动矢量与地理坐标系三轴向的夹角，并根据运载体当前位置与前一位置之间的局部运动距离，结合路径积分整合运算实时精准的获得初始位置相对当前位置的全局运动矢量，为运载体的空间导航提供依据，从而完成导航任务。

与已有技术相比，本发明有益效果体现在：

1、本发明针对现有空间导航技术存在的不足，提出了一种将生物偏振光导航技术中的路径积分思想用于空间，实现空间运载体的路径积分导航方法，为三维空间中运载体的自主导航提供了新的思路与方法。

2、本发明通过获得的运载体姿态信息，根据运载体坐标系和地理坐标系之间的映射关系矩阵，实时获得运载体当前局部运动矢量与地理坐标系三轴向之间的夹角，再结合局部距离信息和路径积分整合运算，实时维持运载体当前位置与初始位置的全局运动矢量，满足了在无通信的情况下，实现空间运载体自主导航的可行性要求。

3、本发明中采集运载体的姿态信息不受人为因素破坏，因此本方法克服了运载体在三维空间导航时，导航信息受外部人为因素破坏的缺陷，是对三维空间载体自主导航方法的有益补充。

附图说明

图1为本发明中基于路径积分的空间导航方法总体框图；

图2为本发明中三维东北天坐标系示意图；

图3为本发明中空间运载体坐标系示意图；

图4为本发明中运载体传感器坐标系与地理坐标系之间的映射关系图；

图5为本发明中三维路径积分空间导航示意图；

具体实施方式

参见图1、图2，本实施例建立描述空间导航的空间直角坐标系OXYZ，以初始位置运载体质心为原点O，以运载体初始位置纬线指向东的方向为X轴方向，以运载体初始位置子午线指向北的方向为Y轴方向，以运载体初始位置地理垂线指向上方的方向为Z轴方向，建立东北天坐标系OXYZ。东北天坐标系OXYZ中XOY平面为大地水准面，YOZ平面为当地子午面。

参见图3，以运载体质心为原点O_i，以垂直于运载体运动方向水平向右的方向为X_i轴方向，以运载体的运动方向为Y_i轴方向，以垂直于X_iO_iY_i面指向上的方向为Z_i轴方向，建立运载体坐标系O_iX_iY_iZ_i，i＝1，L n，n为运载体从初始位置O出发，在空间中运动时路径积分整合运算的步数；运载体从初始位置O运动到空间中第i个位置O_i，初始位置O相对于第i个位置O_i的全局运动矢量表示为

式(1)中，α_i表示初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的X轴之间的夹角，β_i表示初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的Y轴之间的夹角，γ_i表示初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的Z轴之间的夹角，其中，α_i、β_i和γ_i的取值范围均为[0°，180°]；l_i表示初始位置O与第i个位置O_i之间的距离。第i个位置O_i相对于第i-1个位置O_i-1的运动距离用l_i-1，i表征。

运载体运动到第i个位置O_i时，运载体坐标系为O_iX_iY_iZ_i，根据相对运动原理，寻找映射关系C_i使得运载体坐标系O_iX_iY_iZ_i与导航所需的地理坐标系OXYZ的三个坐标轴方向一致，即通过坐标系的映射旋转获得此时运载体在地理坐标系中的三维姿态信息，从何为路径积分提供局部运动矢量的角度信息。假设此时运载体坐标系为0系，先绕自身O_iX_i轴正向顺时针旋转α′_i角得到a系，而后绕自身O_iY_i轴正向顺时针旋转β′_i角得到b系，最后绕自身O_iZ_i轴正向顺时针旋转γ′_i角得到c系，此时O_iX_iY_iZ_i与OXYZ轴向重合，如图4所示。

坐标系O_iX_iY_iZ_i绕自身O_iX_i轴正向旋转α′_i角后的旋转矩阵为：

$C_{{\mathrm{\α}}_i^{\′}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_i^{\′}& \sin {\mathrm{\α}}_i^{\′}\\ 0& -\sin {\mathrm{\α}}_i^{\′}& \cos {\mathrm{\α}}_i^{\′}\end{array}\right]---\left(2\right)$

坐标系O_iX_iY_iZ_i绕自身O_iY_i轴正向旋转β′_i角后的旋转矩阵为：

$C_{{\mathrm{\β}}_i^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i^{\′}& 0& -\sin {\mathrm{\β}}_i^{\′}\\ 0& 1& 0\\ \sin {\mathrm{\β}}_i^{\′}& 0& \cos {\mathrm{\β}}_i^{\′}\end{array}\right]---\left(3\right)$

坐标系O_iX_iY_iZ_i绕自身O_iZ_i轴正向旋转γ′_i角后的旋转矩阵为：

$C_{{\mathrm{\γ}}_i^{\′}}=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_i^{\′}& \sin {\mathrm{\γ}}_i^{\′}& 0\\ -\sin {\mathrm{\γ}}_i^{\′}& \cos {\mathrm{\γ}}_i^{\′}& 0\\ 0& 0& 1\end{array}\right]---\left(4\right)$

则O_iX_iY_iZ_i系与绕自身O_iX_i轴正向旋转α′_i角、绕自身O_iY_i轴正向旋转β′_i角、再绕自身O_iZ_i轴正向旋转γ′_i角后得到的坐标系之间的变换矩阵为：

$C_i=C_{{\mathrm{\α}}_i^{\′}}{C}_{{\mathrm{\β}}_i^{\′}}{C}_{{\mathrm{\γ}}_i^{\′}}---\left(5\right)$

$C_i=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}& \sin {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}+\cos {\mathrm{\α}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}& -\cos {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}+\sin {\mathrm{\α}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}\\ -\cos {\mathrm{\β}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}& -\sin {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}+\cos {\mathrm{\α}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}& \cos {\mathrm{\α}}_i^{\′}\sin {\mathrm{\β}}_i^{\′}\sin {\mathrm{\γ}}_i^{\′}+\sin {\mathrm{\α}}_i^{\′}\cos {\mathrm{\γ}}_i^{\′}\\ \sin {\mathrm{\β}}_i^{\′}& -\sin {\mathrm{\α}}_i^{\′}\cos {\mathrm{\β}}_i^{\′}& \cos {\mathrm{\α}}_i^{\′}\cos {\mathrm{\β}}_i^{\′}\end{array}\right]---\left(6\right)$

通过求解变换矩阵C_i得出运载体当前局部运动矢量

与地理坐标系OXYZ三个坐标轴之间的夹角为α′_i、β′_i、γ′_i。

假设运载体在空间运动时进行n步路径积分整合运算，与平面中的路径积分方法类似，空间中的路径积分的两个重要因素为方向和距离，距离信息可以直接由运载体获得；首先根据坐标系映射矩阵获得当前局部运动矢量与地理坐标系三个坐标轴之间的夹角，然后将前一时刻全局运动矢量和当前局部运动矢量在地理坐标系三个平面上进行投影，根据平面上的路径积分，获得当前全局运动矢量在三个平面XOY、YOZ和XOZ面上的投影，再根据三角函数的运算，获得当前全局运动矢量与地理坐标系OXYZ三个轴向的夹角，即路径积分所需的方向信息，从而为下一步路径积分或者运载体导航提供依据，如图1所示；n步路径积分过程如下：

第1步参见图5，运载体从初始位置O运动到第1个位置O₁点，载体坐标系O₁X₁Y₁Z₁与地理坐标系OXYZ的映射矩阵为

$C_1=\left[\begin{array}{ccc}\cos {\mathrm{\β}}_1^{\′}\cos {\mathrm{\γ}}_1^{\′}& \sin {\mathrm{\α}}_1^{\′}\sin {\mathrm{\β}}_1^{\′}\cos {\mathrm{\γ}}_1^{\′}+\cos {\mathrm{\α}}_1^{\′}\sin {\mathrm{\γ}}_1^{\′}& -\cos {\mathrm{\α}}_1^{\′}\sin {\mathrm{\β}}_1^{\′}\cos {\mathrm{\γ}}_1^{\′}+\sin {\mathrm{\α}}_1^{\′}\sin {\mathrm{\γ}}_1^{\′}\\ -\cos {\mathrm{\β}}_1^{\′}\sin {\mathrm{\γ}}_1^{\′}& -\sin {\mathrm{\α}}_1^{\′}\sin {\mathrm{\β}}_1^{\′}\sin {\mathrm{\γ}}_1^{\′}+\cos {\mathrm{\α}}_1^{\′}\cos {\mathrm{\γ}}_1^{\′}& \cos {\mathrm{\α}}_1^{\′}\sin {\mathrm{\β}}_1^{\′}\sin {\mathrm{\γ}}_1^{\′}+\sin {\mathrm{\α}}_1^{\′}\cos {\mathrm{\γ}}_1^{\′}\\ \sin {\mathrm{\β}}_1^{\′}& -\sin {\mathrm{\α}}_1^{\′}\cos {\mathrm{\β}}_1^{\′}& \cos {\mathrm{\α}}_1^{\′}\cos {\mathrm{\β}}_1^{\′}\end{array}\right]---\left(7\right)$

则运载体当前第一个位置O₁相对初始位置O的全局运动矢量

与地理坐标系OXYZ三个坐标轴之间的夹角分别为α′₁、β′₁、γ′₁。故全局运动矢量为

其中α₁、β₁、γ₁与α′₁、β′₁、γ′₁分别对应相等；

第2步参见图5，运载体从第一个位置O₁运动到第二个位置O₂，O₁与O₂之间的距离为l_1，2，通过坐标系映射矩阵得出局部运动矢量

与东北天地理坐标系的三个轴的夹角为α’₂、β’₂、γ’₂，所以此时运载体全局运动矢量和第1步中全局运动矢量之间的夹角余弦为

$\cos {\mathrm{\φ}}_2=\frac{\cos {\mathrm{\α}}_1\cos {\mathrm{\α}}_2^{\′}+\cos {\mathrm{\β}}_1\cos {\mathrm{\β}}_2^{\′}+\cos {\mathrm{\γ}}_1\cos {\mathrm{\γ}}_2^{\′}}{\sqrt{{\cos }^2{\mathrm{\α}}_1+{\cos }^2{\mathrm{\β}}_1+{\cos }^2{\mathrm{\γ}}_1}\sqrt{{\cos }^2{\mathrm{\α}}_2^{\′}+{\cos }^2{\mathrm{\β}}_2^{\′}+{\cos }^2{\mathrm{\γ}}_2^{\′}}}---\left(9\right)$

由式(9)、l₁和l_1，2，根据三角形几何运算得

$l_2=\sqrt{l_1^2+l_{\mathrm{1,2}}^2+2l_1{l}_{\mathrm{1,2}}\cos {\mathrm{\φ}}_2}---\left(10\right)$

在XOY面上投影

的长度为

l_1XY＝l₁sinγ₁    (11)

在XOY面上投影

的长度为

l_1，2XY＝l_1，2sinγ′₂    (12)

在XOY面上的投影

和

在XOY面上的投影

之间的夹角余弦为

$\cos {\mathrm{\φ}}_{2\mathrm{XY}}=\frac{\cos {\mathrm{\α}}_1}{\sin {\mathrm{\γ}}_1}\frac{\cos {\mathrm{\α}}_2^{\′}}{\sin {\mathrm{\γ}}_2^{\′}}+\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_1}{\sin {\mathrm{\γ}}_1}\right)}^2}\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_2^{\′}}{\sin {\mathrm{\γ}}_2^{\′}}\right)}^2}---\left(13\right)$

由式(13)、l_1XY以及l_1，2XY，根据三角运算得出在XOY面上投影矢量

和

之和

的长度为

$l_{2\mathrm{XY}}=\sqrt{l_{1\mathrm{XY}}^2+l_{\mathrm{1,2}\mathrm{XY}}^2+2l_{1\mathrm{XY}}{l}_{\mathrm{1,2}\mathrm{XY}}\cos {\mathrm{\φ}}_{2\mathrm{XY}}}---\left(14\right)$

故得全局运动矢量

与地理坐标系Z轴的夹角为

${\mathrm{\γ}}_2=\arcsin \frac{l_{2\mathrm{XY}}}{l_2}---\left(15\right)$

α₂和β₂需要将

分别在YOZ面和XOZ面上投影，并通过平面上的路径积分获得；将

在YOZ面上投影，通过与上述相同的投影方法，以式(11)～式(15)同样的计算方式，在平面YOZ上进行路径积分整合可以获得全局运动矢量

与地理坐标系X轴的夹角为α₂，将在XOZ面上投影，通过上述相同的投影方法，以式(11)～式(15)同样的计算方式，在平面XOZ上进行路径积分整合可以获得全局运动矢量

与地理坐标系Y轴的夹角为β₂。则初始位置O与第2个位置O₂的全局运动矢量为

第n步  参见图5，运载体从第n-1个位置O_n-1运动到第n个位置O_n，O_n-1与O_n之间的距离为l_n-1，n，通过坐标系映射矩阵得出运载体局部运动矢量

与地理坐标系的三轴的三个夹角为α′_n、β′_n、γ′_n，所以此时运载体运动方向和第n-1步中全局运动矢量之间的夹角余弦为

$\cos {\mathrm{\φ}}_n=\frac{\cos {\mathrm{\α}}_{n-1}\cos {\mathrm{\α}}_n+\cos {\mathrm{\β}}_{n-1}\cos {\mathrm{\β}}_n+\cos {\mathrm{\γ}}_{n-1}\cos {\mathrm{\γ}}_n}{\sqrt{{\cos }^2{\mathrm{\α}}_{n-1}+{\cos }^2{\mathrm{\β}}_{n-1}+{\cos }^2{\mathrm{\γ}}_{n-1}}\sqrt{{\cos }^2{\mathrm{\α}}_n+{\cos }^2{\mathrm{\β}}_n+{\cos }^2{\mathrm{\γ}}_n}}---\left(17\right)$

由式(17)、l_n和l_n-1，n，根据三角形几何运算得

$l_n=\sqrt{l_{n-1}^2+l_{n-1,n}^2+2l_{n-1}{l}_{n-1,n}\cos {\mathrm{\φ}}_n}---\left(18\right)$

在XOY面投影

的长度为

l_n-1XY＝l_n-1sinγ_n-1    (19)

在XOY面投影

的长度为

l_n-1，nXY＝l_n-1，nsinγ′_n    (20)

投影

和

之间的夹角余弦为

$\cos {\mathrm{\φ}}_{\mathrm{nXY}}=\frac{\cos {\mathrm{\α}}_{n-1}}{\sin {\mathrm{\γ}}_{n-1}}\frac{\cos {\mathrm{\α}}_n^{\′}}{\sin {\mathrm{\γ}}_n^{\′}}+\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_{n-1}}{\sin {\mathrm{\γ}}_{n-1}}\right)}^2}\sqrt{1-{\left(\frac{\cos {\mathrm{\α}}_n^{\′}}{\sin {\mathrm{\γ}}_n^{\′}}\right)}^2}---\left(21\right)$

由式(21)、l_n-1XY以及l_n-1，nXY，根据三角运算得出XOY面上的投影矢量

和

之和

的长度为

$l_{\mathrm{nXY}}=\sqrt{l_{n-1\mathrm{XY}}^2+s_{n-1,\mathrm{nXY}}^2+2l_{n-1\mathrm{XY}}{l}_{n-1,\mathrm{nXY}}\cos {\mathrm{\φ}}_{\mathrm{nXY}}}---\left(22\right)$

故得全局运动矢量与地理坐标系Z轴的夹角为

${\mathrm{\γ}}_n=\arcsin \frac{l_{\mathrm{nXY}}}{l_n}---\left(23\right)$

α_n和β_n需要将

分别在YOZ面和XOZ面上投影，并通过平面上的路径积分获得；将在YOZ面上投影，通过与上述相同的投影方法，以式(19)～式(23)同样的计算方式，在平面YOZ上进行路径积分整合可以获得全局运动矢量与地理坐标系X轴的夹角为α_n，将在XOZ面上投影，通过与上述相同的投影方法，以式(19)～式(23)同样的计算方式，在平面XOZ上进行路径积分整合可以获得全局运动矢量

与地理坐标系Y轴的夹角为β_n。故得初始位置O与第n个位置O_n的全局运动矢量为

上述过程完成了将路径积分推广至空间，实现三维空间中运载体的自主导航，通过运载体坐标系和地理坐标系之间的旋转矩阵映射关系实时获得运载体当前局部运动矢量与地理坐标系三轴向的夹角，并根据运载体当前位置与前一位置之间的局部运动距离，结合路径积分整合运算实时精准的获得运载体初始位置相对当前位置的全局运动矢量，为运载体的空间导航提供依据，从而完成自主导航任务。

Claims (1)

1.一种基于路径积分的空间导航方法，其特征是按如下过程进行： 

首先以运载体初始位置为原点O、以运载体初始位置所在纬线指向东的方向为X轴方向，以运载体初始位置所在子午线指向北的方向为Y轴方向，以运载体初始位置所在地理垂线指向上方的方向为Z轴方向，建立描述三维导航所需的东北天地理坐标系OXYZ；所述东北天地理坐标系OXYZ中的XOY平面为大地水准面，YOZ平面为子午面；以所述运载体质心为原点O_i、以垂直于运载体运动方向水平向右的方向为X_i轴方向、以运载体的运动方向为Y_i轴方向，以垂直于X_iO_iY_i面指向上的方向为Z_i轴方向，建立运载体坐标系O_iX_iY_iZ_i，i=1,…n，n为运载体从初始位置O出发，在空间中运动时路径积分整合运算的步数；运载体从初始位置O运动到空间中第i个位置O_i，初始位置O相对于第i个位置O_i的全局运动矢量表示为： 

式(1)中，α_i为初始位置O相对第i个位置O_i的全局运动矢量与地理坐标系OXYZ的X轴之间的夹角；β_i为初始位置O相对第i个位置O_i的全局运动矢量与地理坐标系OXYZ的Y轴之间的夹角；γ_i为初始位置O相对第i个位置O_i的全局运动矢量

与地理坐标系OXYZ的Z轴之间的夹角，所述α_i、β_i和γ_i的取值范围均为[0°，180°]；l_i为初始位置O与第i个位置O_i之间的距离；第i个位置O_i相对于第i-1个位置O_i-1的运动距离用l_i-1,i表征； 

n步路径积分过程是： 

第1步：运载体从初始位置O运动到第一个位置O₁，通过坐标系映射矩阵得出全局运动矢量

与地理坐标系三轴之间的夹角为α₁′、β₁′、γ₁′，全局运动矢量为： 

式(2)中，α₁、β₁和γ₁与α₁′、β₁′和γ₁′分别对应相等； 

第2步：运载体从第一个位置O₁运动到第二个位置O₂，第一个位置O₁与第二个位置O₂之间的距离为l_1，2，通过坐标系映射矩阵得出运载体局部运动矢量

与东北天地理坐标系的三个轴的夹角为α₂′、β₂′、γ₂′，则此时运载体局部运动矢量

和第1步中全局运动矢量

之间的夹角余弦为： 

由式(3)、l₁和l_1，2，根据三角形几何运算得： 

在XOY面上投影

的长度为： 

l_1XY=l₁sinγ₁    (5) 

在XOY面上投影

的长度为： 

l_1,2XY=γ_1,2sinγ₂′ (6) 

在XOY面上的投影

和

在XOY面上的投影之间的夹角余弦为： 

由式(7)、l_1XY以及l_1,2XY，根据三角运算得出在XOY面上投影矢量

和

之和

的长度为： 

故得全局运动矢量

与地理坐标系Z轴的夹角为： 

将

分别在YOZ面和XOZ面上投影，并通过式(5)~式(9)进行相同方法的计算，分别获得全局运动矢量

与地理坐标系X轴和Y轴的夹角为α₂、β₂；则初始位置O与第2个位置O₂的全局运动矢量为： 

第n步：运载体从第n-1个位置O_n-1运动到第n个位置O_n，O_n-1与O_n之间的距离为l_n-1，n，通过坐标系映射矩阵得出运载体局部运动矢量

与地理坐标系的三轴的三个夹角分别为α_n′、β_n′、γ_n′，此时运载体局部运动矢量

和第n-1步中全局运动矢量

之间的夹角余弦为： 

由式(11)、l_n和l_n-1，n，根据三角形几何运算得： 

在XOY面投影

的长度为： 

l_n-1XY=l_n-1sinγ_n-1    (13) 

在XOY面投影的长度为： 

l_n-1，nXY=l_n-1，nsinγ_n′    (14) 

投影

和

之间的夹角余弦为： 

由式(15)、l_n-1XY以及l_n-1，nXY，根据三角运算得出XOY面上的投影矢量

和之和

的长度为： 

故得全局运动矢量

与地理坐标系Z轴的之间的夹角为： 

将分别在YOZ面和XOZ面上投影，并通过式(13)~式(17)计算，分别获得运载体全局运动矢量与地理坐标系X轴和Y轴的夹角为α_n、β_n；则初始位置O与第n个位置O_n的全局运动矢量为 

当前运载体局部运动矢量与地理坐标系三轴之间的夹角α_i′、β_i′、γ_i′按如下过程进行： 

运载体运动到第i个位置O_i时，运载体坐标系为O_iX_iY_iZ_i，根据相对运动原理，寻找映射关系C_i使得运载体坐标系O_iX_iY_iZ_i与导航所需的地理坐标系OXYZ的三个坐标轴方向一致；假设此时运载体坐标系为0系，先绕自身O_iX_i轴正向顺时针旋转α_i′角得到a系，而后绕自身O_iY_i轴正向顺时针旋转β_i′角得到b系，最后绕自身O_iZ_i轴正向顺时针旋转γ_i′角得到c系，此时O_iX_iY_iZ_i与OXYZ轴向重合； 

坐标系O_iX_iY_iZ_i绕自身O_iX_i轴正向旋转α_i′角后的旋转矩阵为： 

坐标系O_iX_iY_iZ_i绕自身O_iY_i轴正向旋转β_i′角后的旋转矩阵为： 

坐标系O_iX_iY_iZ_i绕自身O_iZ_i轴正向旋转γ_i′角后的旋转矩阵为： 

则O_iX_iY_iZ_i系与绕自身O_iX_i轴正向旋转α_i′角、绕自身O_iY_i轴正向旋转β_i′角、再绕自身O_iZ_i轴正向旋转γ_i′角后得到的坐标系之间的变换矩阵为： 

通过求解变换矩阵C_i得出运载体当前局部运动矢量与地理坐标系OXYZ三个坐标轴之间的夹角为α_i′、β_i′、γ_i′。 

Images