(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 20221048040 3.5 (22)申请日 2022.05.05 (71)申请人 桂林电子科技大 学 地址 541004 广西壮 族自治区桂林市七 星 区金鸡路1号 (72)发明人 伍锡如　黄凤堂　刘金霞　 (51)Int.Cl. G01C 21/16(2006.01) G06V 10/80(2022.01) G06K 9/62(2022.01) (54)发明名称 一种点线融合的视 觉惯性导航方法 (57)摘要 本发明公开了一种点线融合的视觉惯性导 航方法， 具体是一种根据点线特征的丰富程度对 点线特征的重投影残差进行加权的视觉惯性导 航方法。 目的是解决现有视觉惯 性导航方法因场 景缺少纹理和相机图像模糊导致的点特征缺失， 以及线特征容易错误关联的问题。 该方法是由如 下步骤实现的： 步骤一： 相机采集灰度图像， IMU 传感器采集加速度和角速度， 主控进行数据的对 齐； 步骤二： 几何表示点线特征， 为视觉惯 性导航 系统的特征追踪模块引入线特征； 步骤三： 提取 与合并线特征， 剔除无用的线特征， 减小线的误 匹配率； 步骤四： 求出点线得重投影误差， 并进行 点线误差融合； 步骤五： 求出点线重投影误差的 雅可比矩阵。 本发明适用于 视觉惯性导航系统。 权利要求书5页 说明书8页 附图3页 CN 114754768 A 2022.07.15 CN 114754768 A 1.一种点线融合的视 觉惯性导航方法， 其特 征在于， 包括 步骤： 步骤一， 双目相机采集前方的灰度图像， IMU传感器采集加速度和角速度， 主控进行数 据的对齐； 步骤二， 几何表示 点线特征， 为视觉惯性导航系统的特 征追踪模块引入线特 征； 步骤三， 提取与合并线特 征， 剔除无用的线特 征， 减小线的误匹配率； 步骤四， 求出点线得重投影误差， 并进行点线误差融合； 步骤五， 求出点线重投影误差的雅可比矩阵， 为后续的非线性优化做准备。 2.根据权利要求1所述点线融合的视觉惯性导航方法， 其特征在于： 所述步骤一中， 具 体步骤包括： (1)双目相机以10 Hz的频率采集左右目的灰度图像； (2)IMU以10 0Hz的频率采集 三轴加速度和角速度； (3)主控进行数据的对齐； 由于触发延迟、 传输延迟和不同步的时钟， 生成的时间戳不 等于实际采样时间， 导 致相机与IMU之间的时间失调； 对齐数据的计算公式为： tIMU＝tcam+td； 其中， tIMU和tcam分别表示 IMU和相机的时间戳； td表示一个未知常数的时间偏移量。 3.根据权利要求1所述点线融合的视觉惯性导航方法， 其特征在于： 所述步骤二中， 具 体步骤包括： (1)几何表示点特征； 设Ow‑XwYwZw为世界坐标系， Oc‑XcYcZc为相机坐标系， O ‑XYZ为图像 坐标系， uv为像素坐标系； (u,v)为图像坐标， fx、 fy、 cx、 cy为相机内参， cx、 cy是像素坐标相对 于图像坐标的偏移量； 空间中一 点pw＝[xw,yw,zw]T转换到相机坐标系记为： 其中， Tcw是世界坐标系w到相机坐标系c的变换关系， Rcw为旋转矩阵， tcw为平移向量； 由相机投影模型， 得到像素坐标为： 其中， Kc为内参矩阵； 由于单目相机具有尺度不确定性， 定义尺度变换因子为s＝1/ zc； (2)几何表示线特征； 设空间直线L两个端点的齐次坐标为P1＝(x1,y1,z1,w1)T和P2＝ (x2,y2,z2,w2)T， 非齐次坐标分别为 和 则空间中直线的普吕克坐标为： 其中， L是一个6 ×1向量， n是直线的矩向量， v是直线的方向 向量， w1和w2表示其次因子； 将空间中直线Lw转换到相机坐标系， 即Lc， Lc用普吕克坐标表示 为：权　利　要　求　书 1/5 页 2 CN 114754768 A 2其中， Hcw为直线变换矩阵， 为直线旋转矩阵， 为直线平移向量， 表示平移 向量的反对称矩阵； 将Lc投影到图像平面得到lc， lc的普吕克坐标为： 其中， Kl表示直线的投影矩阵。 4.根据权利要求1所述点线融合的视觉惯性导航方法， 其特征在于： 所述步骤三中， 具 体步骤包括： (1)提取线特征； 本发明采用EDLine算法来检测线段， 而不是采用广泛使用的LSD算法； 对于大多 数采用LSD提取线 特征的视觉惯性导航算法， 需要采用LBD来跟踪两个连续图像帧 之间的线； 然而LBD获取线的描述子进行匹配的过程是耗时的； 本发 明选用EDLine来提取线 特征不需要参数调整， 它的运行速度比LSD快11倍并且不影响提取精度； 然而， EDLine提取 的线段存在着分段、 短小、 无用等问题， 这将导致线段匹配和跟踪的失败； 为了解决上述问 题， 本发明提出了一种新的线合并算法来进行 特征跟踪， 即改进的EDL ine； (2)合并线； 本发明设计了两个标准来衡量每一条线对的相似性， 并确定匹配的标准； 本发明的方法在以下几个方面得到了改进； 本发明先合并提取的线， 然后再匹配它们； 考虑 到可能的分段线段， 合并它 们有助于提高算法对错 误检测和噪声的鲁棒 性； 第一步： 得到两条线， l1和l2， l1和l2为检测出的2条相近线段； 线段表示为l(s,e)， s表示 线段起点， e表 示线段终点， l1和l2为检测出的2条相近线 段， l3为合并后的线 段； 根据线 段长 度进行筛选， 忽略长度小于阈值TD的线段； l1和l2与x轴的夹角为θ1与θ2， 两条线之间的夹角 为θ12＝abs( θ1‑θ2)； 当θ12小于设定的角度Tθ时可以认为是同一条直线； 经过实验， 本 发明发 现把Tθ设为10°的效果较好； 第二步： 本发明用下面两个标准 来判断是否合并l1和l2； 标准1:端点间的欧氏距离； 将两条线的端点间的欧氏距离定义为dmin＝min{d1,d2,d3, d4}， 式中， d1,d2,d3,d4是l1的端点s1、 e1和l2的端点s2、 e2的之间欧氏距离； 如 果dmin小于设定 的阈值TE时， 则将这两条线合并； 本发明将TE设为10个 像素； 标准2： 端点和线之间的关系； 本发明根据点与线之间的关系设计了标准2， 以处理其它 情况； 如果 l1和l2可以看作是同一行， 则： b1＝t1∈l2&&d11＜TP,b2＝t2∈l2&&d12＜TP b3＝t3∈l1&&d21＜TP,b4＝t4∈l1&&d22＜TP； b1||b2||b3||b4＝＝TRUE 其中 代表{s1,e1}与l2之间的欧氏距离， 具有垂直点t1和t2， 如图所示； 代表{s2,e2}与l1之间的欧氏距离， 具有垂直点t3和t4； TP是一个被设为5个像素的预定义阈权　利　要　求　书 2/5 页 3 CN 114754768 A 3