CN104101881B - 基于激光测距和mems/gps的目标导航测绘误差角估计方法 - Google Patents

Abstract

本发明公开了一种基于激光测距和MEMS/GPS组合导航***的目标导航测绘误差角估计方法。根据MEMS/GPS组合导航***测出观测点的位置、姿态，通过激光测距仪测出观测点距目标的斜距；基于测量值计算出参考点的三维坐标、相对位置误差，进而构造参考点的观测方程。获取2个外部参考点的位置姿态信息，即估计出姿态误差角和安装失准角。该方法可充分利用多次观测信息，有效解决传统方法中误差不可观测的问题，利用外部参考点信息对安装失准角及姿态误差角进行精确估计与补偿，有效提升目标的导航测绘精度。

Description

基于激光测距和MEMS/GPS的目标导航测绘误差角估计方法

技术领域

本发明涉及目标导航定位领域，特别是基于激光测距和MEMS/GPS实现目标导航测绘误差角估计的方法。

技术背景

当前，大量应用领域需要进行空间目标三维坐标测量，如三维地形测绘、地形变化监测、建筑物变形监测、大坝变形监测和动态目标跟踪等。惯性导航、卫星导航、天文导航、地文导航、无线电导航等传统导航手段都需要将导航传感器安装至目标载体上，才能输出目标的三维坐标测量。对于一些特殊领域的导航、测绘和制导应用，很难甚至无法将导航传感器直接安装在目标载体进行导航定位，如建筑物变形监测、火山监测等；其次，有些应用需要同时多套测量设备，这显然会大大增加测绘成本和设备安装难度。基于三维激光扫描的测绘方法，能够快速测量实体表面点的三维坐标，建立目标的三维模型，但却只能获取目标点相对仪器内部坐标***的三维坐标。

基于激光测距仪(LaserDistanceSensor,LDS)能实现目标点的三维绝对位置的精确测量。不仅克服了导航传感器必须安装在目标载体上的缺点，而且能实现动态目标的跟踪(相比于全站仪等仪器)。基于微机电机械***(Micro-electromechanicalSystems,MEMS)与全球定位***(GlobalPositioningSystem,GPS)组合导航定位***(即MEMS/GPS组合导航***)能测量载体的线运动和角运动信息。但该方法的前提是激光测距仪与MEMS/GPS组合导航***必须实现精确的安装标校，否则会由于安装标校误差激励目标点位置误差，而且在没有完成精确校准的情况下，对MEMS/GPS姿态误差的校准精度也将受到影响。受机械加工和装备精度影响，三维激光扫描仪和MEMS/GPS组合导航都包含了诸多的非正交坐标系。而为了提高测量精度，需要通过标定的方式将非正交坐标系转化为正交坐标系。目前还没有合适的方法能够精确完成激光测距仪与MEMS/GPS组合导航***的标校，只能单纯依赖机械加工和装配的方式完成。这不仅大大增加了对机械加工精度的要求，而且难度大、成本投入高。

本发明基于激光测距仪和MEMS/GPS组合导航***，在目标点定位导航算法的基础上，设计一种合理的误差角估计算法，能够完成LDS与MEMS/GPS间的安装失准角，以及MEMS/GPS姿态误差角的估计，有效提高目标导航的定位精度。

发明内容

本发明的目的在于提出一种安装失准角及姿态误差角估计方法，特别是基于激光测距和MEMS/GPS实现目标导航测绘的安装失准角及姿态误差角估计方法。

为实现上述目的，本发明采用的技术方案包括下列步骤：

(1)在观测点o附近选取一参考点R₁，利用MEMS/GPS组合导航***精确测量参考点R₁的三维位置坐标

(2)将MEMS/GPS组合导航***移至观测点o，将激光测距仪瞄准参考点R₁，利用MEMS/GPS组合导航***测量观测点o三维位置坐标经纬度矢量oR₁相对地理坐标系ox_ty_tz_t的航向角和纵摇角利用激光测距仪测量矢量oR₁的斜距d_oR1。

(3)根据步骤(2)中MEMS/GPS组合导航***和激光测距仪的测量值，计算参考点R₁在地球坐标系o_ex_ey_ez_e中的三维位置坐标

所涉及参考点R₁的三维位置坐标表达式为：

