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

摘要

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

主权项

1.基于路标导航的六自由度位置姿态确定方法，其特征在于：包含如下步骤：步骤1，读取光学相机拍摄图像中导航路标的像素坐标移动载体在场景中利用其携带的光学相机可以对导航路标成像，通过提取图像中导航路标的像元、像线坐标，可以获得导航路标在移动载体相机坐标系下的指向方向。令在场景坐标系下，第i个导航路标的位置矢量为移动载体相机坐标系相对场景坐标系位置矢量和转换矩阵分别为和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，利用移动载体当前位置先验估计值确定观测夹角的预报值A_ij^*以及观测矩阵H观测夹角矩阵的预报值为A→*=A12*A23*···An(n-1)*]]>A_ij^*为任意第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→j)n→jsinAij*]]>和分别为第i个与第j个路标的单位视线向量n→i=r→i*ri*]]>n→j=r→j*rj*]]>其中分别为场景坐标系下第i个路标和第j个路标相对移动载体的预报的位置矢量r→i*=r→*-ρ→i]]>r→j*=r→*-ρ→j]]>r_i^*，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)]]>其中Nba=NbNaT(NaNaT)-1;]]>根据姿态转移矩阵得到移动载体相对于场景坐标系的姿态。