[发明专利]基于路标导航的六自由度位置姿态确定方法

权利要求书

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

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

移动载体在场景中利用其携带的光学相机可以对导航路标成像，通过提取 图像中导航路标的像元、像线坐标，可以获得导航路标在移动载体相机坐标系 下的指向方向，令在场景坐标系下，第i个导航路标的位置矢量为i＝1，2...n， 移动载体相机坐标系相对场景坐标系位置矢量和转换矩阵分别为和C_ba，则在 移动载体相机坐标系下，第i个导航路标的位置矢量为

r→ib=Cba(r→*-ρ→i)]]>

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

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

pi=fc11(x-xi)+c12(y-yi)+c13(z-zi)c31(x-xi)+c32(y-yi)+c33(z-zi)]]>

li=fc21(x-xi)+c22(y-yi)+c23(z-zi)c31(x-xi)+c32(y-yi)+c33(z-zi)]]>

其中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：

Aij=arccos(pipj+lilj+f2|(pi,li,f)||(pj,lj,f)|),i,j=1,2,...,n]]>且i≠j

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

A→=A12A23...An(n-1)]]>

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

观测夹角矩阵的预报值为

A→*=A12*A23*...An(n-1)*]]>

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

Aij*=arccos((r→*-ρ→i)·(r→*-ρ→j)|r→*-ρ→i||r→*-ρ→j|),i,j=1,2,...,n]]>

观测矩阵的构建公式为

H=h→12Th→23T...h→n(n-1)T]]>

其中

h→ij=m→ijri*+m→jirj*]]>

和为辅助向量，定义如下

m→ij=n→j-(n→i·n→j)n→isinAij*]]>m→ji=n→i-(n→i·n→)n→sinAij*]]>

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

n→i=r→i*ri*]]>n→j=r→j*rj*]]>

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

r→i*=r→*-ρ→i]]>r→j*=r→*-ρ→j]]>

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

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

利用测量的观测夹角矩阵与预报的观测夹角矩阵做差求取观测夹角矩 阵偏差构建观测角偏差矩阵

δA→=A→-A→*]]>

进而利用迭代式

δr→=(HTH)-1HTδA→]]>

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

r→=r→*+(HTH)-1HTδA→]]>

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

构建每个导航路标在移动载体坐标系下的单位指向矢量及其组成的矩阵 形式

n→ib=(pi,li.f)|(pi,li,f)|]]>Nb=n→1bn→2b...n→nb]]>

基于步骤4计算出的移动载体位置确定场景坐标系下每个导航路标的单 位指向矢量及其组成的矩阵形式

Na=r→-ρ→1r1r→-ρ→2r2...r→-ρ→nrn]]>

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

Cba=12Nba(3I-NbaTNba)]]>

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