用于月球及深空巡视器导航控制性能的地面室内验证方法

摘要

用于月球及深空巡视器导航控制性能的地面室内验证方法，通过坐标转换得到真实的位置、速度和姿态信息，将真实的位置、速度和姿态信息与巡视器导航计算得到的位置、速度和姿态信息做差，得到验证导航控制性能的参数，从而实现在地面验证导航控制性能。本发明提出的验证方案具有组成简便完备、验证项目全面、实时性好、精度高、跟踪能力强、试验连续性和自主性好、试验过程操作简便、试验设备性价比高等优点，能够同时验证姿态、速度、位置等导航信息的精度以及航向角控制精度；本发明所用的iGPS系统可实时高精度测量巡视器的真实位置、姿态，为评价导航控制性能提供基准，iGPS适用于室内测量，对试验场地无特殊要求，可增强对巡视器的试验验证能力；本发明所采用的公共标识能同时为iGPS和经纬仪提供测量目标，克服了iGPS测量和经纬仪测量之间转换的困难。

说明书

技术领域

本发明涉及一种适用于月球及深空巡视器的导航控制性能地面室内验证方 法，属于导航控制性能地面验证技术领域。

背景技术

月面巡视探测器是工作在月球表面的一类航天器，其导航与控制的功能与 性能指标与典型的星、船有很大区别，是实现巡视探测所必须掌握的关键技术， 需要在地面进行充分验证。

导航控制是巡视器的重要功能，其性能指标反映了巡视器的探测能力。由 于月球探测是我国航天领域全新的工程，还没有有效的手段在地面对巡视器的 导航控制性能进行有效验证。现有卫星和飞船的物理仿真系统是将轨控和姿控 分开进行验证，月面巡视探测器的位置、速度和姿态密切相关，因此现有卫星 物理仿真系统难以实现对巡视探测器的验证。

目前国内外的巡视器地面试验大多集中在对移动性能的验证和评估方面， 如低重力模拟、构建多种月表地形、土槽试验等，对导航控制性能的验证尚未 见详细报导。

前苏联在发射第一辆月面巡视探测器Lunokhod1前，进行了大量地面试 验。研究了多种模拟月壤，并建立土槽用于测试移动系统的相关性能；为了准 确获得1/6重力环境，苏联方面将相关试验台安装在飞机上，利用飞机下降过 程造成1/6重力环境，对简化模型进行相关试验以获得相关试验数据。

国内哈尔滨工业大学、201所、吉林大学等也都针对月球巡视器开展了土 槽试验，对巡视器的动力学特性和移动性能进行了试验分析和验证。

中国空间技术研究院于2006年建立了月面巡视器室内试验场，通过模拟 月壤和灯光，建立了一个室内模拟月面环境，对研制的巡视器原理样机进行了 地面试验。但由于没有准确的位置、速度，姿态测量设备，无法获得巡视器真 实的位置、速度和姿态信息，无法对导航控制性能进行有效验证。

上述试验的重点都是巡视器移动性能的验证，试验时巡视器的移动和导航 主要靠遥控或地面导航设备进行，巡视器没有自主导航性能，或自主导航性能 没有得到有效验证。

月球或火星巡视器导航控制性能的充分验证非常必要，但目前尚没有系统 的试验验证方案和手段，来动态连续获取巡视器的位置、速度和姿态信息真实 值，无法为巡视器提供导航控制性能评价的参考标准。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种月球或深空巡 视器导航控制性能地面验证方法。

本发明的技术解决方案是：用于月球及深空巡视器导航控制性能的地面室 内验证方法，通过以下步骤实现：

第一步，构建导航控制性能地面验证系统，

导航控制性能地面验证系统包括iGPS系统、真北标识设备、3台经纬仪和 若干公共标识，iGPS系统包括四台激光发射器和四个接收器，真北标识设备由 安装在地基台上的立方镜组成，公共标识为安装接收器的同心圆环；

