CN101762277B - 基于路标导航的六自由度位置姿态确定方法 - Google Patents

Abstract

本发明涉及一种基于路标导航的六自由度位置姿态确定方法，属于自动控制***中的信息处理领域。本发明通过像素观测方程中位置元素与姿态元素的耦合性，考虑欧式变换下角度的不变性，把各导航路标观测视线之间所形成的夹角作为观测量，将像素观测方程中位置、姿态状态解耦，对像素观测方程中位置、姿态状态分别求解，减小算法的复杂性，快速确定结果并提高求解精度。该方法的适用性还可以由3个导航路标扩展到多个路标。

Description

基于路标导航的六自由度位置姿态确定方法

技术领域

本发明涉及一种基于路标导航的六自由度位置姿态确定方法，属于自动控制***中的信息处理领域。

背景技术

自主定位导航是自主移动载体的最基本、最重要功能之一，光学传感器由因其能感知丰富的环境信息而受到了普遍关注，许多已有的视觉定位方法都采用了基于路标光学信息进行导航的方式，并在移动机器人、航空航天、车辆导航等领域取得了一定成功。基于路标光学信息进行导航这种方式，主要依靠光学相机拍摄移动载体所处场景的图像，提取出图像中相关路标的像素信息，这些路标在场景中位置是事先已知的，这样通过对路标像素信息和其已知的位置信息的处理，可以解算出移动载体相对于场景的位置、姿态等空间六自由度状态。利用路标像素信息对移动载体的空间状态进行解算是自主路标导航中一个关键环节，六自由度状态估计是一个强非线性、模糊性问题，其解算结果的正确与否以及解算精度都直接影响了移动载体的自主定位能力，决定了移动载体相关任务完成的成功率。因此路标导航的自主状态求取方法是当前科技人员关注的重点问题之一。