$\left[\begin{array}{c}{\tilde{x}}_{R1}^e\\ {\tilde{y}}_{R1}^e\\ {\tilde{z}}_{R1}^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\·C_t^2\·\left[\begin{array}{ccc}1& -\mathrm{\γ}& \mathrm{\β}\\ \mathrm{\γ}& 1& -\mathrm{\α}\\ -\mathrm{\β}& \mathrm{\α}& 1\end{array}\right]\left[\begin{array}{c}0\\ d_{\mathrm{oR}1}\\ 0\end{array}\right]---\left(1\right)$

式中，α、β、γ为MEMS/GPS和LDS间的安装失准角；为地理坐标系与地球坐标系间的转换矩阵，为两次空间旋转变换矩阵：

所涉及的地理坐标系与地球坐标系间的转换矩阵表达式为：

所涉及的两次空间旋转变换矩阵表达式为：

$C_t^2=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \widetilde{\mathrm{\φ}}& \sin \widetilde{\mathrm{\φ}}\\ 0& -\sin \widetilde{\mathrm{\φ}}& \cos \widetilde{\mathrm{\φ}}\end{array}\right]\·\left[\begin{array}{ccc}\cos \tilde{H}& \sin \tilde{H}& 0\\ -\sin \tilde{H}& \cos \tilde{H}& 0\\ 0& 0& 1\end{array}\right].$

(4)根据参考点R₁的真实位置及步骤(3)中计算得出参考点R₁的三维位置坐标，再通过计算得到相对位置误差并构建基于姿态误差和安装失准角的相对位置误差表示。

所涉及的参考点R₁的相对位置误差表达式为：

$\left[\begin{array}{c}{\mathrm{\Δx}}_{R1}^e\\ {\mathrm{\Δy}}_{R1}^e\\ {\mathrm{\Δz}}_{R1}^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{R1}^e-x_{R1}^e\\ {\tilde{y}}_{R1}^e-y_{R1}^e\\ {\tilde{z}}_{R1}^e-z_{R1}^e\end{array}\right]---\left(2\right)$

构建基于姿态误差角和安装失准角的相对位置误差表达式为：

$\left[\begin{array}{c}{\mathrm{\Δx}}_{R1}^e\\ {\mathrm{\Δy}}_{R1}^e\\ {\mathrm{\Δz}}_{R1}^e\end{array}\right]=C_t^e\·C_1\·\left[\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\ΔH}-\mathrm{\γ}\\ \mathrm{\α}\end{array}\right]---\left(3\right)$

式中， $\mathrm{\Δ\φ}=\widetilde{\mathrm{\φ}}-\mathrm{\φ},$ $\mathrm{\ΔH}=\tilde{H}-H,$

$C_1=\left[\begin{array}{ccc}0& d_{\mathrm{oR}}\cos \tilde{H}& 0\\ -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}& -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\\ -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\end{array}\right];$

所涉及的参考点R₁的真实位置表达式为：