第二步，建立北-东-地坐标系F_n和真北指示立方镜坐标系F_d，得到F_n和 F_d的转换矩阵

第三步，在室内试验场地四端固定安装四台激光发射器，建立iGPS系统 测量坐标系F_r；

第四步，建立真北指示立方镜坐标系F_d与iGPS系统测量坐标系F_r的转换 矩阵

A4.1、在室内试验场地设置4个公共标识作为控制点，利用三台经纬仪联 机互瞄得到控制点在真北指示立方镜坐标系F_d下的坐标[x_di0 y_di0 z_di0]^T， i＝1，2，3，4，即

$C_1={\left(\begin{array}{ccc}{x}_{\mathrm{di}0}& y_{\mathrm{di}0}& z_{\mathrm{di}0}\end{array}\right)}^T=\left(\begin{array}{cccc}{x}_{d10}& x_{d20}& x_{d30}& x_{d40}\\ y_{d10}& y_{d20}& y_{d30}& y_{d40}\\ z_{d10}& z_{d20}& z_{d30}& z_{d40}\end{array}\right)---\left(1\right);$

A4.2、将四个接收器安装在4个公共标识上，利用iGPS系统得到控制点 在iGPS系统测量坐标系F_r下的坐标[x_ri0 y_ri0 z_ri0]^T，i＝1，2，3，4，即

$C_2={\left(\begin{array}{ccc}{x}_{\mathrm{ri}0}& y_{\mathrm{ri}0}& z_{\mathrm{ri}0}\end{array}\right)}^T=\left(\begin{array}{cccc}{x}_{r10}& x_{r20}& x_{r30}& x_{r40}\\ y_{r10}& y_{r20}& y_{r30}& y_{r40}\\ z_{r10}& z_{r20}& z_{r30}& z_{r40}\end{array}\right)---\left(2\right);$

A4.3、利用公式(1)和(2)通过公式(3)得到真北指示立方镜坐标系 F_d与iGPS系统测量坐标系F_r的转换矩阵

$C_d^r=C_2{C_1}^T{\left(C_1{C_1}^T\right)}^{-1}---\left(3\right);$

第五步，利用公式(4)得到北-东-地坐标系F_n与iGPS系统测量坐标系 F_r的转换矩阵

$C_n^r=C_n^d{C}_d^r---\left(4\right);$

第六步，在巡视器上安装立方镜，建立巡视器本体坐标系F_b；

第七步，将4个公共标识安装在巡视器上表面，在4个公共标识安装四个 接收器；

第八步，利用三台经纬仪联机互瞄得到第七步中安装在巡视器上表面的四 个接收器在巡视器本体坐标系Fb中的坐标C₃，

$C_3={\left(\begin{array}{ccc}{x}_{\mathrm{bi}}& y_{\mathrm{bi}}& z_{\mathrm{bi}}\end{array}\right)}^T=\left(\begin{array}{cccc}{x}_{b1}& x_{b2}& x_{b3}& x_{b4}\\ y_{b1}& y_{b2}& y_{b3}& y_{b4}\\ z_{b1}& z_{b2}& z_{b3}& z_{b4}\end{array}\right),i=\mathrm{1,2,3,4}---\left(5\right);$

第九步，实时获得iGPS测量坐标系F_r与巡视器本体坐标系F_b的转换矩阵 $C_r^b,$

A9.1、安装了四个接收器的巡视器在室内试验场地运动，利用iGPS系统 获得四个接收器在iGPS测量坐标系Fr中的坐标C₄，

$C_4={\left(\begin{array}{ccc}{x}_{\mathrm{ri}}& y_{\mathrm{ri}}& z_{\mathrm{ri}}\end{array}\right)}^T=\left(\begin{array}{cccc}{x}_{r1}& x_{r2}& x_{r3}& x_{r4}\\ y_{r1}& y_{r2}& y_{r3}& y_{r4}\\ z_{r1}& z_{r2}& z_{r3}& z_{r4}\end{array}\right),i=\mathrm{1,2,3,4}---\left(6\right);$