在已发展的利用路标像素信息进行六自由度状态解算的方法中，在先技术[1](Sharp C S，Shakernia O，Sastry S S.A vision system for landing anunmanned aerial vehicle[C]//IEEE International Conference on Roboticsand Automation，2001，2：1720-1727.)，直接利用像素观测方程对其进行线性化，利用得到的近似线性观测方程对移动载体的状态进行求解，其求解中采用的数学方法可以是最小二乘拟合算法也可以是其他的贯序处理方法，如Kalman滤波算法。但是由于像素观测方程自身的强非线性和模糊性，导致了线性过程中截断误差较大，这导致了直接对像素观测方程进行线性化的方法求解精度低，不当的初值的选取甚至可能引起求解算法的发散。

在先技术[2](参见陈鹰，遥感影像的数字摄影测量.上海：同济大学出版社，2003.)基于角锥体原理对移动载体的位置状态和姿态状态分别求解，该方法首先根据导航路标与其所成的像素点和光学相机成像小孔在一条直线上的特点，利用距离方程组先求解移动载体的位置状态，然后在基于此求解移动载体的姿态状态。该方法有效地克服了位置状态和姿态状态之间的相关性，算法求解简单、方便。但是该方法只适合于基于3个导航路标信息的空间状态求解，当拍摄到的路标像素点大于3个时，该方法只能利用其中3个路标进行求解，这显然不能充分利用到其他路标的信息，无法提高导航精度。

发明内容

本发明的目的是为了解决目前利用路标像素信息进行六自由度状态解算方法中精度低、适用性差等问题，给出一种六自由度位置姿态确定方法。

本发明的设计思想为：针对像素观测方程中位置元素与姿态元素的耦合性，考虑欧式变换下角度的不变性，把各导航路标观测视线之间所形成的夹角作为观测量，将像素观测方程中位置、姿态状态解耦，对像素观测方程中位置、姿态状态分别求解，减小算法的复杂性，快速求得结果并提高求解精度。该算法的适用性还可以由3个导航路标扩展到多个路标。

一种六自由度位置姿态确定方法，具体步骤为：

步骤1，读取光学相机拍摄图像中导航路标的像素坐标

移动载体在场景中利用其携带的光学相机可以对导航路标成像，通过提取图像中导航路标的像元、像线坐标，可以获得导航路标在移动载体相机坐标系下的指向方向。令在场景坐标系下，第i个导航路标的位置矢量为

移动载体相机坐标系相对场景坐标系的位置矢量和转换矩阵分别为

和C_ba，则在移动载体相机坐标系下，第i个导航路标的位置矢量为

${\stackrel{\→}{r}}_i^b=C_{\mathrm{ba}}(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

其中，因场景坐标系为三维坐标系，转换矩阵C_ba为三行三列矩阵。

则所提取到的第i个导航路标的像元p_i、像线l_i坐标值与移动载体的位置和姿态之间满足下式关系

$p_i=f\frac{c_{11}(x-x_i)+c_{12}(y-y_i)+c_{13}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

$l_i=f\frac{c_{21}(x-x_i)+c_{22}(y-y_i)+c_{23}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

其中x，y，z为移动载体在场景坐标系下的三轴位置坐标，x_i，y_i，z_i为第i个导航路标在场景坐标系下的三轴位置坐标，c_ba(a＝1，2，3；b＝1，2，3)为转换矩阵C_ba中相应元素，f为光学相机的焦距。

步骤2，利用步骤1得到的导航路标的像元、像线坐标以及光学相机的焦距f，构建光学相机对导航路标的观测夹角矩阵

设移动载体的光学相机对第i个和第j个导航路标观测视线所形成的观测夹角为A_ij，则

$\cos A_{\mathrm{ij}}=\frac{{\stackrel{\→}{r}}_i\·{\stackrel{\→}{r}}_j}{r_i{r}_j}=\frac{{\stackrel{\→}{r}}_i^b\·{\stackrel{\→}{r}}_j^b}{r_i{r}_j}$

上式中

为场景坐标系下第i个和第j个导航路标相对移动载体的位置矢量，r_i，r_j为第i个和第j个导航路标与移动载体之间的距离。

利用n个导航路标中任意第i个和第j个路标组合的像元p_i、p_j和像线l_i、l_j坐标以及光学相机的焦距f，可以构建测量的观测夹角

$A_{\mathrm{ij}}=\arccos \left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right)(i,j=\mathrm{1,2},\·\·\·,n)$ 且(i≠j)

对于n个导航路标而言，任意两个路标两两组合共能构建出n(n-1)/2个观测夹角，这n(n-1)/2个观测夹角构成了测量的观测夹角矩阵

$\stackrel{\→}{A}=\left[\begin{array}{c}{A}_{12}\\ A_{23}\\ .\\ .\\ .\\ A_{n(n-1)}\end{array}\right]$

步骤3，利用移动载体当前位置先验估计值

确定观测夹角的预报值A_ij ^*以及观测矩阵H

利用移动载体自身位置预报给出的移动载体当前位置先验估计值

可以确定观测夹角矩阵的预报值

以及观测矩阵H，其中

${\stackrel{\→}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ .\\ .\\ .\\ A_{n(n-1)}^{*}\end{array}\right]$

A_ij ^*为任意第i个和第j个路标组合所对应的预报的观测夹角，其表达式为

$A_{\mathrm{ij}}^{*}=\arccos \left(\frac{({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)\·({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j)}{|{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i||{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j|}\right)(i,j=\mathrm{1,2},\·\·\·,n)$

观测矩阵的构建公式为

$H=\left[\begin{array}{c}{\stackrel{\→}{h}}_{12}^T\\ {\stackrel{\→}{h}}_{23}^T\\ .\\ .\\ .\\ {\stackrel{\→}{h}}_{n(n-1)}^T\end{array}\right]$

其中

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i^{*}}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j^{*}}$

和

为辅助向量，定义如下

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}^{*}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_j}{\sin A_{\mathrm{ij}}^{*}}$

和

分别为第i个与第j个路标的单位视线向量

${\stackrel{\→}{n}}_i=\frac{{\stackrel{\→}{r}}_i^{*}}{r_i^{*}}$ ${\stackrel{\→}{n}}_j=\frac{{\stackrel{\→}{r}}_j^{*}}{r_j^{*}}$

其中

分别为场景坐标系下第i个路标和第j个路标相对移动载体的预报的位置矢量

${\stackrel{\→}{r}}_i^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i$ ${\stackrel{\→}{r}}_j^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j$

r_i ^*，r_j ^*为第i个和第j个导航路标与移动载体之间的预报的距离。

步骤4，利用步骤2、3中得到的观测夹角与观测矩阵，确定移动载体相对场景的位置

利用测量的观测夹角矩阵

与预报的观测夹角矩阵做差求取观测夹角矩阵偏差

构建观测角偏差矩阵

$\mathrm{\δ}\stackrel{\→}{A}=\stackrel{\→}{A}-{\stackrel{\→}{A}}^{*}$

进而利用迭代式

$\mathrm{\δ}\stackrel{\→}{r}={\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

迭代运算位置偏差量

利用该偏差量对位置先验估计值进行改进，求解出当前移动载***置：

$\stackrel{\→}{r}={\stackrel{\→}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

步骤5，利用步骤4得到的移动载体相对场景的位置结果，确定移动载体相对场景的姿态

利用n个导航路标的像元p_i、像线l_i坐标以及光学相机的焦距f，构建每个导航路标在移动载体坐标系下的单位指向矢量

及其组成的矩阵形式

${\stackrel{\→}{n}}_i^b=\frac{(p_i,l_i,f)}{\left|(p_i,l_i,f)\right|}$ $N_b=\left[\begin{array}{cccc}{\stackrel{\→}{n}}_1^b& {\stackrel{\→}{n}}_2^b& ...& {\stackrel{\→}{n}}_n^b\end{array}\right]$

基于步骤4计算出的移动载***置

确定场景坐标系下每个导航路标的单位指向矢量

及其组成的矩阵形式

$N_a=\left[\begin{array}{cccc}\frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_1}{r_1}& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_2}{r_2}& ...& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_n}{r_n}\end{array}\right]$

利用N_a和N_b，求解移动载体相对于场景坐标系的姿态转移矩阵最优解C_ba为：

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

其中 $N_{\mathrm{ba}}=N_b{N}_a^T{\left(N_a{N}_a^T\right)}^{-1}.$ 根据姿态转移矩阵便可以得到移动载体相对于场景坐标系的姿态。

有益效果

本发明所提供的方法考虑欧式变换下角度的不变性，利用各导航路标观测视线之间所形成的夹角作为观测量，将像素观测方程中位置、姿态状态解耦，弱化观测***的非线性强度与复杂程度，使解耦后的线性方程求解线性度好、迭代效率高且运算简单；非线性程度的减弱，使得截断误差减小，从而提高解算精度；迭代效率的提高，使得解算时间变小，***的实时性得以提高。

本方法适用于自主定位导航***中存在3个及3个以上导航路标的情况，能够实时地对移动载体的六自由空间位置、状态进行求解，在空间探测、卫星定位、无人机等工程领域有重大的应用价值。

附图说明

图1为本发明方法的流程图。

图2为本发明中的导航路标成像关系示意图。

图3为本发明具体实施方式中夹角测量位置面示意图。

具体实施方式

为了更好地说明本发明的目的与优点，下面结合附图和具体实施方式对本发明做进一步说明。

本实施例的具体步骤如下：

步骤1，读取光学相机拍摄图像中导航路标的像素坐标

移动载体在场景中利用其携带的光学相机可以对导航路标成像，通过提取图像中导航路标的像元、像线坐标，可以获得导航路标在移动载体坐标系下的指向方向，导航路标成像关系如图2所示。

令在场景坐标系下，第i个导航路标的位置矢量为

移动载体相机坐标系相对场景坐标系的位置矢量先验估计值和转换矩阵分别为

和C_ba，则在移动载体相机坐标系下，第i个导航路标的位置矢量为

${\stackrel{\→}{r}}_i^b=C_{\mathrm{ba}}(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

其中，因场景坐标系为三维坐标系，转换矩阵C_ba为三行三列矩阵。

则所提取到的第i个导航路标的像元p_i、像线l_i坐标值与移动载体的位置和姿态之间满足下式关系

$p_i=f\frac{c_{11}(x-x_i)+c_{12}(y-y_i)+c_{13}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

$l_i=f\frac{c_{21}(x-x_i)+c_{22}(y-y_i)+c_{23}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}---\left(1\right)$

其中x，y，z为移动载体在场景坐标系下的三轴位置坐标，x_i，y_i，z_i为第i个导航路标在场景坐标系下的三轴位置坐标，c_ba(a＝1，2，3；b＝1，2，3)为转换矩阵C_ba中相应元素，f为光学相机的焦距。设跟踪观测到的导航路标共有n个，则相应的观测方程为

$y=h(\stackrel{\→}{r},C_{\mathrm{ba}})=\left[\begin{array}{ccccc}{p}_1& l_1& ...& p_n& l_n\end{array}\right]---\left(2\right)$

步骤2，利用步骤1得到的导航路标的像元、像线坐标以及光学相机的焦距f，构建光学相机对导航路标测量的观测夹角矩阵

由步骤1中(1)式和(2)式可见，在观测方程中代表位置信息的x，y，z和代表姿态信息的c_ba存在严重耦合，同时观测方程具有较强的非线性特性，直接利用(2)式对移动载体的六自由度状态进行估计将会导致算法复杂繁琐，甚至出现发散问题。(2)式求解的复杂性主要是由位置、姿态耦合引起的，注意到欧式变换下角度坐标的不变性，即几何空间中角度坐标与正交姿态的转换无关，下面利用这一特性，引入光学相机对导航路标观测视线之间的夹角为观测量对移动载***置、姿态进行解耦求解。

设移动载体的光学相机对第i个和第j个路标观测视线所形成的观测角为A_ij，则

$\cos A_{\mathrm{ij}}=\frac{{\stackrel{\→}{r}}_i\·{\stackrel{\→}{r}}_j}{r_i{r}_j}=\frac{{\stackrel{\→}{r}}_i^b\·{\stackrel{\→}{r}}_j^b}{r_i{r}_j}$

上式中

为场景坐标系下第i个和第j个导航路标相对移动载体的位置矢量，r_i，r_j为第i个和第j个导航路标与移动载体之间的距离。

该观测角可以利用光学图像中像素、像线坐标表示，即

$A_{\mathrm{ij}}=\arccos \left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right)$

由几何关系可知，对应该夹角测量值的位置面是以两导航路标连线为轴线，旋转通过这两点的一段圆弧而获得的超环面，即在该超环面上观测角都满足A_ij值，如图3所示。这段圆弧的中心O在两路标P_iP_j边线的垂直平分线上，圆弧半径R与两路标之间的距离以及A_ij的关系为

$R=\frac{{\mathrm{\ρ}}_i-{\mathrm{\ρ}}_j}{2\sin A_{\mathrm{ij}}}$

圆心到P_iP_j的距离L为

L＝RcosA_ij

其中，ρ_i和ρ_j分别为向量

和

的模

上述几何描述也可用矢量公式表达，利用

和

的内积关系，有

$\left(\stackrel{\→}{r}{-\stackrel{\→}{\mathrm{\ρ}}}_i\right)\·(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_j)=|\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i||\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_j|\cos A_{\mathrm{ij}}---\left(3\right)$

可见，上式为移动载***置

与测量夹角A_ij的关系式，而与载体的姿态状态无关，因此，可以利用上式对载体的位置状态进行单独求解。

对于n个导航路标而言，任意两个路标两两组合共能构建出n(n-1)/2个观测夹角，这n(n-1)/2个观测夹角构成了测量的观测夹角矩阵

$\stackrel{\→}{A}=\left[\begin{array}{c}{A}_{12}\\ A_{23}\\ .\\ .\\ .\\ A_{n(n-1)}\end{array}\right]$

步骤3，利用移动载体当前位置先验估计值确定测量夹角的预报值A_ij ^*以及观测矩阵H

考虑到式(3)为非线性方程，直接求解比较困难，下面在小偏差线性化假设的条件下，在移动载***置先验估计值

处，对其线性化量测方程进行推导，可以得到位置偏差量

与观测夹角偏差量δA_ij之间的近似线性关系

$\mathrm{\δ}{A}_{\mathrm{ij}}={\stackrel{\→}{h}}_{\mathrm{ij}}\·\mathrm{\δ}\stackrel{\→}{r}$

其中

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i^{*}}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j^{*}}$

和为辅助向量，定义如下

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}^{*}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_j}{\sin A_{\mathrm{ij}}^{*}}$