$\left[\begin{array}{c}{x}_{R1}^e\\ y_{R1}^e\\ z_{R1}^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\·\left[\begin{array}{c}{d}_{\mathrm{oR}1}\sin H\\ d_{\mathrm{oR}1}\cos \mathrm{\φ}\cos H\\ -d_{\mathrm{oR}1}\sin \mathrm{\φ}\cos H\end{array}\right]---\left(4\right)$

式中，H、φ为观测点o与参考点R₁构成矢量oR₁相对地理坐标系ox_ty_tz_t的真实航向角和纵摇角。

(5)根据式(3)中基于姿态误差角和安装失准角的相对位置误差表达式，构建基于参考点R₁的观测方程。

所涉及的参考点R₁的观测方程表达式为：

$Z_1=C_1\·\left[\begin{array}{c}\mathrm{\Δ\φ}\\ {\mathrm{\ΔH}}_{\mathrm{\γ}}\\ \mathrm{\α}\end{array}\right]---\left(5\right)$

式中，

(6)重复步骤(1)～(5)，基于外部参考点R₂，获取新的观测方程Z₂和观测矩阵C₂，并构建观测方程。

所涉及的参考点R₂的观测方程表达式为：

$Z_2=C_2\·\left[\begin{array}{c}\mathrm{\Δ\φ}\\ {\mathrm{\ΔH}}_{\mathrm{\γ}}\\ \mathrm{\α}\end{array}\right]---\left(6\right)$

(7)利用式(5)和式(6)中参考点R₁、R₂的观测方程，构成组合观测方程，利用最小二乘法，同时实现姿态误差角和安装失准角的估计。

所涉及的组合观测方程为：

姿态误差角和安装失准角的估计结果为：

采用本发明所提出方法的有益效果为：

(1)该方法可充分利用多次观测信息，有效解决传统方法中由安装失准角引起的误差不可观测的问题，利用外部参考点信息对安装失准角及姿态误差角进行精确估计。

(2)本发明设计的安装失准角及姿态误差角估计方法，降低标校设备的精度要求，降低成本，减轻维护费用。

(3)本方法对安装失准角和姿态误差角进行精确估计，并对其进行补偿，能有效提升了目标的导航定位精度。

附图说明

图1为本发明的实现流程示意图。

图2为目标点在地球系和地球系内的描述示意图。

图3为m系与b系空间旋转关系示意图。

图4a～图4c为MEMS/GPS组合导航***误差和LDS误差及安装误差角引起的目标定位误差仿真图；其中，图4a为MEMS/GPS组合导航***误差和LDS误差引起的目标定位误差仿真图；图4b为大小不同的安装误差角所引起的目标定位误差对比仿真图；图4c为MEMS/GPS组合导航***的误差和LDS的误差、及只考虑安装误差角引起的误差与考虑全部误差引起的目标定位误差对比仿真图。

图5a～图5c为MEMS/GPS组合导航***姿态误差为常值条件下，误差角估计算法对姿态误差和失准角的估计性能仿真图；其中，图5a为航向角误差的估计性能结果；图5b为纵摇角误差的估计性能结果；图5c为安装失准角的估计性能结果。

图6a～图6c为MEMS/GPS组合导航***姿态误差呈正弦变化时，姿态误差和失准角估计性能仿真图；其中，图6a为航向角误差的估计性能结果；图6b为纵摇角误差的估计性能结果；图6c为安装失准角的估计性能结果。

图7a～图7c校准前后目标定位误差对比仿真图；其中，图7a为α＝γ＝-0.2°，d_oR1＝300m，d_oR2＝280m，ΔH＝0.2°，Δφ＝0.1°，不考虑LDS误差时，校准前后目标定位误差的比较；图7b为α＝γ＝-0.2°，d_oR1＝300m，d_oR2＝280m，ΔH＝0.2°，Δφ＝0.1°时，校准前后目标定位性能的比较；图7c为α＝γ＝-0.1°，d_oR1＝300m，d_oR2＝140m，ΔH＝0.2°，Δφ＝0.1°时，校准前后目标定位性能的比较。

具体实施方式

1.目标导航定位方法

以MEMS/GPS组合导航***和LDS输出为基础，计算目标点三维坐标的基本思想是：首先根据空间矢量旋转的思想，将观测点与目标点构成矢量oT采用航向角H、纵摇角φ、o点与T点距离表示，计算地理坐标系内，坐标点相对观测点的三维坐标；再结合观测点自身的位置，将地理系内的坐标转换为地球系内的坐标系，计算目标点在地球坐标系内的三维绝对位置。T点在地球坐标系o_ex_ey_ez_e和地理坐标系ox_ty_tz_t的描述如图2所示。

基于MEMS/GPS组合导航***和LDS输出，计算目标点坐标的基本步骤：

(1)定义基准矢量oC

在地理坐标系内采用MEMS/GPS组合导航***的姿态信息描述矢量oT，沿地理坐标系oy_t轴定义一基准矢量oC，该矢量描述为：

oC＝0e₁+de₂+0e₃(9)

式中，e₁,e₂,e₃分别是沿地理系ox_t，oy_t，oz_t轴的单位矢量，d为观测点o与目标点T的距离。

(2)在地理系内描述矢量oT

由于LDS指向目标点T，因此o点与T点连线构成的矢量沿载体坐标系oy_b方向。该矢量由基准矢量oC通过两次空间旋转形成。量测空间旋转的变换矩阵为：

$C_t^2=C_1^2{C}_t^1=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\·\left[\begin{array}{ccc}\cos H& \sin H& 0\\ -\sin H& \cos H& 0\\ 0& 0& 1\end{array}\right]---\left(10\right)$

式中， 描述的是地理系与载体系的空间位置关系；H、φ分别表示航向角和纵摇角。

根据式(10)的定义，及矢量oT与oC的关系，得矢量oT在地理系的描述：

$\mathrm{oT}=C_1^2{C}_t^1\mathrm{oC}---\left(11\right)$

根据式(11)计算在地理系内，T点相对o点的三维坐标：

$\left[\begin{array}{c}{x}_{\mathrm{oT}}^t\\ y_{\mathrm{oT}}^t\\ z_{\mathrm{oT}}^t\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right]\·\left[\begin{array}{ccc}\cos H& \sin H& 0\\ -\sin H& \cos H& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ d\\ 0\end{array}\right]=\left[\begin{array}{c}d\sin H\\ d\cos \mathrm{\φ}\cos H\\ -d\sin \mathrm{\φ}\cos H\end{array}\right]---\left(12\right)$

(3)在地球系内描述矢量oT

在已知观测点o的地理纬度地理经度λ₀的情况下，将T点相对o点的三维坐标转化至地球坐标系下：

$\left[\begin{array}{c}{x}_{\mathrm{oT}}^e\\ y_{\mathrm{oT}}^e\\ z_{\mathrm{oT}}^e\end{array}\right]=C_t^e\left[\begin{array}{c}{x}_{\mathrm{oT}}^t\\ y_{\mathrm{oT}}^t\\ z_{\mathrm{oT}}^t\end{array}\right]---\left(13\right)$

式中，为地球系相对地理系的旋转矩阵。

如图2所示，由地球球心o_e、观测点o和目标点T构成的矢量，具有以下关系：

o_eT＝o_eo+oT(14)

由此求得目标点在地球系内的三维坐标为：

$\left[\begin{array}{c}{x}_T^e\\ y_T^e\\ z_T^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\left[\begin{array}{c}d\sin H\\ d\cos \mathrm{\φ}\cos H\\ -d\sin \mathrm{\φ}\cos H\end{array}\right]---\left(15\right)$

由于观测点的地理坐标 航向角H、纵摇角φ由MEMS/GPS组合导航***测量并输出，距离d由LDS测量并输出。因此基于LDS与MEMS/GPS组合导航***联合，可实现目标点的三维绝对位置定位，并根据位置计算高程差、倾斜角和水平角参数等。

2.考虑初始安装误差的目标点定位方法

由于LDS与MEMS/GPS组合导航***很难实现精确的共指向安装，LDS与MEMS/GPS所处的正交系必然存在微小的失准角，因此需要建立考虑初始安装误差的目标点定位表达式。假设MEMS/GPS组合导航***所处的坐标系指向ox_my_mz_m，LDS处于载体系ox_by_bz_b的oy_b轴上，两个坐标系间存在微小的失准角α、β、γ，如图3所示。

失准角α、β、γ构成了两个空间直角坐标系的坐标变换矩阵，对于微小失准角，两个坐标系间的空间变换矩阵表示为：

$C_m^b=\left[\begin{array}{ccc}1& \mathrm{\γ}& -\mathrm{\β}\\ -\mathrm{\γ}& 1& \mathrm{\α}\\ \mathrm{\β}& -\mathrm{\α}& 1\end{array}\right]---\left(16\right)$

定义沿oy_m方向，长度为d的空间矢量oC_m，oC_m与oC的关系表示为：

${\mathrm{oC}}_m=C_b^m\mathrm{oC}---\left(17\right)$

式中，考虑到方向余弦矩阵的正交性，有

根据式(17)可知，当MEMS/GPS组合导航***与LDS初始指向不同时，由MEMS/GPS组合导航***姿态描述的空间旋转，看成是由oC_m矢量为初始位置，进行旋转后的矢量，即为受指向误差后计算的目标位置。

则考虑安装误差在内的目标点定位计算方法为：

$\left[\begin{array}{c}{\tilde{x}}_T^e\\ {\tilde{y}}_T^e\\ {\tilde{z}}_T^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+{\tilde{C}}_t^e\·{\tilde{C}}_1^2\·{\tilde{C}}_t^1\left[\begin{array}{ccc}1& -\mathrm{\γ}& \mathrm{\β}\\ \mathrm{\γ}& 1& -\mathrm{\α}\\ -\mathrm{\β}& \mathrm{\α}& 1\end{array}\right]\·\left[\begin{array}{c}0\\ \tilde{d}\\ 0\end{array}\right]---\left(18\right)$

将公式(18)进行化简，得：

$\left[\begin{array}{c}{\tilde{x}}_T^e\\ {\tilde{y}}_T^e\\ {\tilde{z}}_T^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_o^e\\ {\tilde{y}}_o^e\\ {\tilde{z}}_o^e\end{array}\right]+{\tilde{C}}_t^e\·\left[\begin{array}{c}\tilde{d}\sin \tilde{H}-\mathrm{\γ}\tilde{d}\cos \tilde{H}\\ \tilde{d}\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}+\mathrm{\γ}\tilde{d}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}+\mathrm{\α}\tilde{d}\sin \widetilde{\mathrm{\φ}}\\ -\tilde{d}\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}-\mathrm{\γ}\tilde{d}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}+\mathrm{\α}\tilde{d}\cos \widetilde{\mathrm{\φ}}\end{array}\right]---\left(19\right)$