A9.2、利用第八步得到的安装在巡视器上表面的四个接收器在巡视器本体 坐标系Fb中的坐标C₃和步骤A9.1得到的四个接收器在iGPS测量坐标系Fr 中的坐标C₄，根据公式(7)得到iGPS测量坐标系Fr与巡视器本体坐标系Fb 的转换矩阵

$C_r^b=C_3{C_4}^T{\left(C_4{C_4}^T\right)}^{-1}---\left(7\right);$

第十步，实时获得巡视器本体坐标系Fb在北-东-地坐标系Fn的位置、姿 态与速度信息，

A10.1、利用公式(8)得到巡视器本体坐标系Fb在iGPS测量坐标系Fr 中的实时位置信息P_r，

$P_r=\frac{1}{k}\left(\begin{array}{c}\underset{i=1}{\overset{k}{\Sigma }}(x_{\mathrm{ri}}-x_{\mathrm{bi}})\\ \underset{i=1}{\overset{k}{\Sigma }}(y_{\mathrm{ri}}-y_{\mathrm{bi}})\\ \underset{i=1}{\overset{k}{\Sigma }}(z_{\mathrm{ri}}-z_{\mathrm{bi}})\end{array}\right)---\left(8\right)$

其中k为接收到激光发射器有效信号的接收器个数，且k≥3；

A10.2、利用公式(9)得到巡视器本体坐标系Fb在北-东-地坐标系Fn中 的实时位置信息P_n，

$P_n={\left(\begin{array}{ccc}{x}_n& y_n& z_n\end{array}\right)}^T=C_n^{\mathrm{rT}}{P}_r---\left(9\right);$

A10.3、利用公式(10)得到巡视器本体坐标系Fb相对iGPS测量坐标系 Fr的姿态信息

其中r、p和ψ为巡视器滚动、俯仰和航向姿态角， -90°＜r_r＜90°，-90°＜p_r＜90°，-180°＜ψ_r≤180°，表示矩阵中的第i′行第j′ 列的元素；

A10.4、利用公式(11)得到巡视器本体坐标系Fb相对北-东-地坐标系Fn 中的实时姿态信息

A10.5利用公式(12)获得巡视器本体坐标系Fb相对北-东-地坐标系Fn 的速度信息V_n，

$V_n={\left(\begin{array}{ccc}{v}_{\mathrm{nx}}& v_{\mathrm{ny}}& v_{\mathrm{nz}}\end{array}\right)}^T=\frac{\left(\begin{array}{c}{x}_n-x_{\mathrm{nlast}}\\ y_n-y_{\mathrm{nlast}}\\ z_n-z_{\mathrm{nlast}}\end{array}\right)}{\mathrm{\Delta T}}---\left(12\right)$

其中，x_n、y_n和z_n为步骤A10.2得到的本次位置获取时刻的巡视器本体坐 标系Fb在北-东-地坐标系Fn中的位置信息，x_nlast、y_nlast和z_nlast为上一位置获取 时刻得到的实时位置信息，ΔT是本次位置确定时刻距上次位置获取时刻的时 间差；

第十一步，导航控制性能验证，

A11.1、巡视器自主导航运算实时得到位置P_m、姿态和速度V_m；

A11.2、根据步骤A11.1得到的位置P_m、姿态和速度V_m和第十步的得到 的位置P_n、姿态和速度V_n，利用公式组(13)得到导航性能评价参数ΔP(t)、 ΔV(t)、和ψ_c(t)，

ΔP(t)＝|P_n(t)-P_m(t)|

ΔV(t)＝|V_n(t)-V_m(t)|

Δψ(t)＝|ψ_n(t)-ψ_c(t)|

其中，t为时间，且t＝0～T，T为试验时间，ψ_c(t)是t时刻的期望航向角度， ψ_n(t)是t时刻的巡视器本体坐标系Fb相对北-东-地坐标系Fn中的航向角度；

