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

摘要

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