申请/专利权人：深圳市北斗产业互联网研究院

申请日：2018-06-22

公开（公告）日：2020-09-15

公开（公告）号：CN108801248B

主分类号：G01C21/16(20060101)

分类号：G01C21/16(20060101)

优先权：

专利状态码：失效-未缴年费专利权终止

法律状态：2022.06.07#未缴年费专利权终止;2018.12.07#实质审查的生效;2018.11.13#公开

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

主权项：1.基于UKF的平面视觉惯性导航方法，其特征在于，包括如下步骤：S1：计算惯性导航系统的非线性惯导误差传播模型；S2：根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；S3：根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；S4：根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；S5：根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正；其中，步骤S1具体包括如下步骤：S10：定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为其中，CθG∈R3×1表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，GvC为摄像机在全局坐标系{G}中的速度，GpC为摄像机在全局坐标系{G}中的位置，ba为加速度计的偏差矢量，bg为陀螺仪的偏差矢量；S11：通过公式获取惯性导航系统的状态向量的时间更新；其中，ωt＝[ω1ω2ω3]T为惯性测量单元的角速率，at＝[a1a2a3]T为惯性测量单元的线性加速度，Gg＝[00g]T为惯性测量单元的重力加速度，Cq为惯性测量单元的旋转矩阵对应的四元数，q∈R4×1，nδa为加速度计误差造成的高斯白噪声，nδg为陀螺仪误差造成的高斯白噪声；S12：建立陀螺仪的输出测量信号模型为：ωmt＝ωt+bgt+ngt，以及建立加速度计的输出测量信号模型为：amt＝CCqGtGat-Gg+bat+nat；S13：定义惯性导航系统的误差状态向量为：从而建立非线性惯导误差传播模型为其中，和是分别是离散时间状态和系统噪声的传播矩阵；步骤S2包括如下子步骤：S20：设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为Cpf，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为和Cz，Cpf＝CCqGGpf-GpC；S21：定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式进行转换，使得特征点f、pf分别在两个坐标系中的位置通过公式Cpf＝CCVVpf得到关联；S22：将Vpf在虚拟影像平面中的投影替代Cpf＝CCVVpf中的Vpf得到S23：根据公式将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到也就是S24:设定虚拟摄像机的光轴与所需的影像平面正交，则S25：设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其{z}轴是平面的法线，则可计算得到为摄像机的投影模型；步骤S3具体包括如下子步骤：S30：设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和{Cl}，则特征点f对于这两个参考坐标系的位置为其中，为相应的旋转矩阵，为平移矢量；S31：根据公式得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型即步骤S4具体包括如下子步骤：S40：设定当前的特征点fi与前一帧的参考坐标系{Cl}观察的特征点匹配，则ni为零均值噪声；S41：建立同一个平面上的M个特征点匹配后构成的非线性测量方法为其中，为摄像机在Cl坐标系下的位置误差，为摄像机在Cl坐标系下的姿态角误差，是参考坐标系Cl中匹配特征的归一化像素坐标。