A11.3、对步骤A11.2得到导航性能评价参数ΔP(t)、ΔV(t)、和ψ_c(t)在 试验时间T内的均值和均方差，利用均值和均方差来评价巡视器导航控制的精 度和稳定度和巡视器航向角控制精度和稳定度。

所述第七步中接收器之间的间距不小于0.5m。

本发明设计要点：

1)利用iGPS系统，对巡视器的位置、姿态进行高精度测量，并转换得到 速度；

2)利用公共标识同时为iGPS系统和经纬仪提供测量目标，以采用公共点 转换方法获得坐标系间的转换关系；

3)利用坐标系间转换关系将经纬仪、真北测量和iGPS系统测量结果统一 到北-东-地坐标系下。

本发明与现有技术相比有益效果为：

(1)本发明提出的验证方案具有组成简便完备、验证项目全面、实时性好、 精度高、跟踪能力强、试验连续性和自主性好、试验过程操作简便、试验设备 性价比高等优点，能够同时验证姿态、速度、位置等导航信息的精度以及航向 角控制精度；

(2)本发明所用的iGPS系统可实时高精度测量巡视器的真实位置、姿态， 为评价导航控制性能提供基准，iGPS适用于室内测量，对试验场地无特殊要求， 可增强对巡视器的试验验证能力；

(3)本发明所采用的公共标识能同时为iGPS和经纬仪提供测量目标，克 服了iGPS测量和经纬仪测量之间转换的困难；

(4)本发明所提出的方法与试验对象(巡视器)的自身配备独立，不影响 巡视器的运动灵活性，适用性好。

附图说明

图1为本发明真北指示立方镜坐标系及北-东-地坐标系示意图；

图2为本发明iGPS系统测量坐标系示意图；

图3为本发明iGPS系统组成图；

图4为本发明流程图。

具体实施方式

北-东-地坐标系F_n定义：原点与真北指示立方镜原点重合，X_n轴指向 真北方向，Z_n轴垂直指向地心，Y_n轴按照右手定则定义。北-东-地坐标系是 巡视器的导航坐标系，是评价巡视器导航控制性能的基准。

巡视器本体坐标系F_b定义：原点位于巡视器本体中心，X_b轴指向巡视 器前方，Z_b轴指向地心，Y_b轴按右手定则定义。F_b用安装在巡视器上的立 方镜表示。

真北指示立方镜坐标系F_d定义：原点位于真北指示立方镜基准面(基 准面可视标定方便确定，一般选在立方镜侧面)中心，Z_d轴平行于基准面指 向地，X_d轴和Y_d轴一个垂直于基准面指向外，另一个平行于基准面垂直于 Z_d轴，X_d、Y_d、Z_d轴满足右手定则。真北指示立方镜坐标系的定义如图1 所示。

Indoor GPS系统测量坐标系F_r定义：Indoor GPS(简称iGPS)技术 是20世纪90年代美国Arcsecond公司在GPS技术原理启发下开发的用于 室内精密定位的测量系统。该系统使用红外脉冲激光发射器代替卫星的作 用，接收器根据发射器投射来的光线时间特征参数，计算接收器所在点的角 度和位置，并将模拟信号转换成数字脉冲信号，通过无线网络发送给中央控 制室的服务器，最后通过Indoor GPS自行开发的软件或第三方测量软件(如 SA、Metrolog II、MAYA等)处理数据获得高精度的信息，并供远端的多用 户共享。测量精度取决于发射器的数量、接受器的数量和工作空间的大小， 发射器位置的布置及使用不同种类的接收器会产生不同的测量精度。本方法 采用四个发射器和四个接收器。测量坐标系F_r以接收器Q₁为原点，Q₂-Q₁-S₄平面为参考平面建立测量坐标系，X_r轴指向Q₁一Q₂连线方向，Z_r轴指向垂直 参考平面方向向上，Y_r轴按照右手定则定义。如图2所示。