和分别为第i个与第j个路标的单位视线向量

${\stackrel{\→}{n}}_i=\frac{{\stackrel{\→}{r}}_i^{*}}{r_i^{*}}$ ${\stackrel{\→}{n}}_j=\frac{{\stackrel{\→}{r}}_j^{*}}{r_j^{*}}$

其中

分别为场景坐标系下第i个路标和第j个路标相对移动载体的预报的位置矢量

${\stackrel{\→}{r}}_i^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i$ ${\stackrel{\→}{r}}_j^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j$

r_i ^*，r_j ^*为第i个和第j个导航路标与移动载体之间的预报的距离。在实际运行过程中，利用位置预报给出的移动载体当前位置先验估计值

可以确定测量角度的预报值A_ij ^*以及线性向量

这样通过测量偏差量δA_ij，利用 $\mathrm{\δ}{A}_{\mathrm{ij}}={\stackrel{\→}{h}}_{\mathrm{ij}}\·\mathrm{\δ}\stackrel{\→}{r}$ 式可以对移动载体的当前位置进行改进。如果在图像中共提取到n个导航路标，则共有n(n-1)/2种测量夹角组合，即可以联立n(n-1)/2个如 $\mathrm{\δ}{A}_{\mathrm{ij}}={\stackrel{\→}{h}}_{\mathrm{ij}}\·\mathrm{\δ}\stackrel{\→}{r}$ 形式的方程，组成方程组对移动载体的位置

