一种手持移动平台的光机电自主定位方法与流程

1.本发明属于导航定位技术领域，特别是指一种手持移动平台的光机电自主定位方法。


背景技术：

2.多模态传感器融合是自主导航系统在现实环境中成功运行需具备的关键能力，这一技术从地下自主勘探到室外动态测绘都得到了广泛应用。激光雷达硬件的发展促进了激光雷达-惯导融合的研究，激光雷达传感器广泛的视场、密度、范围和精度使得其适合于导航、定位和测绘任务。然而在复杂的环境中，如长隧道或开阔空间，单独使用雷达可能会失败，另外单独使用imu不能提供超过几秒钟的可靠姿态估计。为了解决这些问题，还需要与更多的传感器，特别是相机进行融合。虽然过去已经通过松耦合方法实现了视觉-惯导-激光雷达的融合，但像增量平滑这样的紧耦合方法由于其优越的鲁棒性而更具有实用性。在平滑方法方面，视觉惯性导航系统(vins)的研究已经成熟，激光雷达惯性导航系统的研究也越来越普遍。然而，三种传感器的紧密融合仍然是一个有待解决的问题。imu、激光雷达和相机传感器融合的两个主要挑战是：1)在移动平台有限的计算预算下实现实时性；2)在不同频率和采集方法下实现三种信号的适当同步。


技术实现要素：