取：

$C_n^b=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ c_{41}& c_{42}& c_{43}\end{array}\right)$为由F_n到F_b的转换矩阵；

$C_n^d=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ c_{41}& c_{42}& c_{43}\end{array}\right)$为由F_n到F_d的转换矩阵；

$C_d^r=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ c_{41}& c_{42}& c_{43}\end{array}\right)$为由F_d到F_r的转换矩阵；

$C_r^b=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ c_{41}& c_{42}& c_{43}\end{array}\right)$为由F_r到F_b的转换矩阵；

$C^n=\left(\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ c_{41}& c_{42}& c_{43}\end{array}\right)$为由F_n到F_r的转换矩阵；

且有$C_n^b=C_n^r{C}_r^b=C_n^d{C}_d^r{C}_r^b.$

本发明可用于获得巡视器本体坐标系相对北-东-地坐标系的姿态、位置 和速度，包括：

姿态信息：r，p，ψ分别为滚动、俯仰和航向姿态；

位置信息：x_n，y_n，z_n分别为巡视器在F_n坐标系中表示的位置信息；

速度信息：v_nx，v_ny，v_nz分别为巡视器在F_n坐标系中表示的运动速度信息。

本发明如图4所示，通过以下步骤实现：

1、构建试验验证系统组成如下：

(1)iGPS系统

iGPS系统的测量精度取决于发射器的数量、接收器的数量和工作空间的大 小，发射器位置的布置及使用不同种类的接收器会产生不同的测量精度。iGPS 系统的发射器有效信号覆盖面积是50m×50m，只要安装足够多数量的发射器， 系统的工作区域不受限制。本发明如图3所示，采用四台激光发射器和四个接 收器组成iGPS测量系统，接收器的安装位置间距不小于0.5m，位置测量精度 可达0.1mm，姿态测量精度可达到0.01°，测量范围大于50m×50m。

(2)真北标识设备

在室内试验场地内建立稳定地基台，地基台上固封安放立方镜，立方镜的 尺寸不小于50mm×50mm×50mm。以真北标识立方镜前端面十字刻线交点为 原点，建立立方镜坐标系。寻北完成后，用环氧结构胶固定立方镜。

(3)经纬仪

本发明采用3台高精度电子经纬仪，用于辅助获得坐标系Fn与Fd、Fb 与Fr之间的转换关系。

(4)公共标识

本发明特别设计加工的、经纬仪测量系统和IGPS测量系统都能作为目标 使用的公共标识。这种公共标识既可作为IGPS系统中的接收器基座，也可作 为经纬仪系统的测量目标。公共标识是一个同心圆环，其中圆环的内径是一个 直径约为10mm的空心圆。iGPS系统测量时，将接收器插入圆环中，系统所 测位置就是基座中心空心圆在其上表面的圆心。对于经纬仪测量系统，可以直 接测量基座圆心的位置，从而保证两种测量系统测量的是同一目标。本发明采 用此类公共标识4～6个。

2、建立北-东-地坐标系F_n与真北指示立方镜坐标系F_d的转换关系

由专业测绘人员通过天文测量原理，如“北极星任意时角法”测得天文方 位角，解算获得室外天文方位基准，详细测量步骤可咨询专业测绘机构。获得 天文方位基准后，采用“经纬仪对瞄联测法”建立真北指示立方镜坐标系与北- 东-地坐标系的转换关系

3、建立iGPS测量坐标系F_r

如图3所示，4台激光发射器Q₁、Q₂、Q₃、Q₄固定在试验场地四端，4 台接收器P₁、P₂、P₃、P₄通过公共标识固定安放在巡视器的上表面，保证两个 接收器之间的间距不小于0.5m。

通过iGPS系统标定，建立如图2所示的坐标系F_r。详细坐标系建立过程 可参见iGPS系统用户手册。

