一种基于视觉的点云处理避障方法与流程

1.本发明属于机器人定位技术领域，特别涉及一种基于视觉的点云处理避障方法。


背景技术：

2.移动机器人是通过视觉信息来识别周围环境的障碍物并且获得它的定位信息，从而进行移动机器人的避障策略选择。获得障碍物较全面的地图信息对于障碍物的定位坐确定是非常重要的。伴随着激光雷达，相机等传感器技术的不断发展，机载算力的不断提升以及多学科基础理论的不断完善，一种称为同时定位与建图(simultaneous localization and mapping,slam)的理论研究应运而生，它解决了机器人在上述环境中运行，“我在哪儿”，“我周围是什么”的问题。slam根据传感器类型的不同，分为激光式和视觉式。激光式是通过激光雷达获取环境空间点云信息进行感知计算。激光雷达分为2d激光雷达和3d激光雷达。2d激光雷达仅能采集到一个平面内的环境信息，在复杂的环境无法实现完整的建图。并且2d激光雷达的扫描范围单一。3d激光雷达采集信息丰富，但价格昂贵且算法复杂。激光雷达导致构建的地图存在信息不完整、精度较差的缺点。视觉式是通过相机获取环境的纹理信息，但传统的rgb相机通常只能获得多通道的二维图像纹理信息。


技术实现要素：