进行求解。

利用移动载体自身位置预报给出的移动载体当前位置先验估计值

可以确定观测夹角矩阵的预报值

以及观测矩阵H，其中

${\stackrel{\→}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ .\\ .\\ .\\ A_{n(n-1)}^{*}\end{array}\right]$

A_ij ^*为任意第i个和第j个路标组合所对应的预报的观测夹角，其表达式为

$A_{\mathrm{ij}}^{*}=\arccos \left(\frac{({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)\·({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j)}{|{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i||{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j|}\right)(i,j=\mathrm{1,2},\·\·\·,n)$

观测矩阵的构建公式为

$H=\left[\begin{array}{c}{\stackrel{\→}{h}}_{12}^T\\ {\stackrel{\→}{h}}_{23}^T\\ .\\ .\\ .\\ {\stackrel{\→}{h}}_{n(n-1)}^T\end{array}\right]$

步骤4，利用步骤2、3中得到的观测夹角与观测矩阵，确定移动载体相对场景的位置

利用测量的观测夹角矩阵

与预报的观测夹角矩阵

做差求取观测夹角矩阵偏差

构建观测角偏差矩阵

$\mathrm{\δ}\stackrel{\→}{A}=\stackrel{\→}{A}-{\stackrel{\→}{A}}^{*}=\left[\begin{array}{c}\mathrm{\δ}{A}_{12}\\ \mathrm{\δ}{A}_{23}\\ .\\ .\\ .\\ \mathrm{\δ}{A}_{n(n-1)}\end{array}\right]$

进而利用迭代式

$\mathrm{\δ}\stackrel{\→}{r}{=\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

迭代运算位置偏差量

利用该偏差量对位置先验估计值

进行改进，求解出当前移动载***置：

$\stackrel{\→}{r}={\stackrel{\→}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

步骤5，利用步骤4得到的移动载体相对场景的位置结果，确定移动载体相对场景的姿态

由于在移动载体相机坐标系下，导航路标的位置可以表示为

${\stackrel{\→}{r}}_i^b=C_{\mathrm{ba}}(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i)---\left(4\right)$

对式(4)进行单位归一化，可得

${\stackrel{\→}{n}}_i^b=C_{\mathrm{ba}}\frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i}{r_i}$

其中，

利用导航路标的像元、像线坐标可以表示为

${\stackrel{\→}{n}}_i^b=\frac{(p_i,l_i,f)}{\left|(p_i,l_i,f)\right|}$

利用步骤4中得到的移动载***置，以及多矢量定姿原理，确定移动载体相对于场景坐标系的姿态转移矩阵最优解为

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

其中

$N_{\mathrm{ba}}=N_b{N}_a^T{\left(N_a{N}_a^T\right)}^{-1}$

$N_a=\left[\begin{array}{cccc}\frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_1}{r_1}& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_2}{r_2}& ...& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_n}{r_n}\end{array}\right]$ $N_b=\left[\begin{array}{cccc}{\stackrel{\→}{n}}_1^b& {\stackrel{\→}{n}}_2^b& ...& {\stackrel{\→}{n}}_n^b\end{array}\right]$

这样利用步骤4的位置求解式

$\stackrel{\→}{r}={\stackrel{\→}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

与步骤5的姿态矩阵

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

可以确定移动载体相对于场景的位置和姿态等六自由空间状态。

Claims (1)

1.基于路标导航的六自由度位置姿态确定方法，其特征在于：包含如下步骤：

步骤1，读取光学相机拍摄图像中导航路标的像素坐标

移动载体在场景中利用其携带的光学相机可以对导航路标成像，通过提取图像中导航路标的像元、像线坐标，可以获得导航路标在移动载体相机坐标系下的指向方向，令在场景坐标系下，第i个导航路标的位置矢量为

i＝1，2...n，移动载体相机坐标系相对场景坐标系位置矢量和转换矩阵分别为

和C_ba，则在移动载体相机坐标系下，第i个导航路标的位置矢量

为

${\stackrel{\→}{r}}_i^b=C_{\mathrm{ba}}({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

其中，因场景坐标系为三维坐标系，转换矩阵C_ba为三行三列矩阵；

则所提取到的第i个导航路标的像元p_i、像线l_i坐标值与移动载体的位置和姿态之间满足下式关系

$p_i=f\frac{c_{11}\left({x-x}_i\right)+c_{12}\left({y-y}_i\right)+c_{13}\left({z-z}_i\right)}{c_{31}\left({x-x}_i\right)+c_{32}\left({y-y}_i\right)+c_{33}\left({z-z}_i\right)}$

$l_i=f\frac{c_{21}\left({x-x}_i\right)+c_{22}\left({y-y}_i\right)+c_{23}\left({z-z}_i\right)}{c_{31}\left({x-x}_i\right)+c_{32}\left({y-y}_i\right)+c_{33}\left({z-z}_i\right)}$

其中x，y，z为移动载体在场景坐标系下的三轴位置坐标，x_i，y_i，z_i为第i个导航路标在场景坐标系下的三轴位置坐标，c_ba，a＝1，2，3；b＝1，2，3，为转换矩阵C_ba中相应元素，f为光学相机的焦距；

步骤2，利用步骤1得到的导航路标的像元、像线坐标以及光学相机的焦距f，构建测量的观测夹角矩阵

设移动载体的光学相机对第i个和第j个导航路标观测视线所形成的观测夹角为A_ij：

$A_{\mathrm{ij}}=\arccos \left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right),i,j=\mathrm{1,2},...,n$ 且i≠j

对于n个导航路标而言，任意两个路标两两组合共能构建出n(n-1)/2个观测夹角，这n(n-1)/2个观测夹角构成了测量的观测夹角矩阵

$\stackrel{\→}{A}=\left[\begin{array}{c}{A}_{12}\\ A_{23}\\ .\\ .\\ .\\ A_{n(n-1)}\end{array}\right]$

步骤3，利用移动载体当前位置先验估计值

，确定观测夹角的预报值

以及观测矩阵H

观测夹角矩阵的预报值

为

${\stackrel{\→}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ .\\ .\\ .\\ A_{n(n-1)}^{*}\end{array}\right]$

为任意第i个和第j个路标组合所对应的预报的观测夹角，其表达式为

$A_{\mathrm{ij}}^{*}=\arccos \left(\frac{({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)\·({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j)}{|{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i||{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j|}\right),i,j=\mathrm{1,2},...,n$

观测矩阵的构建公式为

$H=\left[\begin{array}{c}{\stackrel{\→}{h}}_{12}^T\\ {\stackrel{\→}{h}}_{23}^T\\ .\\ .\\ .\\ {\stackrel{\→}{h}}_{n(n-1)}^T\end{array}\right]$

其中

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i^{*}}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j^{*}}$

和

为辅助向量，定义如下

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}^{*}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·\stackrel{\→}{n})\stackrel{\→}{n}}{\sin A_{\mathrm{ij}}^{*}}$

和

分别为第i个与第j个路标的单位视线向量

${\stackrel{\→}{n}}_i=\frac{{\stackrel{\→}{r}}_i^{*}}{r_i^{*}}$ ${\stackrel{\→}{n}}_j=\frac{{\stackrel{\→}{r}}_j^{*}}{r_j^{*}}$

其中

分别为场景坐标系下第i个路标和第j个路标相对移动载体的预报的位置矢量

${\stackrel{\→}{r}}_i^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i$ ${\stackrel{\→}{r}}_j^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j$

为第i个和第j个导航路标与移动载体之间的预报的距离；

步骤4，利用步骤2、3中得到的观测夹角与观测矩阵，确定移动载体相对场景的位置

利用测量的观测夹角矩阵

与预报的观测夹角矩阵

做差求取观测夹角矩阵偏差

构建观测角偏差矩阵

$\mathrm{\δ}\stackrel{\→}{A}=\stackrel{\→}{A}-{\stackrel{\→}{A}}^{*}$

进而利用迭代式

$\mathrm{\δ}\stackrel{\→}{r}={\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

迭代运算位置偏差量

利用该偏差量对位置先验估计值

进行改进，求解出当前移动载***置：

$\stackrel{\→}{r}={\stackrel{\→}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

步骤5，利用步骤4得到的移动载体相对场景的位置结果，确定移动载体相对场景的姿态

构建每个导航路标在移动载体坐标系下的单位指向矢量

及其组成的矩阵形式

${\stackrel{\→}{n}}_i^b=\frac{(p_i,l_i.f)}{\left|(p_i,l_i,f)\right|}$ $N_b=\left[\begin{array}{cccc}{\stackrel{\→}{n}}_1^b& {\stackrel{\→}{n}}_2^b& ...& {\stackrel{\→}{n}}_n^b\end{array}\right]$

基于步骤4计算出的移动载***置

确定场景坐标系下每个导航路标的单位指向矢量

及其组成的矩阵形式

$N_a=\left[\begin{array}{cccc}\frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_1}{r_1}& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_2}{r_2}& ...& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_n}{r_n}\end{array}\right]$

利用N_a和N_b，求解移动载体相对于场景坐标系的姿态转移矩阵最优解C_ba为：

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

其中

根据姿态转移矩阵得到移动载体相对于场景坐标系的姿态。

Images