3.本发明的目的在于提供一种手持移动平台的光机电自主定位方法，本方法可面向剧烈运动、突变光照等极端环境下的应用场景，实现激光雷达-视觉-惯导间的紧耦合协同定位。
4.为了实现上述目的，本发明采用的技术方案为：
5.一种手持移动平台的光机电自主定位方法，所述手持移动平台装备有imu、激光雷达和一个具有低延迟和全传感器率的单目或双目摄像机；该方法包括以下步骤：
6.(1)将所有通过雷达获得的点特征投影到图像平面上，若被投影的点与图像平面上的特征点(u
l
，v
l
)的位置小于等于3个像素，则残差表示为：
[0007][0008]
式中，表示在状态xi时，双目地标m
l
的残差，ti为手持移动平台姿态矩阵，x
l
为被投影点，下标i表示第i时刻，下标l表示第l个点；
[0009]
(2)若不能将激光雷达深度信息与视觉特征联系起来，或者深度信息是不稳定的，则通过双目视觉立体匹配获得深度信息，此时残差表示为：
[0010]
[0011]
其中，π表示投影函数，下标u和v表示投影方向，上标l代表左相机，上标r代表右相机，u
i,l
表示第i时刻第l个点的横坐标；只用到单目相机时，上式中只用到第一个和最后一个元素；
[0012]
(3)当跟踪单目视觉特征时，使用激光雷达的重叠视场提供深度估计；为了匹配不同频率的激光雷达和相机的测量信息，从激光雷达点云中提取特征基元，同时表示为一个相机帧，从而一次对所有传感器执行优化。
[0013]
进一步地，步骤(3)的具体方式为：
[0014]
(301)当imu和相机样本被瞬间捕获时，激光雷达在其内部围绕z轴旋转时不断捕获点，完成一个完整的旋转后，积累的激光信息转换成一个点云，随即开始新的扫描；
[0015]
(302)在点云未失真的情况下，将点分割成簇，少于5个点的簇被标记为离群值并丢弃；然后，计算预滤波云中各点的局部曲率，将曲率最低和曲率最高的点分别赋给平面候选和直线候选集合；
[0016]
(303)在各自的候选集合中跟踪平面和直线；在预测位置附近，跟踪采用局部视觉特征跟踪的方式，具体方式为：首先对点云进行欧几里得聚类划分，对平面进行法线区域增长划分，然后在每个聚类中检测新的地标；
[0017]
跟踪完成后，在剩余的候选云中检测新的地标。
[0018]
本发明与现有技术相比所取得的有益效果为：
[0019]
1、本发明解决了传感器的退化问题——激光雷达特征跟踪和视觉跟踪之间的混合取决于特定环境中的约束。
[0020]
2、在极端条件如剧烈运动或光照突变时，本发明的三种传感器紧耦合方式具有更好的性能。
附图说明
[0021]
图1是本发明实例中手持移动平台的光机电自主定位方法的流程图。
具体实施方式
[0022]
以下结合附图和具体实施方式对本发明做进一步的说明。
[0023]
一种手持移动平台的光机电自主定位方法，包括以下步骤：
[0024]
(1)首先将所有通过雷达获得的点特征投影到图像平面上。如果被投影的点与图像平面上的特征点的位置接近3个像素，则残差可如下式计算：
[0025][0026]
(2)如果不能将激光雷达深度信息与视觉特征联系起来，或者深度信息是不稳定的(例如，由于动态障碍物或噪声导致帧间深度信息发生大于0.5m的变化时)，则恢复到通过双目视觉立体匹配的方式获得深度信息。在状态xi时，双目地标m
l
的残差为：
[0027]
[0028]
式中，(u
l
，v)，(ur，v)是被检测地标的像素位置。如果只用到单目相机，则上式中只需要用到第一个和最后一个元素；
[0029]
(3)当跟踪单目视觉特征时，使用激光雷达的重叠视场提供深度估计。为了匹配不同频率的激光雷达和相机的测量信息，从激光雷达点云中提取特征基元，同时表示为一个相机帧，这样可以一次对所有传感器执行优化。处理过程包括点云不失真和同步、滤波、原始提取和跟踪；具体步骤为：
[0030]
(301)当imu和相机样本被瞬间捕获时，激光雷达在其内部围绕z轴旋转时不断捕获点，完成一个完整的旋转后，积累的激光信息转换成一个点云，随即开始新的扫描。
[0031]
由于激光是在运动时捕获的，点云需要在运动之前保持不失真，并与唯一的任意时间戳相关联——通常是扫描的开始。这种方法意味着相机和激光雷达的测量有不同的时间戳，因此图节点是分开的。本实施例选择在扫描开始后将激光雷达测量还原到最近的相机时间戳。不仅保证了在图形优化中添加一致数量的新节点和因子，而且保证了imu、摄像头和激光雷达输入之间的优化是联合执行的。同时也确保了固定的输出频率，即摄像机帧率或激光雷达帧率(当摄像机不可用时)；
[0032]
(302)在点云未失真的情况下，将点分割成簇。其中，小簇(少于5个点)被标记为离群值并丢弃，因为可能有噪声。然后，计算预滤波云中各点的局部曲率。将曲率最低和曲率最高的点分别赋给平面候选和直线候选集合。分割和基于曲率的滤波通常会将点云中的点数量减少90％，为后续的平面和直线处理节省算力；
[0033]
(303)然后，在各自的候选集中跟踪平面和直线。在其预测位置附近跟踪特征采用局部视觉特征跟踪的方式，跟踪完成后在剩余的候选云中检测新的地标。具体方式为，首先对点云进行欧几里得聚类划分，对平面进行法线区域增长划分，然后使用相同的方法在每个聚类中检测新的地标。
[0034]
该方法提出了一种新的因子图公式用于手持移动平台上视觉、激光雷达和imu紧耦合测量；并提出了一种高效的激光雷达特征提取方法，激光雷达和视觉特征均用点、线、面特征统一表示。
[0035]
以下为一个更具体的例子：
[0036]
图1所示为一种手持移动平台的光机电自主定位方法的流程图。本方法可在长走廊和黑暗环境等困难环境中实现自主定位，这里以估计一个移动平台(手持传感器负载)的位置、方向和线速度为例，该平台装备有imu、激光雷达和一个具有低延迟和全传感器率的单目或双目摄像机。该方法的详细步骤如下：
[0037]
(1)首先将所有通过雷达获得的点特征投影到单目相机的图像平面上。如果被投影的点与单目相机图像平面上的特征点的位置接近3个像素，残差可如下式计算：
[0038][0039]
(2)当环境中存在动态障碍物或噪声导致帧间深度信息发生大于0.5m的变化导致深度信息不稳定时，将通过双目视觉获得深度信息。在状态xi时，双目地标m
l
的残差为：
[0040][0041]
式中，(u
l
，v)，(ur，v)是被检测地标的像素位置。如果只用到单目相机，上式中只需要用到第一个和最后一个元素；
[0042]
(3)当跟踪单目视觉特征时，使用激光雷达的重叠视场提供深度估计。为了匹配不同频率的激光雷达和相机的测量信息，从激光雷达点云中提取特征基元，同时表示为一个相机帧。处理过程包括以下步骤：点云不失真和同步、滤波、原始提取和跟踪；
[0043]
(301)当imu和相机样本被瞬间捕获时，激光雷达在其内部围绕z轴旋转时不断捕获点，完成一个完整的旋转后，积累的激光信息转换成一个点云，随即开始新的扫描。
[0044]
由于激光是在运动时捕获的，点云需要在运动之前保持不失真，并与唯一的任意时间戳相关联——通常是扫描的开始。这种方法意味着相机和激光雷达的测量有不同的时间戳，因此图节点是分开的。本实施例选择在扫描开始后将激光雷达测量还原到最近的相机时间戳。不仅保证了在图形优化中添加一致数量的新节点和因子，而且保证了imu、摄像头和激光雷达输入之间的优化是联合执行的。同时也确保了固定的输出频率，即摄像机帧率或激光雷达帧率(当摄像机不可用时)；
[0045]
(302)在点云未失真的情况下，将点分割成簇。小簇(少于5个点)被标记为离群值并丢弃，因为可能有噪声。然后，计算预滤波云中各点的局部曲率。将曲率最低和曲率最高的点分别赋给平面候选和直线候选集合。分割和基于曲率的滤波通常会将点云中的点数量减少90％，为后续的平面和直线处理节省算力；
[0046]
(303)然后在各自的候选集中跟踪平面和直线。在其预测位置附近跟踪特征采用局部视觉特征跟踪的方式，跟踪完成后在剩余的候选云中检测新的地标。首先对点云进行欧几里得聚类划分，对平面进行法线区域增长划分，然后使用相同的方法在每个聚类中检测新的地标。
[0047]
总之，本发明解决了传感器的退化问题——激光雷达特征跟踪和视觉跟踪之间的混合取决于特定环境中的约束。在极端条件如剧烈运动或光照突变时，三种传感器紧耦合具有更好的性能。本发明还提出了在同一因子图中联合优化激光雷达和视觉特征的新方法，可在长走廊和黑暗环境等困难环境中进行鲁棒估计。