根据公式(19)可知，当MEMS/GPS组合导航***与LDS存在初始安装失准角时，α、γ将直接对目标点的三维位置解算精度产生影响。

3.误差角估计方法

根据公式(19)描述的目标点定位方程，在忽略MEMS/GPS组合导航***位置误差和LDS测距误差的情况下，参考点R的定位方程写成：

$\left[\begin{array}{c}{\tilde{x}}_R^e\\ {\tilde{y}}_R^e\\ {\tilde{z}}_R^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\·\left[\begin{array}{c}{d}_{\mathrm{oR}}\sin \tilde{H}-\mathrm{\γ}{d}_{\mathrm{oR}}\cos \tilde{H}\\ d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}+\mathrm{\γ}{d}_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}+\mathrm{\α}{d}_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\\ -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}-\mathrm{\γ}{d}_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}+\mathrm{\α}{d}_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\end{array}\right]---\left(20\right)$

由于参考点R的实际位置表示为：

$\left[\begin{array}{c}{x}_R^e\\ y_R^e\\ z_R^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\·\left[\begin{array}{c}{d}_{\mathrm{oR}}\sin H\\ d_{\mathrm{oR}}\cos \mathrm{\φ}\cos H\\ -d_{\mathrm{oR}}\sin \mathrm{\φ}\cos H\end{array}\right]---\left(21\right)$

由公式(20)和(21)比较，得：

$\left[\begin{array}{c}{\mathrm{\Δx}}_R^e\\ {\mathrm{\Δy}}_R^e\\ {\mathrm{\Δz}}_R^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_R^e-x_R^e\\ {\tilde{y}}_R^e-y_R^e\\ {\tilde{z}}_R^e-z_R^e\end{array}\right]{=C}_t^e\·\left[\begin{array}{c}{d}_{\mathrm{oR}}(\sin \tilde{H}\mathrm{\γ}\cos \tilde{H}-\sin H)\\ d_{\mathrm{oR}}(\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}+\mathrm{\γ}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}+\mathrm{\α}\sin \widetilde{\mathrm{\φ}}-\cos \mathrm{\φ}\cos H)\\ -d_{\mathrm{oR}}(\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}+\mathrm{\γ}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}+\mathrm{\α}\cos \widetilde{\mathrm{\φ}}-\sin \mathrm{\φ}\cos H\end{array}\right]---\left(22\right)$

