[发明专利]基于UKF的平面视觉惯性导航方法

说明书

本发明公开了基于UKF的平面视觉惯性导航方法，包括如下步骤：计算惯性导航系统的非线性惯导误差传播模型；根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正。本发明通过对状态空间进行推导，结合UKF框架进行误差估计和修正，降低视觉惯性导航的误差，适用于任意平面模型。

技术领域

本发明涉及导航技术，尤其涉及基于UKF的平面视觉惯性导航方法。

背景技术

将航行载体从起始点引导到目的地的过程称为导航。导航有多种技术途径，如无线电导航、天文导航、卫星导航、红外导航、惯性导航、视觉导航等。

其中，惯性导航是利用加速度计与陀螺仪计算航程，推知当前位置和下一步目的，自主性强，不易受干扰，是目前的主要导航方法、但惯性导航系统由于其固有的导航误差积累，导航精度随时间增长而降低，并且设备成本高，不能完全满足实际应用的需求。

而视觉导航采用成像设备拍摄图像，运用机器视觉等相关技术识别路径，实现自动导航。视觉导航因其应用范围广，在理论上具有最佳指导柔性，近年来发展十分迅速。但是视觉导航主要存在图像匹配的精度差、目标图像中的像点定位有误差、测量系统的标点误差以及成像系统的空间分辨率有限的缺点，导致其应用也受到限制。

发明内容

为了克服现有技术的不足，本发明的目的在于提供基于UKF的平面视觉惯性导航方法，其能解决现有技术存在的精度低、存在误差等的问题。

本发明的目的采用以下技术方案实现：

基于UKF的平面视觉惯性导航方法，包括如下步骤：

S1：计算惯性导航系统的非线性惯导误差传播模型；

S2：根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；

S3：根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；

S4：根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；

S5：根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正。

优选的，步骤S1具体包括如下步骤：

S10：定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为其中，^Cθ_G∈R^3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，^Gv_C为摄像机在全局坐标系{G}中的速度，^Gp_C为摄像机在全局坐标系{G}中的位置，b_a为加速度计的偏差矢量，b_g为陀螺仪的偏差矢量；