[发明专利]一种基于双目立体视觉的巡视探测器自主避障规划方法

摘要

一种基于双目立体视觉的巡视探测器自主避障规划方法：生成局部地形的DEM数据；根据双目立体视觉系统的视场范围构建栅格地图，对地图上每个栅格进行DEM数据统计，并对DEM进行去噪处理；进行地形可通过性分析，生成适宜度地图；利用前后两次规划时生成的适宜度地图，进行数据融合，得到融合后的用于避障规划的适宜度地图；在适宜度地图上放置若干条备选路径弧；对每一条备选路径弧，从到目标点的距离和安全避障能力两个方面进行评价；从所有备选路径弧中筛选出评价值高于设定阈值的备选路径弧，并从中选择评价值最高的路径弧作为本次避障规划的结果。本发明简单完备、安全性高，规划效率高，易于工程实现，可用于复杂地形上的巡视探测任务。

主权项

1.一种基于双目立体视觉的巡视探测器自主避障规划方法，其特征在于实现步骤如下：(1)生成局部地形的三维数据(DEM)根据双目立体视觉成像原理，利用图像匹配得到的视差数据，计算出相机图像中像素点对应的在相机坐标系下的三维坐标(x_c，y_c，z_c)，根据相机在巡视探测器的安装位置和相机相对于巡视探测器的姿态，将三维坐标(x_c，y_c，z_c)转换到本体坐标系下，求得(x_b，y_b，z_b)；结合巡视探测器当前的姿态信息，将三维坐标(x_b，y_b，z_b)转换到水平投影坐标系下，求得(x_r，y_r，z_r)，从而得到巡视探测器周围局部地形的三维信息，即局部地形的DEM数据；(2)构建栅格地图，进行局部地形的DEM数据去噪处理根据双目立体视觉系统的视场范围，在巡视探测器前方构建一张栅格地图，用(i，j)标记栅格地图中的单元格，其中i和j分别代表单元格所在栅格地图中的行数和列数，对于栅格地图中的每一个单元格(i，j)，定义一个以该单元格为中心、以巡视探测器原地转弯一圈形成的圆，即回转圆的直径为边长的方形的地形块Patch；从步骤(1)得到的DEM数据中找出落在栅格地图单元格对应的地形块Patch区域内的数据点，统计这些数据点中z_r坐标的平均值和方差σ_z，根据统计得到的z_r坐标的平均值和方差σ_z，对地形块Patch中的DEM数据点进行噪点去除；(3)地形可通过性分析，生成适宜度地图针对步骤(2)的栅格地图，利用去噪后的DEM数据对地图中的每一个单元格(i，j)进行地形可通过性分析，生成适宜度地图，分析步骤为：(31)首先找到所述单元格(i，j)对应的地形块Patch中包含的DEM数据点，对所述DEM数据点的三维坐标进行平面拟合；(32)计算拟合平面的法向量与垂直向量的夹角即表示该地形区域的坡度；由地形块Patch中每个DEM数据点到拟合平面的垂直距离计算拟合残差r，r表示该地形区域的粗糙度；计算地形块Patch中每个DEM数据点到拟合平面垂直距离的高度差Δh，Δh表示该地形区域的高度落差；从所述坡度、粗糙度和高度三方面，对该地形块Patch对应的地形区域的可通过性指标进行评价，分别得到坡度适宜度s_g、粗糙度适宜度r_g和高度适宜度h_g，从三者中取最小值，作为当前分析的单元格(i，j)的适宜度g_i，j；对栅格地图中的所有单元格进行上述计算，从而得到适宜度地图；(4)历史数据融合，得到融合后的适宜度地图若当前规划是第一次规则，则执行步骤(5)；若当前规划不是第一次规划，则将前一次规划获得的适宜度地图与当前由步骤(3)获得的适宜度地图进行数据融合，得到融合后的适宜度地图；(5)在适宜度地图上生成备选路径弧在步骤(4)得到的融合后的适宜度地图上均匀设置若干条备选路径弧，每条备选路径弧的弧长L_i相同，但对应的方向角α_i和半径R_i不同，其中，第i条备选路径弧的方向角αi=(i-Na-12)·αmax-αminNa-1;]]>第i条备选路径弧的半径R_i＝L_i/α_i，若α_i为0，则R_i等于弧长L_i；其中，N_a为备选路径弧的总个数，α_min为备选路径弧对应的方向角最小值，α_max为备选路径弧对应的方向角最大值，α_min、α_max是根据立体视觉系统的视场范围及巡视探测器的最小转弯半径进行设置；(6)备选路径弧评价备选路径弧评价分为备选路径弧的安全评价和趋向目标评价，其中备选路径弧的安全评价方法为：对步骤(5)生成的备选路径弧，分别找出其在步骤(4)的适宜度地图中所穿过的栅格地图中单元格，利用所穿过的栅格地图中单元格的适宜度对备选路径弧的安全性进行评价，计算出备选路径弧的安全评价值S，S计算如下式所示：S=Σk=1nw(lk)gkΣk=1nw(lk)]]>n为备选路径弧所穿过的地图单元格的个数；为备选路径弧上从起始点到它所穿过的第k个单元格的那段弧长；g_k为的第k个单元格适宜度；w(l_k)是关于距离的分段线性函数，它的取值在[0，l_u]区间为1，在[l_u，L_i]区间内线性减小到0，l_u的取值要大于备选路径弧弧长L_i的一半；为了进一步增加安全性，将所穿过的栅格地图中单元格适宜度与设定的阈值进行比较，若所述适宜度小于该阈值，则将所述栅格地图中单元格视为“障碍”，当某条备选路径弧连续经过若干个障碍栅格地图中单元格，则认为所述备选路径弧为危险的路径，令所述备选路径弧安全评价值S为0；趋向目标评价如下：统计各备选路径弧末端到规划目标点的距离d_i，得到最近距离d_min和最远距离d_max，令距离目标点最近的备选路径弧的评价值最高，其它备选路径弧的评价值按照标准正态分布的形式减小，从而得到备选路径弧的趋向目标评价值G，计算方法如下：G=e-π(0.47(di-dmin)dmax-dmin)2]]>对上述计算的备选路径弧的安全评价值和趋向目标评价值进行加权求和，得到最终的评价值V：V＝a·S+b·G式中，a和b分别为安全评价值为S和趋向目标评价值G的加权值；(7)选取避障路径从所有备选路径弧中筛选出评价值V高于设定阈值的备选路径弧，然后从中选择出评价值V最高的路径弧，作为本次避障规划的结果；将该路径弧的弧长L_c和曲率C_c＝α_c/L_c输出给运动控制模块，并结束当前避障规划进程；如果所有备选路径弧的评价值V都低于设定的阈值，则返回“无安全路径”的结果。