式(22)所示的解算方程中，未知量包括α、β、H和φ。由于MEMS/GPS组合导航***的姿态误差视为小角度，因此真实的姿态量表示为：

$\left\{\begin{array}{c}\mathrm{\φ}=\widetilde{\mathrm{\φ}}-\mathrm{\Δ\φ}\\ H=\tilde{H}-\mathrm{\ΔH}\end{array}\right.---\left(23\right)$

式中，Δφ、ΔH分别是MEMS/GPS组合导航***的纵摇误差和航向误差。

将公式(23)代入至公式(24)中，得：

$\left[\begin{array}{c}{\mathrm{\Δx}}_R^e\\ {\mathrm{\Δy}}_R^e\\ {\mathrm{\Δz}}_R^e\end{array}\right]=C_t^e\·\left[\begin{array}{cccc}0& d_{\mathrm{oR}}\cos \tilde{H}& 0& {-d}_{\mathrm{oR}}\cos \tilde{H}\\ -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}& -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}& d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}\\ -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}& -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}\end{array}\right]\·\left[\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\ΔH}\\ \mathrm{\α}\\ \mathrm{\γ}\end{array}\right]---\left(24\right)$

由式(24)中的矩阵第二列和第四列在每一行的对应元素是大小相等、符号相反的，即无法单独对ΔH和γ进行求解。为此，将其转化为：