4、建立真北指示立方镜坐标系F_d与iGPS系统测量坐标系F_r的转换关系 $C_d^r.$

在地面试验场布设4个公共标识D₁、D₂、D₃和D₄，如图3所示。利用三 台经纬仪联机互瞄获得这4个控制点在真北指示立方镜坐标系下的坐标，分别 表示为[x_di0 y_di0 z_di0]^T，i＝1，2，3，4，写成矩阵形式为：

$C_1=\left(\begin{array}{cccc}{x}_{d10}& x_{d20}& x_{d30}& x_{d40}\\ y_{d10}& y_{d20}& y_{d30}& y_{d40}\\ z_{d10}& z_{d20}& z_{d30}& z_{d40}\end{array}\right)---\left(1\right)$

在4个控制点处固定放置接收器，利用iGPS系统得到4个控制点在iGPS 测量坐标系下的坐标[x_ri0 y_ri0 z_ri0]^T，i＝1，2，3，4，写成矩阵形式为：

$C_2=\left(\begin{array}{cccc}{x}_{r10}& x_{r20}& x_{r30}& x_{r40}\\ y_{r1}& y_{r2}& y_{r3}& y_{r40}\\ z_{r10}& z_{r20}& z_{r30}& z_{r40}\end{array}\right)---\left(2\right)$

通过公共点(即4个控制点)转换，由(3)式求解真北指示立方镜坐标 系到iGPS测量坐标系的转换关系

$C_d^r=C_2{C_1}^T{\left(C_1{C_1}^T\right)}^{-1}---\left(3\right)$

5、建立北-东-地坐标系F_n与iGPS测量坐标系F_r的转换关系

北-东-地坐标系F_n到iGPS系统测量坐标系F_r的转换矩阵为由(4) 式获得

$C_n^r=C_n^d{C}_d^r---\left(4\right)$

6、获得iGPS系统接收器在巡视器本体坐标系F_b中的坐标。

巡视器本体坐标系表示在安装在巡视器上的立方镜上。由于接收器P₁、P₂、 P₃、P₄安装在巡视器的公共标识基座上，因此可同时作为经纬仪的测量目标。 利用三台经纬仪联机互瞄可获得P₁、P₂、P₃和P₄在巡视器本体坐标系中的坐 标，分别表示为[x_bi y_bi z_bi]^T，i＝1，2，3，4，写成矩阵形式为：

$C_3=\left(\begin{array}{cccc}{x}_{b1}& x_{b2}& x_{b3}& x_{b4}\\ y_{b1}& y_{b2}& y_{b3}& y_{b4}\\ z_{b1}& z_{b2}& z_{b3}& z_{b4}\end{array}\right)---\left(5\right)$

以上步骤均为验证试验前静态操作，所得结果确定后不随巡视器运动而变 化。如iGPS系统发射器位置发生变化时，须重新进行上述步骤确定转化矩阵。

7、实时获得iGPS测量坐标系F_r与巡视器本体坐标系F_b的转换关系

巡视器运动时，固定安装在巡视器上的4个接收器P₁、P₂、P₃和P₄作为 iGPS系统的跟踪控制点，iGPS系统每秒同步采集40帧4个跟踪控制点(4 个接收器所在位置)的信息，经第三方软件或自带软件解算后，获得跟踪控制 点在F_r坐标系中的坐标，表示为[x_ri y_ri z_ri]^T，i＝1，2，3，4，写成矩阵形式为：

$C_4=\left(\begin{array}{cccc}{x}_{r1}& x_{r2}& x_{r3}& x_{r4}\\ y_{r1}& y_{r2}& y_{r3}& y_{r4}\\ z_{r1}& z_{r2}& z_{r3}& z_{r4}\end{array}\right)---\left(6\right)$

由式(7)获得iGPS测量坐标系F_r与本体坐标系F_b的转换关系

$C_r^b=C_3{C_4}^T{\left(C_4{C_4}^T\right)}^{-1}---\left(7\right)$

