[发明专利]基于双目摄像头与二维激光雷达的移动机器人导航方法

权利要求书

1.基于双目摄像头与二维激光雷达的移动机器人导航方法，其特征在于，包括以下步骤：

1)建立二维栅格地图模型：建立以二维栅格地图作为环境障碍信息的环境地图模型，建立世界坐标系与地图坐标系的转换关系；

2)建立移动机器人位姿模型：建立以移动机器人中心为原点的机器人坐标系，建立二维坐标平面上的机器人位姿模型，并以坐标系转换关系的形式表示机器人在环境中的位姿；

3)建立激光雷达数据模型：根据二维激光雷达的安放位置与朝向，建立激光雷达数据模型，并根据步骤1)建立的地图模型和步骤2)建立的机器人位姿模型，以及激光雷达的数据协议，实现激光雷达对环境距离的测量并将测量到的障碍物数据映射到环境地图当中；

所述激光雷达数据模型形式如下：

二维激光雷达能够扫描一个平面内360度范围内的障碍信息，二维激光雷达扫描范围可达8米，角度分辨率为1度，在二维激光雷达的数据模型中，每次传输数据将传输360个浮点数据，从激光雷达的0度方向开始，逆时针方向传输每1角方向上的障碍距离激光雷达中心的距离，以米为单位，并且记录i度方向上的障碍物距离雷达中心距离为ρ_i，当二维激光雷达中心安装在机器人坐标系中的(0.08,0)处，激光雷达的0度方向与机器人坐标系的y轴方向平行且朝向相同时，当机器人处于位姿状态Pose(x,y,θ)时，雷达数据ρ_i映射到世界坐标系中坐标；

并且将雷达数据ρ_i最终映射到地图坐标系中的坐标，

其中，height代表地图图片行坐标，width代表地图图片列坐标；

4)建立双目摄像头与激光雷达的联合标定模型：使用三角形标定法，实现对双目摄像头与激光雷达相对位置和朝向的确定，并建立双目视觉系统坐标系与激光雷达坐标系的转换模型，将步骤3)所述的激光雷达数据与双目摄像头探测到的障碍信息融合到同一坐标系进行表示；

使用三角形标定法的双目摄像头与激光雷达的联合标定模型，如下：

根据双目视觉系统的定义，以双目摄像头中的左摄像头的光心为原点，光轴方向为z轴，基线方向从左向右为x轴，以米为单位，建立右手空间直角坐标系，称为双目坐标系，在空间中有一个任意形状的三角形，三角形的三个顶点(X1,Y1,Z1)，(X2,Y2,Z2)，(X3,Y3,Z3)确定一个平面，该平面与激光雷达的探测平面相交，在三角形内产生一条交线，该交线两端对应了激光雷达的两个数据，记为ρ_i，ρ_j，记激光雷达原点与交线两端连线的夹角δ＝j-i，通过三角形标定法得到激光雷达中心在双目坐标系中的位置坐标(dx,dy,dz)，以及激光雷达的0度角到双目坐标系的x轴的夹角β，方法如下：

交线长度设交线两端的点坐标分别为(PiX,dy,PiZ)，(PjX,dy,PjZ)；设比例系数K满足：

则(PjX,dy,PjZ)满足：

其中而点(dx,dy,dz)坐标能够通过解三角形求得；

其中，激光雷达在双目视觉系统中的相对朝向

对于双目视觉系统检测到的障碍物，如果该障碍物满足低于移动机器人的高度，则通过双目视觉系统计算得出该障碍物在双目坐标系当中的坐标(X,Y,Z)，根据双目摄像头与激光雷达的相对位置能够将双目摄像头探测到的障碍物坐标映射到激光雷达模型中进行表示，

其中，γ代表该障碍在激光雷达模型中的角度信息，ρ代表在该角度上的距离信息，通过比较角度γ上激光雷达检测到的距离信息ρ_γ，取ρ与ρ_γ的较小值为当前角度γ上的障碍距离值；

5)利用Dijkstra算法和A-Star算法进行路径规划与避障导航：采用步骤1)所使用的环境地图与步骤4)中的双目摄像头和激光雷达融合的机器人周边的局部环境障碍数据，使用Dijkstra算法和A-Star算法计算机器人到达目标点的导航路径线，使得移动机器人能够沿着该路径线运动到达目标点并且能避开双目摄像头和激光雷达探测到的障碍物；

进行路径规划与避障导航，采用Dijkstra最短路径算法与启发式搜索A-Star优化完成路径规划，使用二维栅格地图表示全局环境的障碍物信息，使用双目摄像头和二维激光雷达同时探测障碍物，并且融合二者测量到的障碍物距离信息作为局部环境障碍物信息，将局部环境障碍信息映射到环境地图当中，将机器人周边5米范围内的全局障碍信息使用局部障碍信息替换，在替换后的地图中根据机器人半径大小，在障碍物外围添加“禁区”，在“禁区”外围设置一个半径大小的安全距离，在安全距离范围内的像素赋予安全距离权值，安全距离范围内的像素的安全距离权值按照0到255线性均匀变化，对于每一个非“禁区”且非障碍物像素，设置与其8-连通方向相邻像素的边权值为像素之间的欧几里得距离，而相邻像素之间的实际权值模型如下：

COST(P₁,P₂)＝α*dist(P₁,P₂)+β*safty(P₂)

其中，COST(P₁,P₂)即是相邻的像素P₁与P₂的综合考虑距离与安全因素的权值，dist(P₁,P₂)是相邻像素P₁与P₂的欧几里得距离，safty(P₂)是像素P₂的安全距离权值，α,β是比重参数，α＝1,β＝0.02；

所述的Dijkstra最短路径算法与启发式搜索A-Star优化，如下：

定义地图图片中非障碍且非“禁区”像素为点集，点集中所有8-连通相邻的像素之间的COST(P₁,P₂)权值为带权边集，机器人当前位置所在像素为起点，用户指定的目标点为终点，使用Dijkstra算法寻找一条起点到终点权值和最低的路径，作为该次导航的路径线，其中，Dijkstra使用小根堆进行优化，而启发式搜索A-Star用到的启发式函数h(x)定义为像素x到目的地像素的欧几里得距离。