$\left[\begin{array}{c}{\mathrm{\Δx}}_R^e\\ {\mathrm{\Δy}}_R^e\\ {\mathrm{\Δz}}_R^e\end{array}\right]=C_t^e\·\left[\begin{array}{cccc}0& d_{\mathrm{oR}}\cos \tilde{H}& 0& \\ -d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}& -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}& \\ -d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}& d_{\mathrm{oR}}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{\mathrm{oR}}\cos \widetilde{\mathrm{\φ}}& \end{array}\right]\·\left[\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\ΔH}-\mathrm{\γ}\\ \mathrm{\α}\end{array}\right]=C_t^e\·C\·\left[\begin{array}{c}\mathrm{\Δ\φ}\\ \mathrm{\ΔH}-\mathrm{\γ}\\ \mathrm{\α}\end{array}\right]---\left(25\right)$

然而，采用公式(17)进行误差估计仍存在det|C|＝0的问题。为此，在能够获取两个外部观测点R₁和R₂的情况下，分别获取两组坐标误差值和两个不同的观测矩阵C₁和C₂，避免det|C|＝0的问题，顺利完成误差估计。

为使估计误差更方便，需要对公式(25)的观测方程进行转化。由于：

结合式(26)所示的特征，对式(25)左乘得变换后的观测方程为：

式中，ΔH_γ＝ΔH-γ。

在能够获取两个外部观测点R₁和R₂的情况下，分别获得观测方程Z₁、Z₂，观测矩阵C₁和C₂，从而构成组合的观测方程：

根据式(27)利用最小二乘法，同时实现姿态误差角和安装失准角的估计。

在能够获取更多参考点时，可以通过对式(28)增补观测方程的方式，完成对姿态误差和安装失准角的估计。例如，当取得三个参考点时，式(28)可以增补为：

完成 和的估计后，仍可先根据式(20)计算目标点的初步测量再根据式(26)计算坐标误差通过式(30)所示的坐标修正，即可得到目标点位置的准确估值：

$\left[\begin{array}{c}{\hat{x}}_T^e\\ {\hat{y}}_T^e\\ {\hat{z}}_T^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_T^e\\ {\tilde{y}}_T^e\\ {\tilde{z}}_T^e\end{array}\right]-\left[\begin{array}{c}{\mathrm{\Δ}\hat{x}}_T^e\\ \mathrm{\Δ}{\hat{y}}_T^e\\ \mathrm{\Δ}{\hat{z}}_T^e\end{array}\right]---\left(30\right)$

式(30)所示，即经过校准补偿后的目标点三维坐标。即经过校准后，可进行目标点的精确定位。

下面对本发明所提出方法的有效性，进行仿真验证。

(1)考虑安装失准角在内的目标点定位误差仿真

假设观测者所在位置为东经126°，北纬45°。MEMS/GPS组合导航***的位置误差为0.2m，航向角和纵摇角误差分别为0.2°和0.1°，姿态误差随机噪声0.03°。LDS在200米距离范围内的测距误差为5mm，300米距离以上的线性度误差为0.15％。目标点相对观测点的姿态变化由H＝30°+7°sin(2π*t/7)，φ＝2°+1°sin(2π*t/7)描述。

条件1：只考虑MEMS/GPS组合导航***和LDS的误差；

条件2：只考虑安装误差角的影响α＝γ＝-0.1°；

条件3：只考虑安装误差角的影响α＝γ＝-0.2°；

条件4：考虑MEMS/GPS组合导航***和LDS的误差，及安装误差角的影响，且α＝γ＝-0.2°。

图4a～图4c给出了对应上述仿真条件的目标点定位误差仿真结果。

如图4a所示，采用未经校准的MEMS/GPS组合导航***姿态输出求解目标点位置时，姿态误差会激励较大的定位误差，而且距离越远误差越大。而图4b表明，MEMS/GPS组合导航***与LDS间的安装失准角也会激励很大的目标点定位误差，且定位误差随距离和失准角的增大而增大。图4c表明，同时存在姿态误差角和安装失准角将会激励更大的目标点定位误差。

(2)误差角估计算法性能仿真

误差角估计算法能否准确完成姿态误差角和安装失准角的估计，是提高目标点位置解算的关键。采用与误差仿真类似的误差特性，对误差角估计算法性能进行仿真验证。给定初始位置、MEMS/GPS组合导航***位置误差、LDS误差同考虑安装失准角在内的目标点定位误差仿真，其他误差源根据如下的条件进行动态设定：

条件1：α＝γ＝-0.2°，d_oR1＝300m，d_oR2＝280m，ΔH＝0.2°，Δφ＝0.1°；得到的姿态误差和安装失准角估计结果曲线如图5所示；