8、实时获得巡视器的位置、姿态与速度。

(1)位置获取：

由式(8)获得巡视器本体坐标系F_b在F_r中的位置P_r。

$P_r=\left(\begin{array}{c}{p}_{\mathrm{rx}}\\ p_{\mathrm{ry}}\\ p_{\mathrm{rz}}\end{array}\right)=\frac{1}{k}\left(\begin{array}{c}\underset{i=1}{\overset{k}{\Sigma }}(x_{\mathrm{ri}}-x_{\mathrm{bi}})\\ \underset{i=1}{\overset{k}{\Sigma }}(y_{\mathrm{ri}}-y_{\mathrm{bi}})\\ \underset{i=1}{\overset{k}{\Sigma }}(z_{\mathrm{ri}}-z_{\mathrm{bi}})\end{array}\right)---\left(8\right)$

其中k为有效的接收器个数，且保证k≥3。

由式(9)转换到北-东-地坐标系F_n下，获得本体坐标系F_b在F_n中的位置 P_n。

$P_n={\left(\begin{array}{ccc}{x}_n& y_n& z_n\end{array}\right)}^T=C_n^{\mathrm{rT}}{P}_r---\left(9\right)$

(2)姿态获取：

由式(10)获得F_b相对F_r的姿态

其中-90°＜r_r＜90°，-90°＜p_r＜90°，-180°＜ψ_r≤180°，因此式(10)不会分母为 零的奇异情况。

公式(10)适用于3-2-1转序的欧拉角表示方法，即第一次Fr绕Zr轴转 动ψ，第二次绕转动后的Yr轴转动p，最后绕转动后的Xr轴转动r，每次的 转动角r、p、ψ称为欧拉角。

对3-1-2转序的欧拉角表示方法，式(10)修改为：

对于其他转序欧拉角表示方法，可按欧拉定理中用姿态角表示的姿态矩阵 形式，计算相应的欧拉角。

由式(11)获得F_b相对F_n的姿态表示

(3)速度获取：

由式(12)获得本体坐标系F_b相对F_n的速度V_n。

$V_n={\left(\begin{array}{ccc}{v}_{\mathrm{nx}}& v_{\mathrm{ny}}& v_{\mathrm{nz}}\end{array}\right)}^T=\left(\begin{array}{c}{x}_n-x_{\mathrm{nlast}}\\ y_n-y_{\mathrm{nlast}}\\ z_n-z_{\mathrm{nlast}}\end{array}\right)/\mathrm{\Delta T}---\left(12\right)$

其中x_nlast、y_nlast、z_nlast分别是上一位置获取时刻得到的位置信息，ΔT是本 次位置确定时刻距上次位置获取时刻的时间差。

9、验证导航控制性能

巡视器自主导航运算实时得到位置P_m、姿态和速度V_m，分别按式(13) 进行处理。

ΔP(t)＝|P_n(t)-P_m(t)|

ΔV(t)＝|V_n(t)-V_m(t)|

Δψ(t)＝|ψ_n(t)-ψ_c(t)|

其中，t为时间，且t＝0～T，T为试验时间。

对试验时间T内的ΔP(t)、ΔV(t)和按统计方法分别求取均值和均方差， 可用于评价巡视器导航控制的精度和稳定度。其中ΔP(t)的均值和均方差可用于 评价巡视器自主导航定位的精度和稳定度，ΔV(t)的均值和均方差可用于评价巡 视器自主速度确定的精度和稳定度，自主姿态确定的精度和稳定度；

式(13)中ψ_c(t)是t时刻的期望航向角度(试验时的已知值)，ψ_n(t)是t时 刻的巡视器本体坐标系Fb相对北-东-地坐标系Fn中的航向角度；对试验时间 T内的Δψ(t)按统计方法求取均值和均方差，可用于评价巡视器航向角控制精度 和稳定度。

本发明未详细说明部分属本领域技术人员公知常识。