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

说明书

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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