深度视觉相机与激光雷达融合的导航方法、系统及设备与流程

1.本发明涉及一种将深度视觉相机与激光雷达相融合的导航方法、实现该方法的系统、装配该系统的设备。


背景技术：

2.目前各类工业机器人、服务型机器人已经广泛应用于社会生产、生活的各个方面，为生产生活带来了极大的便捷。其中应用最广泛的是应用于各类搬运、巡检、清扫等应用场景的机器人设备。
3.现有的机器人通过搭载大量激光传感器、红外传感器、超声波传感器、碰撞传感器等辅助设备进行目标定位、物体识别、距离感知等功能，造成机器人价格高昂，体积庞大等缺点。另外现有机器人的导航算法在在某些场景会发生偏差，导致导航不准确。


技术实现要素：

4.有鉴于此，本发明提供一种深度视觉相机与激光雷达融合的导航方法、系统及设备，采用深度视觉相机和激光雷达作为导航传感器并融合二者的导航算法，减小传感器数量和体积，提高导航准确率。
5.为解决以上技术问题，本发明提供一种深度视觉相机与激光雷达融合的导航方法，包括：s1利用深度视觉相机和激光雷达获取信息，并根据该信息生成地图；s2在生成的地图中对自身即时位置进行定位；s3规划从自身即时位置到目的地的路径。
6.作为一种改进，步骤s1包括：
7.s11设定粒子数量，并通过激光雷达将粒子平均分布在规划区域内；s12根据状态转移方程对每个粒子进行状态估计，从而产生与之对应的预测粒子；s13将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加获得更精确的预测粒子，并对预测粒子进行评价并赋予相应的权重，权重大小和其与真实值的差距成反比，即与真实值的差距越小权重越大；s14对预测粒子进行筛选，保留权重大于设定值的预测粒子，部分保留权重小于设定值的预测粒子，并添加新的预测粒子使得预测粒子的总量不变；s16选取最优预测粒子，利用激光雷达采集的信息构建2d地图，并在按照2d地图运动的过程中利用深度视觉相机采集的信息构建3d地图。
8.作为一种进一步的改进，所述步骤s12中状态转移方程为：
9.p(:,i)＝p(:,i)+distance*[
‑
cos(k*theta)；sin(k*theta)]+wgn(2,1,10*log10(q))i
[0010]
dist＝norm(p(:,i)
‑
z(:,k))ii
[0011]
w(i)＝(1/sqrt(r)/sqrt(2*pi))*exp(
‑
(dist)^2/2/r)iii
[0012]
p(:,i)为粒子群；theta＝pi/t表示旋转角度(t为测量时间)；k为当前时间；q为过程噪声；wgn为高斯白噪声计算函数；norm函数，在向量空间里对向量赋予长度和大小；r为测量噪声；sqrt求平方根函数；exp指数函数。
[0013]
作为另一种更进一步的改进，步骤s13中，将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加包括：s131对激光雷达采集的数据进行分析，得到规划区域中障碍物的距离信息；s132对深度视觉相机采集的数据进行分析，得到规划区域中障碍物的旋转角度信息；s133融合距离信息和旋转角度信息。
[0014]
作为一种改进，所述步骤s133包括：
[0015]
设深度视觉相机获取的旋转角度信息为z
v
，激光雷达获取的距离信息为z
l
；若(z
il
,z
jv
)可配对，则
[0016][0017]
式中：
[0018]
从公式ix中可以获得其协方差矩阵：
[0019]
e
ij
＝b
ij
e
il
b
tij
+c
ij
e
jv
c
tij
v
[0020]
式中：e
il
,e
jv
分别是z
il
,z
jv
的协方差矩阵；
[0021]
根据z
il
,z
jv
之间的马氏距离的x2验证进行相容性判断：
[0022][0023]
式中：d＝rank(hij),α为可信度；
[0024]
当出现l
it
与v
t
中有多个特征相匹配时，则选取与其距离最近的特征作为最佳匹配；
[0025]
通过基本的扩展卡尔曼滤波进行信息融合：
[0026][0027][0028][0029]
作为一种改进，还包括滤波步骤s15，所述滤波步骤s15包括：循环执行步骤s12、步骤s13、步骤s14。
[0030]
作为一种改进，所述步骤s2采用amcl来实现。
[0031]
作为一种改进，所述步骤s3采用move_base来实现。
[0032]
本发明还提供一种导航系统，包括深度视觉相机和激光雷达，并采用上述导航方法进行导航。
[0033]
本发明还提供一种智能设备，装配有上述导航系统。
[0034]
本发明的有益之处在于：深度视觉可以比激光雷达更准确的得到物体轮廓、大小、与其他物体的相对位置等信息，但无法精确得到距离信息。激光雷达可以精确得到距离信息，但无法精确得到物体轮廓、与其他物体相对位置等信息。将两种传感器所得到的信息进行汇总，以激光雷达数据为主，辅助增加深度视觉摄像头提供的轮廓、大小、相对位置等信息后，可以在粒子权重估算方面得到更准确的权重值。
[0035]
在综合建图方面，最终在服务器端呈现的地图样本将为叠加了深度视觉数据与激光雷达点云数据的地图，可更直观的用于其他应用。
附图说明
[0036]
图1为发明中导航方法的流程图。
[0037]
图2为导航方法中生成地图的流程图。
[0038]
图3为粒子示意图。
具体实施方式
[0039]
为了使本领域的技术人员更好地理解本发明的技术方案，下面结合具体实施方式对本发明作进一步的详细说明。
[0040]
如图1所示，本发明提供一种深度视觉相机与激光雷达融合的导航方法，包括以下步骤：
[0041]
s1利用深度视觉相机和激光雷达获取信息，并根据该信息生成地图；
[0042]
s2在生成的地图中对自身即时位置进行定位；机器人在导航的过程中需要时刻确定自身当前的位置，本实施例中采用的amcl(自适应蒙特卡洛定位)包。amcl是一种概率定位系统，以2d方式对移动机器人定位，它实现了自适应(或者kld
‑
采样)蒙特卡洛定位法，使用粒子滤波跟踪机器人在已知地图中的位姿。下面的图片显示用里程计和amcl定位的不同之处，amcl估计base结构(机器人)相当于global结构(世界地图)tf转换(ros中的坐标系转换)。从本质上，这种转换利用航位推算来处理漂移，所发布的转换是远期的。
[0043]
s3规划从自身即时位置到目的地的路径。本实施例中，路径导航部分使用move_base包，move_base能够获取机器人周围信息(如激光雷达扫描结果)并生成全局与局部的代价地图，根据这些代价地图可以使机器人绕开障碍物安全到达指定的位置。move_base的路径规划主要分为全局规划和局部规划，分别采用的是a*算法和dwa(dynamicwindowapproach))算法。move_base包是ros机器人操作系统环境下关于机器人路径规划的中心枢纽。move_base订阅了激光雷达、地图、amcl(自适应蒙特卡洛定位)的定位等数据，然后规划出全局和局部路径，再将路径转化为机器人的速度信息，最终实现机器人自主导航。move_base提供了ros导航的配置、运行、交互接口，它主要包括两个部分：(1)全局路径规划(globalplanner)：根据给定的目标位置进行总体路径的规划；(2)本地实时规划(localplanner)：根据附近的障碍物进行躲避路线规划。
[0044]
其中，如图2所示，步骤s1又包括：
[0045]
s11设定粒子数量，并通过激光雷达将粒子平均分布在规划区域内；本发明中所述的“粒子”见步骤s12中解释。
[0046]
s12根据状态转移方程对每个粒子进行状态估计，从而产生与之对应的预测粒子；状态转移方程为：
[0047]
p(:,i)＝p(:,i)+distance*[
‑
cos(k*theta)；sin(k*theta)]+wgn(2,1,10*log10(q))i
[0048]
dist＝norm(p(:,i)
‑
z(:,k))ii
[0049]
w(i)＝(1/sqrt(r)/sqrt(2*pi))*exp(
‑
(dist)^2/2/r)iii
[0050]
p(:,i)为粒子群；theta＝pi/t表示旋转角度(t为测量时间)；k为当前时间；q为过程噪声；wgn为高斯白噪声计算函数；norm函数，在向量空间里对向量赋予长度和大小；r为测量噪声；sqrt求平方根函数；exp指数函数。
[0051]
如图3所示，实心圆表示移动机器人真实的信息情况及粒子,空心圆表示估计信息情况及预测粒子。箭头表示估计信息情况；x
t
表示t时刻的实际位姿向量,则x
1:t
表示机器人的轨迹；z
t,k
表示t时刻对第k个路标的观测,则z
1:t
表示观测信息；u
t
表示移动机器人的控制信息,使其从x
t
‑1到达位姿x
t
,则u
1:t
表示控制的历史信息；m
k
表示第k个路标的位置状态向量,则m表示地图。
[0052]
s13将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加获得更精确的预测粒子，并对预测粒子进行评价并赋予相应的权重，权重大小和其与真实值的差距成反比，即与真实值的差距越小权重越大；在这步骤中，需要对预测粒子进行评价，越接近于真实状态的粒子，其权重越大，反之，与真实值相差较大的粒子，其权重越小。此步骤是为下一步骤重采样做准备。激光雷达和深度视觉相机会返回周围位置信息，如果这些信息与期望值相差较大，亦或者在运动中某些粒子本应该没有碰到障碍或者边界，然而在运算中却到达甚至穿过了障碍点或边界，那么这种粒子就是坏点粒子，这样的粒子权重也就比较低一些。将上一步中上一时刻每个粒子的预测状态估计与该时刻实际观测的粒子状态进行比对，如果差值越小，说明上一时刻的预测越准确，权重越大，反之差值越大，说明上一时刻的预测约不精准，权重越小。
[0053]
s14对预测粒子进行筛选，保留权重大于设定值的预测粒子，部分保留权重小于设定值的预测粒子，并添加新的预测粒子使得预测粒子的总量不变；权重小粒子存在两种情况，一种是坏点，就是采集错误；另一种是不重要点，比如对机器人运动轨迹不会造成很大影响的点。但由于无法分辨哪些是坏点哪些是不重要点，所以采取的措施就是删除一部分权重小的点，最终通过下一步滤波将坏点尽可能的清除。
[0054]
s15滤波：循环执行步骤s12、步骤s13、步骤s14。将步骤s14中重采样后的粒子带入状态转移方程得到新的预测粒子，然后将它们继续进行上述步骤s13、步骤s14过程，经过这种循环迭代，最终绝大部分粒子会聚集在与真实值最接近的区域内，从而得到机器人准确的位置。
[0055]
s16选取最优预测粒子，利用激光雷达采集的信息构建2d地图，并在按照2d地图运动的过程中利用深度视觉相机采集的信息构建3d地图。
[0056]
步骤s13中将深度视觉相机采集到的信息与激光雷达采集到的信息进行叠加的步骤具体包括：
[0057]
s131对激光雷达采集的数据进行分析，得到规划区域中障碍物的距离信息；
[0058]
s132对深度视觉相机采集的数据进行分析，得到规划区域中障碍物的旋转角度信息；
[0059]
s133融合距离信息和旋转角度信息。设深度视觉相机获取的旋转角度信息为z
v
，激光雷达获取的距离信息为z
l
；若(z
il
,z
jv
)可配对，则
[0060]
[0061]
式中：
[0062]
从公式ix中可以获得其协方差矩阵：
[0063]
e
ij
＝b
ij
e
il
b
tij
+c
ij
e
jv
c
tij
v
[0064]
式中：e
il
,e
jv
分别是z
il
,z
jv
的协方差矩阵；
[0065]
根据z
il
,z
jv
之间的马氏距离的x2验证进行相容性判断：
[0066][0067]
式中：d＝rank(hij),α为可信度；
[0068]
当出现l
it
与v
t
中有多个特征相匹配时，则选取与其距离最近的特征作为最佳匹配；
[0069]
通过基本的扩展卡尔曼滤波进行信息融合：
[0070][0071][0072][0073]
本发明还提供一种导航系统，包括深度视觉相机和激光雷达，并采用上述导航方法进行导航。
[0074]
本发明还提供一种智能设备，装配有上述导航系统。
[0075]
深度视觉相机主要作用是通过红外结构光或time
‑
of
‑
flight原理，直接测出图像中各像素离相机的距离，但深度视觉相机对光线的依赖程度较高，所以在该机器人产品开发过程中，使用深度视觉相机+激光雷达的两种传感器同时获得图像位置、距离信息，将两个传感器获得的位置信息进行融合比对，后端根据参照物和预先配置的配置文件情况，对目标物体位置进行精确测距和定位，保障导航路径规划和行驶路径更加精确。
[0076]
以上仅是本发明的优选实施方式，应当指出的是，上述优选实施方式不应视为对本发明的限制，本发明的保护范围应当以权利要求所限定的范围为准。对于本技术领域的普通技术人员来说，在不脱离本发明的精神和范围内，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。