条件2：α＝γ＝-0.1°，d_oR1＝300m，d_oR2＝280m，ΔH＝0.1°sin(2πt/50),Δφ＝0.05°sin(2πt/50)；得到的姿态误差和安装失准角估计结果曲线如图6a～图6c所示；

由图5a～图5c、图6a～图6c可知，虽然给定了不同量级的误差源,，误差角估计算法均能准确实现误差源Δφ、ΔH_γ和α的准确估计，校准误差小于0.03°。当姿态误差角的误差特性发生变化时，误差角估计算法具有同样的性能。由此可知，在给定外部基准参考点后，该算法能够准确完成姿态误差角和安装失调角的估计。

(3)校准后的定位性能仿真

误差角估计算法的性能仿真表明，在存在姿态误差角和安装失准角的情况下，误差角估计算法能够准确估计这些误差并进行补差，这对于提高目标点位置解算精度应该是非常有帮助的。下面的仿真将通过三种仿真条件对校准后的定位性能进行分析。

条件1：α＝γ＝-0.2°，d_oR1＝300m，d_oR2＝280m，ΔH＝0.2°，Δφ＝0.1°，不考虑LDS误差；

条件2：α＝γ＝-0.2°，d_oR1＝300m，d_oR2＝280m，ΔH＝0.2°，Δφ＝0.1°；

条件3：α＝γ＝-0.1°，d_oR1＝300m，d_oR2＝140m，ΔH＝0.2°，Δφ＝0.1°。

如图7a所示，在不考虑LDS误差的情况下，未校准的目标点定位误差很大，而经过校准后，目标点的定位误差几乎为零，这说明姿态误差角和安装失调角均得到了准确估计。在图7b中，受LDS误差的影响，目标点的误差较图7a有所增长，但较未校准状态仍有大幅减小。在图7c中，虽然参考点与观测点的斜据较图7b中的有所减少，但校准算法仍能较为准确的实现姿态误差和安装失调角估计，并大幅提升目标点定位精度。

Claims (1)

1.一种基于激光测距和MEMS/GPS组合导航***目标导航测绘误差角估计方法，其特征在于，包括如下步骤：

(1)在观测点o附近选取一参考点R₁，利用MEMS/GPS组合导航***精确测量参考点R₁的三维位置坐标

(2)将MEMS/GPS组合导航***移至观测点o，将激光测距仪瞄准参考点R₁，利用MEMS/GPS组合导航***测量观测点o三维位置坐标经纬度矢量oR₁相对地理坐标系ox_ty_tz_t的航向角和纵摇角利用激光测距仪测量矢量oR₁的斜距d_oR1；

(3)根据步骤(2)中MEMS/GPS组合导航***和激光测距仪的测量值，计算参考点R₁在地球坐标系o_ex_ey_ez_e中的三维位置坐标

所涉及参考点R₁的三维位置坐标表达式为：

$\left[\begin{array}{c}{\tilde{x}}_{R1}^e\\ {\tilde{y}}_{R1}^e\\ {\tilde{z}}_{R1}^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\·C_t^2\·\left[\begin{array}{ccc}1& -\mathrm{\γ}& \mathrm{\β}\\ \mathrm{\γ}& 1& -\mathrm{\α}\\ -\mathrm{\β}& \mathrm{\α}& 1\end{array}\right]\left[\begin{array}{c}0\\ d_{oR1}\\ 0\end{array}\right]$

式中，α、β、γ为MEMS/GPS和激光测距仪间存在的安装失准角；为地理坐标系与地球坐标系间的转换矩阵，为两次空间旋转变换矩阵；

所涉及的地理坐标系与地球坐标系间的转换矩阵表达式为：

所涉及的两次空间旋转变换矩阵表达式为：

$C_t^2=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\widetilde{\mathrm{\φ}}& sin\widetilde{\mathrm{\φ}}\\ 0& -\sin \widetilde{\mathrm{\φ}}& cos\widetilde{\mathrm{\φ}}\end{array}\right]\·\left[\begin{array}{ccc}\cos \tilde{H}& \sin \tilde{H}& 0\\ -\sin \tilde{H}& \cos \tilde{H}& 0\\ 0& 0& 1\end{array}\right];$

(4)根据参考点R₁的真实位置及步骤(3)中计算得出参考点R₁的三维位置坐标，再通过计算得到相对位置误差并构建基于姿态误差角和安装失准角的相对位置误差表示；

