专利解读： 一种深度相机与单线激光雷达融合的移动机器人避障方法
发明人：朱威 巫浩奇 洪力栋 韩慧 陈伟锋 何德峰 浙江工业大学
该方法包括以下步骤：
1.对深度相机和单线激光雷达进行联合标定
2.获取同一帧深度图像和同一时刻单线激光雷达数据）
3.深度图像预处理及裁剪
4.判断预处理后的深度图像是否需要与激光雷达进行数据融合
5.使用 贝叶斯估计法 进行栅格地图融合
6.使用局部避障算法进行避障
流程图：
发明特点：弥补了使用单个深度相机只能获得前方视野范围内的区域而不能获得后面和左右两侧的环境数据的缺陷，并且通过对深度图像进行预处理及裁剪，大幅减少了需要计算的深度图像数据量，有效增快了避障的数据处理速度。
详细说明：
（1）所述步骤1中，以棋盘标定法对深度进行标定，得到深度相机的内参矩阵和外参矩阵，按照式a和式b将深度相机坐标系转换到移动机器人本体坐标系：
其中，u、v分别为深度图像中各个橡塑点的行列号，cx、cy分别为深度图像中心坐标，d表示深度图像中各个像素点的深度距离，fx、fy分别为深度相机水平、垂直方向的焦距，R为旋转矩阵，T为转换向量；xc、yc、zc分别为深度相机的三轴坐标；
PS：移动机器人本体坐标与单线激光雷达的坐标系一致。
（2）步骤3中，对深度图形的预处理包括：
1.将步骤2中获得的深度图像使用双边滤波算法进行处理；
2.设经过1滤波处理后的深度图像的有效像素区域为水平像素坐标在[cx‑width ,cx+width]、竖直像素坐标在[cy‑height ,cy+height]的矩形区域，其中cx、cy分别为深度图像中心坐标，width为有效像素区域到中心坐标的最大水平像素距离，height为有效像素区域到中心坐标的最大竖直像素距离；
3.基于有效像素区域和预设的像素深度值范围，对经过1滤波处理后的深度图像进行裁切，输出裁剪后的深度图像。
4.上述3中所预设的像素深度值范围为[10,120]
（3）步骤4中包括以下步骤：
1.将预处理后的深度图像通过深度相机内参矩阵和外参矩阵转换成点云数据，将点云数据通过步骤1的联合标定的方式转换至机器人本体坐标系中，得到坐标系转换后的点云数据P；
2.在三维空间下，对点云数据P使用半径滤波器进行处理，设置点云邻域半径r，对于P中的每个点云点，统计以其为中心、半径为r的球体内的球体内的空间相邻点云点的数量Num1；
若Num1≤邻近点云点数量阈值THnum1，则将当前点云点消除，否则保留该点云点，得到滤除异常空间噪声点云点的点云数据M；
3.统计点云数据M中的点云点个数Num2，若Num2大于等于总点云数阈值THnum2，说明深度相机检测到障碍物，则进行步骤5，否则不进行数据融合，直接使用单线激光雷达数据生成栅格地图并进行步骤6。
（4）r∈[0.5,5]，单位厘米；THnum1∈[2，6]；THnum2∈[2，6]
（5）步骤5包括以下：
1.将步骤4中的3生成的点云数据M中每个点云点都投影到移动机器人坐标系平面上形成二维的伪激光数据，然后再将伪激光点数据生成的栅格地图与单线激光雷达获得的激光数据所生成的栅格地图，使用经典的贝叶斯递推公式来进行融合；
2.设置占据概率阈值，用于确定栅格地图是否被占据；
3.以深度相机的光心为起点，扫描所有角度，以光心和扫描到的障碍物间的栅格为 \bar{O} ，表示观测到栅格未被占据，此区域可行，以0表示观测到栅格被占据；
4.以 E 表示障碍物真实存在， \bar{E} 表示障碍物不存在，基于式（3）和（4），得到后验概率：
其中，P(E)表示先验概率，P(O|E)与P(O| \bar{E} )表示传感器的测量模型，并且
5.当递推公式被转换为占据栅格结构图时，如式（5）为改进的融合公式：
其中， P_{m}^{o} 和 (1-P_{m}^{o}) 分别代表先验地图的栅格占据和未被占据单元的先验概率， P^{o} 表示栅格单元条件概率， P_{S}^{o} 代表传感器通过观测确认栅格是否被占据的条件概率；
6.通过栅格被占据的概率大小来确定栅格地图的生成，被占据的概率越大，生成栅格地图的可能性越大，设置概率阈值后，若计算出的概率大于设定的阈值则可生成栅格地图，小于设定的概率阈值则不生成。相机生成的栅格地图与雷达生成的栅格地图相融合后，新生成的栅格概率补充占据原有的栅格单元的概率。
（6）基于步骤（5）得到的栅格，定义单线激光雷达距离障碍物之间的距离为Distance，单位为米；定义移动机器人与深度相机镜头朝向所在的局部立体空间内障碍物之间的距离为3D_Distance，单位为米；定义移动机器人能够安全移动的情况下周围障碍物距离机器人本体最小距离为Min_Distance；
（7）根据障碍物与移动机器人之间的平面距离Distance和局部立
体空间内障碍物之间的距离3D_Distance将融合后的区域分为：
安全区域：机器人前方无障碍或是障碍物不影响移动机器人正常行走；
障碍区域：机器人前方有障碍或障碍物影响移动机器人正常行走；
碰撞区域：机器人前方有障碍且障碍物影响移动机器人正常行走。