3.本发明针对上述现有技术的存在的问题，提供一种基于视觉的点云处理避障方法。
4.本发明通过以下技术手段实现解决上述技术问题的：
5.深度相机能够同时获得rgb图像和深度图像。深度相机相对容易获取彩色图像上每个像素的深度数据，并且具有性价比高、获取环境信息丰富、能耗低等优点。视觉slam将机器人收集到的视觉信息通过前端图像特征提取实时转化为点云的环境地图信息。通过分析slam输出的实时地图信息，计算出有利于准确构建地图的运动策略。最后利用输出的地图、定位以及路径信息，实现机器人在未知环境中的无碰撞安全运行。
6.一种基于视觉的点云处理避障方法，该方法为：
7.通过深度相机将三维世界的坐标点投影到相机的二维成像平面上，形成二维点云；
8.基于坐标系转换矩阵，将二维点云中点转换至世界坐标系下，进行不同二维点云的点云拼接，并生成点云地图；
9.通过体素建立占据网格地图，并以特定值标记更新网格地图中的栅格表征障碍区域，以实现避障处理。
10.进一步的，所述三维世界的坐标点p＝[x,y,z]
t
与二维成像平面上对应的坐标点p'＝[x',y',z']
t
通过下式(1)转换：
[0011][0012]
式(1)中，p
uv
为p'在像素坐标系下的坐标，二维成像平面上像素坐标系为o-u-v，k为相机的内参数矩阵，r、t分别为相机相对于世界坐标系fw的旋转矩阵与平移量，t为相机相对于世界坐标系fw的变换矩阵。
[0013]
进一步的，生成所述点云地图时，对点云地图进行优化，所述优化方法为：
[0014]
过滤深度值超过阈值的点；
[0015]
利用统计滤波器过滤掉孤立的点；
[0016]
利用体素滤波器进行降采样。
[0017]
进一步的，根据下式(2)更新网格地图中栅格占据概率，
[0018][0019]
式(2)中，和分别代表占据和未被占据栅格单元的先验概率，初始值为0.5；代表占据栅格单元状态的似然估计；po代表占据栅格单元的后验概率。
[0020]
进一步的，用网格区域的灰度值表示网格占据状态，预设占据概率阈值设定为to＝0.5。
[0021]
本发明的有益效果为：在低成本的前提下得到环境信息丰富、精度高的环境地图，通过深度相机转化的点云地图信息，使移动机器人能够对障碍物进行识别与定位并且实现自主高效地躲避障碍物。
附图说明
[0022]
图1为本发明基于视觉的点云处理避障方法的流程图；
[0023]
图2为相似三角形原理图；
[0024]
图3为小孔成像模型图；
[0025]
图4为机器人在建图过程中三个坐标图；
[0026]
图5为栅格地图中占据概率的示意图；
[0027]
图6为通过rviz建立的地图模型；
[0028]
图7为机器人仿真避障结果。
具体实施方式
[0029]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0030]
实施例
[0031]
参见图1，为本发明基于视觉的点云处理避障方法的流程图。
[0032]
具体方法如下：
[0033]
第一步、前端图像特征提取
[0034]
采用intel realsense d430深度相机将三维世界的坐标点投影到相机的二维成像平面上，这个过程可以用针孔相机模型描述。示意图如图2和3，其中图2为相似三角形原理，图3为小孔成像模型。
[0035]
oxyz为相机坐标系，o为光心，从相机背面往前看，z轴指向相机前方，x轴水平向右，y轴垂直向下；物理成像平面为o'x'y'z'，坐标定义与相机坐标系相似；还有一个像素坐标系o-u-v，o位于图像左上角，u轴水平向右与x轴平行，v轴垂直向下与y轴平行。
[0036]
设定一个空间中的点p＝[x,y,z]
t
,落在成像平面的点为p'＝[x',y',z']
t
，相机焦距为f,通过上图相似三角形的投影关系，有：
[0037][0038]
为了简化，可以理解为成像平面位于相机前方，从而去掉负号：
[0039][0040]
从而有：
[0041][0042]
由于像素坐标系与成像坐标系之间相差一个平移和一个缩放，原点平移了[c
x
,cy]
t
，u轴与v轴分别缩放了α倍、β倍，于是p'在像素坐标系下的坐标p
uv
为：
[0043][0044]
进一步可以得到：
[0045][0046]
其中f
x
＝αf,fy＝βf，使用矩阵形式：
[0047][0048]
k为相机的内参数矩阵，相机的位姿为：
[0049][0050]
其中r、t分别为相机相对于世界坐标系的旋转矩阵与平移量，t为相机相对于世界
坐标系的变换矩阵。相机经过去畸变处理后，可以投影更为准确的图像。
[0051]
第二步、轮式机器人坐标轴定义
[0052]
如图4所示，机器人在建图过程中主要定义三个坐标：世界坐标系fw，底盘坐标系fb以及相机坐标系fc，其中fb与fc根据关键帧的不同，存在多个时刻值。坐标轴之间由位姿变换矩阵t进行表示，是视觉slam计算的t时刻的相机位姿，ctb是相机到底盘的安装位置参数，也是在线估计参数，则是由轮式里程计所获取的盘底运动的测量值。
[0053]
第三步、点云地图创建
[0054]
视觉slam可输出多帧图像机器人坐标系相对世界坐标系的位姿变换同时每一帧还附带了多个t时刻机器人坐标系下的地图点点云地图的创建即是使用将所有的转换到世界坐标系fw下，达到点云拼接的作用，最终生成所需要的稠密点云地图，既可用于机器人导航。
[0055]
第四步、优化点云地图
[0056]
对于机器人避障来说，更加注重局部的、动态的障碍物处理，必须要知道某个特征点是否为障碍物，然而只有少数的离散的特征点是无法判断的。因此如果想要使用slam构建的点云地图进行导航，必须对地图进行优化。可以使用三角网格、面片来进行地图重建，生成具有物体表面信息的更为精细的地图。通过体素建立占据网格地图，将地图空间的某一方块填充占据，一次构建可以用于导航的网格地图。步骤如下：
[0057]
(1)在生成点云图时，过滤深度值太大的点，即无效的点。主要是考虑到深度相机存在有效量程，超过了有效量程的点肯定是无效的点；
[0058]
(2)利用统计滤波器过滤掉孤立的点。该滤波器统计每个点与距离它最近n个点的距离值分布，去除距离均值过大的点，保留了“抱在一团”的点。这样就被去除了的孤立的点很有可能就是噪声的点；
[0059]
(3)利用体素滤波器进行降采样。由于slam建图阶段，多个视角存在视野重叠，在重叠区域内保留大量位置十分相近的点，不仅占用空间，而且对导航产生不利影响。体素滤波器则保证了某个一定大小的立方体中仅有一个点，相当于进行了降采样。
[0060]
第五步、二维栅格地图的表示
[0061]
贝叶斯递推公式是基于贝叶斯定理的后验概率统计数据融合的算法。假定一个状态空间，k时刻的概率为xk，已知k组测量zk＝{z1,...,zk}，计算后验概率分布的公式如下：
[0062][0063]
其中p(zk|xk)为似然函数；p(xk|z
k-1
)为先验分布函数；p(zk|z
k-1
)为归一化概率密度函数。
[0064]
设定o表示观测到栅格被占据，表示观测到栅格未被占据，e表示障碍物真实存在，表示障碍物不存在，可以得到后验概率：
[0065]
[0066][0067]
其中p(e)表示先验概率；p(o|e)与表示测量模型；同时有在更新的栅格地图占据概率时可简得到：
[0068][0069]
其中和分别代表占据和未被占据栅格单元的先验概率，初始值为0.5；代表占据栅格单元状态的似然估计；po代表一个传感器测量该栅格单元的更新占据概率值，即此占据栅格单元的后验概率。对网格的占据概率进行更新，用网格区域的灰度值表示网格占据状态，取(200，0，-1)，分别表示占据、空闲、不确定；一般占据概率阈值设定为to＝0.5
[0070]
参见图5，为栅格地图的表示与更新，黑色表示占据；灰色表示空闲；白色表示不确定。
[0071]
第六步、利用rviz进行建图及模拟定位计算，参见图6，为通过rviz建立的地图模型。
[0072]
第七步、利用amcl进行定位
[0073]
amcl是机器人在2d中移动的概率定位系统。它实现了自适应蒙特卡罗定位方法，其使用粒子滤波器来针对已知的地图跟踪机器人的位姿。输入深度相机数据、历程及数据，输出机器人在地图中的位姿后就可以实时定位了。
[0074]
第七步、路径规划
[0075]
移动机器人依据行走路线最短指标，在运动空间中找到一条从起始状态到目标状可以避开障碍物的最优路径，路径规划分为全局规划和局部路径规划。全局路径规划是宏观的规划，主要为机器人在运动中提供核心运动点，保证机器人安全到达目的地。局部路径规划为了实现机器人的路线更加合理，它可以对机器人的速度、加速度进行约束。
[0076]
第八步、避障
[0077]
参见图7，在之前建立的地图基础上，让机器人按照指定路线运动，在运动过程中对障碍物进行识别，并实时更新地图。由上图我们可以看出，中间的白色部分为深度相机识别的障碍物，白色部分外部膨胀的包围区域为激光雷达所识别的障碍物，激光雷达所识别的障碍物均被深度相机识别出来了。
[0078]
要说明的是，在本文中，如若存在第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0079]
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例
对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。