所涉及的参考点R₁的相对位置误差表达式为：

$\left[\begin{array}{c}\mathrm{\Δ}{x}_{R1}^e\\ {\mathrm{\Δy}}_{R1}^e\\ {\mathrm{\Δz}}_{R1}^e\end{array}\right]=\left[\begin{array}{c}{\tilde{x}}_{R1}^e-x_{R1}^e\\ {\tilde{y}}_{R1}^e-y_{R1}^e\\ {\tilde{z}}_{R1}^e-z_{R1}^e\end{array}\right]$

构建基于姿态误差角和安装失准角的相对位置误差表达式为：

$\left[\begin{array}{c}\mathrm{\Δ}{x}_{R1}^e\\ {\mathrm{\Δy}}_{R1}^e\\ {\mathrm{\Δz}}_{R1}^e\end{array}\right]=C_t^e\·C_1\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}H-\mathrm{\γ}\\ \mathrm{\α}\end{array}\right]$

式中， $\mathrm{\Δ}\mathrm{\φ}=\widetilde{\mathrm{\φ}}-\mathrm{\φ},\mathrm{\Δ}H=\tilde{H}-H,$

$C_1=\left[\begin{array}{ccc}0& d_{{\mathrm{oR}}_1}\cos \widetilde{\mathrm{\φ}}& 0\\ -d_{{\mathrm{oR}}_1}\sin \widetilde{\mathrm{\φ}}\cos \tilde{H}& -d_{{\mathrm{oR}}_1}\cos \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{{\mathrm{oR}}_1}\sin \widetilde{\mathrm{\φ}}\\ -d_{{\mathrm{oR}}_1}\cos \widetilde{\mathrm{\φ}}\cos \tilde{H}& d_{{\mathrm{oR}}_1}\sin \widetilde{\mathrm{\φ}}\sin \tilde{H}& d_{{\mathrm{oR}}_1}\cos \widetilde{\mathrm{\φ}}\end{array}\right];$

所涉及的参考点R₁的真实位置表达式为：

$\left[\begin{array}{c}{x}_{R1}^e\\ y_{R1}^e\\ z_{R1}^e\end{array}\right]=\left[\begin{array}{c}{x}_o^e\\ y_o^e\\ z_o^e\end{array}\right]+C_t^e\·\left[\begin{array}{c}{d}_{oR1}\sin H\\ d_{oR1}\cos \mathrm{\φ}\cos H\\ -d_{oR1}\sin \mathrm{\φ}\cos H\end{array}\right]$

式中，H、φ为观测点o与参考点R₁构成矢量oR₁相对地理坐标系ox_ty_tz_t的真实航向角和纵摇角；

(5)根据步骤(4)中基于姿态误差角和安装失准角的相对位置误差表达式，构建基于参考点R₁的观测方程；

所涉及的参考点R₁的观测方程表达式为：

$Z_1=C_1\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ {\mathrm{\ΔH}}_{\mathrm{\γ}}\\ \mathrm{\α}\end{array}\right]$

式中，ΔH_γ＝ΔH-γ，

(6)重复步骤(1)～(5)，基于外部参考点R₂，获取新的观测量Z₂和观测矩阵C₂，并构建观测方程；

所涉及的参考点R₂的观测方程表达式为：

$Z_2=C_2\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ {\mathrm{\ΔH}}_{\mathrm{\γ}}\\ \mathrm{\α}\end{array}\right]$

(7)基于步骤(5)和步骤(6)中参考点R₁、R₂的观测方程，构成组合观测方程，利用最小二乘法，同时实现姿态误差角和安装失准角的估计；

所涉及的组合观测方程为：

$\[\frac{Z_1}{Z_2}\]=\[\frac{C_1}{C_2}\]\·\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ {\mathrm{\ΔH}}_{\mathrm{\γ}}\\ \mathrm{\α}\end{array}\right]$

姿态误差角和安装失准角的估计结果为：

$\left[\begin{array}{c}\mathrm{\Δ}\widehat{\mathrm{\φ}}\\ \mathrm{\Δ}{\hat{H}}_{\mathrm{\γ}}\\ \widehat{\mathrm{\α}}\end{array}\right]={\left\{{\[\frac{C_1}{C_2}\]}^T\·\[\frac{C_1}{C_2}\]\right\}}^{-1}\·\[\frac{C_1}{C_2}\]\·\[\frac{Z_1}{Z_2}\].$