全文数据：基于UKF的平面视觉惯性导航方法技术领域[0001]本发明涉及导航技术，尤其涉及基于UKF的平面视觉惯性导航方法。背景技术[0002]将航行载体从起始点引导到目的地的过程称为导航。导航有多种技术途径，如无线电导航、天文导航、卫星导航、红外导航、惯性导航、视觉导航等。[0003]其中，惯性导航是利用加速度计与陀螺仪计算航程，推知当前位置和下一步目的，自主性强，不易受干扰，是目前的主要导航方法、但惯性导航系统由于其固有的导航误差积累，导航精度随时间增长而降低，并且设备成本高，不能完全满足实际应用的需求。[0004]而视觉导航采用成像设备拍摄图像，运用机器视觉等相关技术识别路径，实现自动导航。视觉导航因其应用范围广，在理论上具有最佳指导柔性，近年来发展十分迅速。但是视觉导航主要存在图像匹配的精度差、目标图像中的像点定位有误差、测量系统的标点误差以及成像系统的空间分辨率有限的缺点，导致其应用也受到限制。发明内容[0005]为了克服现有技术的不足，本发明的目的在于提供基于UKF的平面视觉惯性导航方法，其能解决现有技术存在的精度低、存在误差等的问题。[0006]本发明的目的采用以下技术方案实现：[0007]基于UKF的平面视觉惯性导航方法，包括如下步骤：[0008]SI:计算惯性导航系统的非线性惯导误差传播模型；[0009]S2:根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；[0010]S3:根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；[0011]S4:根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；[0012]S5:根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正。[0013]优选的，步骤Sl具体包括如下步骤：[0014]S10:定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为.其中，e0ceR3xl表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，evc为摄像机在全局坐标系{G}中的速度，ePc为摄像机在全局坐标系{G}中的位置，ba为加速度计的偏差矢量，¥为陀螺仪的偏差矢量；[0015]SI1:通过公式获取惯性导航系统的状态向量的时间更新;其中，ωt=[ωιω2ω3]τ为惯性测量单元的角速率，a⑴=[aia2a3]T为惯性测量单元的线性加速度，Gg=[0Og]T为惯性测量单元的重力加速度，Cq为惯性测量单元的旋转矩阵对应的四元数，qeR4xl，nSaS加速度计误差造成的高斯白噪声，nSg为陀螺仪误差造成的高斯白噪声；[0016]S12:建立陀螺仪的输出测量信号模型为：cojt=ω⑴+bjt+ndt，，以及建立加速度计的输出测量信号模型为:am⑴=CeqGtGa⑴-¾+ba⑴+na⑴，；[0017]S13:定义惯性导航系统的误差状态向量为：从而建立非线性惯导误差传播模型为是分别是离散时间状态和系统噪声的传播矩阵。[0018]优选的，步骤S2包括如下子步骤：[0019]S20:设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为ePf，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为和、，cPf=CcqcGpf-Gpc；?[0020]S21:定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式进行转换，使得特征点f、Pf分别在两个坐标系中的位置通过公式ePf=eCvvPf得到关联；[0021]S22:将vPf在虚拟影像平面（9中的投影替代epf=eCvvpf中的vPf得到[0022]S23:根据公式将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到也就是[0023]S24:设定虚拟摄像机的光轴与所需的影像平面正交，则[0025]S25:设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其Iz}轴是平面的法线，则可计算得到为摄像机的投影模型。[0026]优选的，步骤S3具体包括如下子步骤：[0027]S30:设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和{CU，则特征点f对于这两个参考坐标系的位置为epf=eCclelpf+eU1，其中，eCci为相应的旋转矩阵，etQ为平移矢量；[0028]S31:根据公式得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型即[0029]优选的，步骤S4具体包括如下子步骤：[0030]S40:设定当前的特征点匕与前一帧的参考坐标系{M观察的特征点匹配，则m为零均值噪声；[0031]S41:建立同一个平面上的M个特征点匹配后构成的非线性测量方法为其中，δχ_=[Sgpc1δαθ[0056]S24:设定虚拟摄像机的光轴与所需的影像平面正交，则[0058]S25:设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其Iz}轴是平面的法线，参考图2,可得到虚拟摄像机坐标系相对于局部坐标系的方向为C0V=diag1，_1，_1,，从图2可以看aLpcSLpc=CSgGpc+gPl，上式两边同时乘以，则可以得到将代入则可计算得到为摄像机的投影模型。为了测量这个值，可以使用用于测量斜率相对于重力角度的测斜仪。[0059]步骤S3具体包括如下子步骤：[0060]S30:设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和{CU，则特征点f对于这两个参考坐标系的位置为ePf=eCclelPf+eU1，其中，eCci为相应的旋转矩阵，为平移矢量；[0061]ePf=eCclQpf+eta，将两个摄像机视图中的特征f的观察结果与其相应的平移关联起来，由旋转矩阵eCa和平移矢量etei进行表示，因此，epf的投影与系统运动状态变量相关，可以对累积的惯性导航系统误差施加约束。惯性测量单元的摄像机及在当前{C}时间下的坐标系并且以1滞后于当前的{M，不同时刻之间的相对旋转和平移由表示。假定全局参考系G位于水平面上，其中z轴垂直于水平面。位于所需平面上的样本特征点f被认为是在相机的视野中；[0062]S31:由于该点属于目标平面，Qpf可以用相机姿•icnCfqQ以及观测值表示，根据公式得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型即♦[0063]步骤S4具体包括如下子步骤：[0064]S40:设定当前的特征点匕与前一帧的参考坐标系{M观察的特征点匹配，则m为零均值噪声;具有协方差矩阵i[0065]S41:建立同一个平面上的M个特征点匹配后构成的非线性测量方法为其中，δχ_=[Sgpc1δαθ且这里λ=α2Ν+κ-Ν，其中参数设为α=〇.1，β=2和κ’O=0,这也决定了sigam点通过权重_进行传播。[0068]基于上述的N=15+6+2M时，基于UKF的运动估计算法流程如下：[0071]对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。

权利要求：1.基于UKF的平面视觉惯性导航方法，其特征在于，包括如下步骤：SI:计算惯性导航系统的非线性惯导误差传播模型；S2:根据状态变量以及所需导航平面上的三维位置计算出摄像机的投影模型；S3:根据所述投影模型计算得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型；S4:根据所述投影模型和位置模型计算得到同一平面上的多个测量特征点匹配构成的非线性测量方程；S5:根据所述非线性测量方法获取UKF线性误差，根据该UKF线性误差对非线性惯导误差传播模型进行修正。2.如权利要求1所述的平面视觉惯性导航方法，其特征在于，步骤Sl具体包括如下步骤：S10:定义惯性导航系统相对于参考坐标系的姿态由全局坐标系{G}表示，以向量形式定义惯性导航系统的状态变量为其中，e9ceR3xl表示的是全局坐标系{G}在摄像机参考坐标系{C}中的方向，evC为摄像机在全局坐标系{G}中的速度，ePC为摄像机在全局坐标系{G}中的位置，baS加速度计的偏差矢量，bg为陀螺仪的偏差矢量；Sl1:通过公式获取惯性导航系统的状态向量的时间更新；其中，ωt=[c〇iω2ω3]τ为惯性测量单元的角速率，a⑴=[aia2a3]T为惯性测量单元的线性加速度，Gg=[0Og]T为惯性测量单元的重力加速度，Cq为惯性测量单元的旋转矩阵对应的四元数，qeR4xl，nSa为加速度计误差造成的高斯白噪声，Mg为陀螺仪误差造成的高斯白噪声；S12:建立陀螺仪的输出测量信号模型为：ωmt=ω⑴+bgt+ngt，，以及建立加速度计的输出测量信号模型为:am⑴=CeqGtGaW_Gg+ba⑴+na⑴，；S13:定义惯件导航系统的误差状杰向量为：从而建立非线性惯导误差传播模型为，其中，是分别是离散时间状态和系统噪声的传播矩阵。3.如权利要求2所述的平面视觉惯性导航方法，其特征在于，步骤S2包括如下子步骤：S20:设定摄像机观察的任意一个特征点f，设定相对于摄像机中心{C}的位置为、f，该特征点在摄像机的图像平面上的均匀和归一化像素坐标分别为和Cz，S21:定义一个虚拟坐标系，使虚拟坐标系{V}的中心与摄像机坐标系的中心重合，将摄像机坐标系与虚拟坐标系通过公式进行转换，使得特征点f、pf分别在两个坐标系中的位置通过公式得到关联；S22:将在虚拟影像平面中的投影替代中的.得到S23:根据公式将特征点f在虚像影像平面中的投影可以被映射到真实影像平面，从而计算得到也就是^?S24:设定虚拟摄像机的光轴与所需的影像平面正交，则S25:设定一局部坐标系{L}，假设该局部坐标系{L}在所需导航平面和水平面的交点上，其Iz}轴是平面的法线，则可计算得到为摄像机的投影模型。4.如权利要求3所述的平面视觉惯性导航方法，其特征在于，步骤S3具体包括如下子步骤：S30:设定两个不同位置的摄像机，该两个摄像机分别对应的参考坐标系为{C}和{CU，则特征点f对于这两个参考坐标系的位置为，其中，为相应的旋转矩阵为平移矢量；S31:根据公式得到任意一个测量特征点相对于两个不同位置的摄像机的位置模型5.如权利要求4所述的平面视觉惯性导航方法，其特征在于，步骤S4具体包括如下子步骤：S40:设定当前的特征点6与前一帧的参考坐标系{Cd观察的特征点匹配，则m为零均值噪声；S41:建立同一个平面上的M个特征点匹配后构成的非线性测量方法为其中，为摄像机在C1坐标系下的位置误差，为摄像机在C1坐标系下的姿态角误差，是参考坐标系C1中匹配特征的归一化像素坐标。

百度查询： 深圳市北斗产业互联网研究院 基于UKF的平面视觉惯性导航方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD