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

权利要求书

1.基于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为陀螺仪的偏差矢量；

S11：通过公式获取惯性导航系统的状态向量的时间更新；其中，ω(t)＝[ω₁ ω₂ ω₃]^T为惯性测量单元的角速率，a(t)＝[a₁ a₂ a₃]^T为惯性测量单元的线性加速度，G_g＝[0 0 g]^T为惯性测量单元的重力加速度，C(q)为惯性测量单元的旋转矩阵对应的四元数，q∈R^4×1，n_δa为加速度计误差造成的高斯白噪声，n_δg为陀螺仪误差造成的高斯白噪声；

S12：建立陀螺仪的输出测量信号模型为：ω_m(t)＝ω(t)+b_g(t)+n_g(t)，以及建立加速度计的输出测量信号模型为：a_m(t)＝C(^CqG(t))(^Ga(t)-^Gg)+b_a(t)+n_a(t)；

S13：定义惯性导航系统的误差状态向量为：从而建立非线性惯导误差传播模型为其中，和是分别是离散时间状态和系统噪声的传播矩阵；

步骤S2包括如下子步骤：

S20：设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为^Cp_f，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为和^Cz，^Cp_f＝C(^Cq_G)(^Gp_f-^Gp_C)；

S21：定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式进行转换，使得特征点f、p_f分别在两个坐标系中的位置通过公式^Cp_f＝^CC_V^Vp_f得到关联；

S22：将^Vp_f在虚拟影像平面中的投影替代^Cp_f＝^CC_V^Vp_f中的^Vp_f得到

S23：根据公式将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到也就是

S24:设定虚拟摄像机的光轴与所需的影像平面正交，则

S25：设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其{z}轴是平面的法线，则可计算得到为摄像机的投影模型；

步骤S3具体包括如下子步骤：

S30：设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和{C_l}，则特征点f对于这两个参考坐标系的位置为其中，为相应的旋转矩阵，为平移矢量；

S31：根据公式得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型即

步骤S4具体包括如下子步骤：

S40：设定当前的特征点f_i与前一帧的参考坐标系{C_l}观察的特征点匹配，则n_i为零均值噪声；

S41：建立同一个平面上的M个特征点匹配后构成的非线性测量方法为其中，为摄像机在C_l坐标系下的位置误差，为摄像机在C_l坐标系下的姿态角误差，是参考坐标系C_l中匹配特征的归一化像素坐标。