一种基于双目立体视觉的巡视探测器自主避障规划方法

摘要

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

说明书

技术领域

本发明涉及一种自主避障规划方法，适用于一类复杂未知地形中巡视探 测器的自主避障规划。

背景技术

自主避障规划是移动机器人路径规划研究领域中的一个分支。所谓路径 规划，是指：在具有障碍物的环境中，为移动机器人寻找一条从起始点到目 标点的安全、无碰的运动路径。路径规划的研究一般涉及环境描述和路径搜 索两个问题。其中，环境描述是指如何将机器人所处的环境有效地表达出来， 建立可用于进行规划的环境模型；路径搜索是指采用有效的方法，从所建立 的环境模型中搜索出合适的路径。

根据对环境信息掌握程度的不同，路径规划算法一般可分为两类：全局 路径规划和局部路径规划。

全局路径规划利用现有的环境信息进行路径规划，如果环境信息完全已 知，则能够求解出在一定评价标准下的最优(如路径长度最短、消耗能量最 小等)路径。全局路径规划在环境模型的构建和更新方面一般具有较大的计 算负担，而且算法的复杂性高，比较适合进行离线规划。

局部路径规划不需要先验的环境信息，此类规划主要依靠移动机器人装 载的传感器感知环境，获取局部的环境信息，通过对环境信息的分析，按照 一定的策略寻找合适的路径。与全局路径规划相比，局部路径规划能够在环 境完全未知的情况下进行规划，实时性高，适合在线规划。但由于仅依靠局 部环境信息进行规划，往往无法得到到目标点的最优路径，局部路径规划更 多地是用来进行局部避障，指引移动机器人在躲避障碍物的同时朝向目标点 行驶，并最终走到目标点。所以，局部路径规划也可称为局部避障规划。

目前针对行星探测器的避障规划的研究主要有：基于Bug算法的一类避 障规划算法，例如TangentBug、WedgeBug、RoverBug等，其中RoverBug 成功应用在了JPL研制的火星探测车原理样机Rocky7上；基于Morphin算 法的避障规划。这些规划方法在应用中都存在不同的缺点，Bug类算法建立 的环境模型为二值地图，这种模型过于简单，不适用于粗糙复杂地形上的规 划；Morphin算法仅考虑了地形的安全性，没有结合目标点的位置进行路径搜 索，不具备路径规划的完备性，另外该方法需对巡视探测器的不同航向进行 分析，建模复杂，自主避障规划的效率较低。

发明内容

本发明的技术解决问题：克服现有技术的不足，提出了一种基于双目立 体视觉的行星巡视探测器自主避障规划方法，该方法的避障规划结果安全性 高，规划效率高，且简单，易于实现。

本发明的技术解决方案：一种基于双目立体视觉的巡视探测器自主避障 规划方法，实现步骤如下：

(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数据点进 行噪点去除；

所述步骤(2)地图单元格的尺寸取为巡视探测器车轮直径大小。

(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条备选路径弧的方向角${\alpha }_i=(i-\frac{N_a-1}{2})·\frac{{\alpha }_{\max }-{\alpha }_{\min }}{N_a-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=\frac{\underset{k=1}{\overset{n}{\Sigma }}w\left(l_k\right)g_k}{\underset{k=1}{\overset{n}{\Sigma }}w\left(l_k\right)}$

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^{-\pi {\left(\frac{0.47(d_i-d_{\min })}{d_{\max }-d_{\min }}\right)}^2}$

对上述计算的备选路径弧的安全评价值和趋向目标评价值进行加权求 和，得到最终的评价值V：

V＝a·S+b·G

式中，a和b分别为安全评价值为S和趋向目标评价值G的加权值；

(7)选取避障路径

从所有备选路径弧中筛选出评价值V高于设定阈值的备选路径弧，然后从 中选择出评价值V最高的路径弧，作为本次避障规划的结果；将该路径弧的弧 长L_c和曲率C_c＝α_c/L_c输出给运动控制模块，并结束当前避障规划进程；如果 所有备选路径弧的评价值V都低于设定的阈值，则返回“无安全路径”的结果。

所述步骤(1)中相机坐标系定义为：原点位于相机光心，Z_c轴沿相机的 光轴向前，与图像平面垂直，X_c轴与Z_c轴垂直，指向相机系统的右侧，Y_c轴 与X_c轴、Z_c轴满足右手定则。

所述步骤(1)中本体坐标系定义为：原点位于巡视探测器质心，X_b轴指 向巡视探测器的前进方向，Y_b轴与X_b轴垂直，指向巡视探测器的右侧，Z_b轴 与X_b轴、Y_b轴满足右手定则。

所述步骤(1)中水平投影坐标系定义为：原点位于巡视探测器质心，X_r轴为本体坐标系X_b轴在水平面上的投影，Y_r轴在水平面上，与X_r轴垂直，指 向巡视探测器的右侧，Z_r轴与X_r轴、Y_r轴满足右手定则。

所述步骤(32)中坡度适宜度其中为巡视探测器的 最大安全倾角；粗糙度适宜度r_g＝1-min(1，r/3H_s)，其中H_s为巡视探测器的最大 越障高度；高度适宜度h_g＝1-(Δh/H_s)²。

所述步骤(4)的将前一次规划获得的适宜度地图与当前由步骤(3)获 得的适宜度地图进行数据融合方法为：首先，根据双目立体视觉系统的视场 范围，确定巡视探测器前方无数据区域的大致范围；然后，根据该区域的范 围，从步骤(2)中的栅格地图中确定该区域所包含的栅格地图单元格；最后， 利用前后两次规划得到的适宜度地图数据，对所找出的栅格地图单元格的适 宜度进行赋值计算，从而得到融合后的适宜度地图。所述赋值计算的方法采 用双线性插值的方法。

所述步骤(5)中的备选路径弧方向角α_i的定义为备选路径弧末端的切线 与巡视探测器前进方向的夹角。

本发明与现有技术相比的优点在于：

(1)本发明相比于已知的各局部路径规划方法，通过对DEM进行去噪 处理、历史数据融合等步骤，实现了避障规划结果的安全性高。

(2)本发明采用地形可通过性分析、备选路径评价等方法，实现了适应 性强，具有更为广泛的应用性；

(3)本发明原理和实现方法简单，规划效率高，且完备、易于工程实现， 可用于复杂地形上的巡视探测任务。

附图说明

图1为本发明方法的实现流程图；

图2为本发明的双目立体视觉成像原理示意图；

图3为本发明的坐标系定义示意图；

图4为本发明的地形块示意图；

图5为本发明的适宜度地图融合示意图；

图6这本发明的栅格地图及备选路径弧示意图；

图7为本发明的备选路径弧方向角示意图；

图8为本发明的距离分段线性函数示意图。

具体实施方式

本发明的基本思路为利用双目立体视觉系统处理得到的图像视差数据进 行三维恢复，得到局部地形的原始DEM，并对原始DEM进行去噪处理；构 建栅格地图，利用DEM对栅格地图进行地形可通过性分析，获得用于避障规 划的适宜度地图；对适宜度地图上的备选路径弧进行评价，若存在安全路径， 则从中选取评价值最高的备选路径弧作为本次避障规划的结果。

如图1所示，本发明实现步骤如下：

(1)生成局部地形的DEM数据

根据双目立体视觉成像原理，利用图像匹配得到的视差数据，首先计算 出图像中像素点对应的表示在相机坐标系下的三维坐标(x_c，y_c，z_c)；根据双目立 体视觉系统在巡视探测器上的安装位置和当前的相机姿态，求得从相机坐标 系到巡视器本体坐标系的齐次坐标变换矩阵，记为根据当前巡视探测器 的姿态，求得从本体坐标系到当地水平投影坐标系的齐次坐标变换矩阵，记 为对相机坐标系下的坐标(x_c，y_c，z_c)经过两次齐次坐标变换，求得在水平投 影坐标系中的坐标(x_r，y_r，z_r)，即：

$\left(\begin{array}{c}{x}_r\\ y_r\\ z_r\\ ------\\ 1\end{array}\right)=T_b^r{T}_c^b\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\\ ------\\ 1\end{array}\right)---\left(1\right)$

利用式(1)计算各像素点对应的水平投影坐标系坐标，从而得到局部地 形的DEM。

(2)构建栅格地图，进行局部地形的DEM数据去噪处理

根据双目立体视觉系统的视场范围，在巡视探测器前方构建一张栅格地 图，栅格地图的尺寸应该至少能够包含步骤(1)中生成的DEM，地图单元 格的尺寸一般取为巡视探测器车轮直径大小。

用(i，j)标记栅格地图中的单元格，对于地图中的每一个单元格(i，j)，定义 一个以该单元格为中心、以巡视探测器回转圆的直径为边长的方形的地形块 (Patch)，见图4。

从步骤(1)得到的DEM中找出落在栅格地图单元格对应的地形块区域 内的数据点，统计这些数据点z_r坐标的平均值和方差σ_z。根据统计得到的z_r坐标的平均值和方差σ_z，对地形块中的DEM数据点进行噪点去除。

(3)地形可通过性分析，生成适宜度地图

针对步骤(2)的栅格地图，对地图中的每一个单元格(i，j)进行地形可通 过性分析。

具体分析单元格(i，j)的地形可通过性时，首先找到该单元格(i，j)对应的 Patch中包含的DEM数据点，然后这些DEM数据点的三维坐标进行平面拟 合。

计算拟合平面的法向量与垂直向量的夹角表示该地形区域的坡度；由 Patch中每个DEM数据点到拟合平面的垂直距离计算拟合残差r，表示该地 形区域的粗糙度；计算Patch中每个DEM数据点到拟合平面垂直距离的高度 差Δh，表示该地形区域的高度落差。从坡度、粗糙度和高度三方面，对该Patch 对应的地形区域的可通过性指标进行评价，分别得到坡度适宜度s_g、粗糙度适 宜度r_g和高度适宜度h_g，从三者中取最小值，作为当前分析的单元格(i，j)的适 宜度g_i，j。

对栅格地图中的所有单元格进行上述计算，从而得到适宜度地图。

(4)历史数据融合，得到融合后的适宜度地图

若当前规划是第一次规则，则执行步骤(5)；

若当前规划不是第一次规划，则将前一次规划获得的适宜度地图与当前 由步骤(3)获得的适宜度地图进行数据融合。首先，根据双目立体视觉系统 的视场范围，确定巡视探测器前方无数据区域的大致范围；然后，根据该区 域的范围，从步骤(2)中的栅格地图中确定该区域所包含的栅格地图单元格； 最后，利用前后两次规划得到的适宜度地图数据，对所找出的栅格地图单元 格的适宜度进行赋值计算，从而得到融合后的适宜度地图。

(5)在适宜度地图上生成备选路径弧

在步骤(4)得到的适宜度地图上均匀设置若干条备选路径弧(见图6)， 备选路径弧的方向角α_i(定义见图7)和弧长L_i应根据立体视觉系统的视场范 围以及巡视探测器的最小转弯半径进行设置。

备选路径弧的弧长L_i相同，但对应的方向角α_i和半径R_i不同。巡视探测器 前方左侧的备选路径弧对应的方向角和半径为负值，右侧的备选路径弧对应 的方向角和半径为正值。一般设置奇数条备选路径弧，这样左右各有相同数 量的圆弧型备选路径弧，正中间则是一条直线型的备选路径弧。

假设总共有N_a条备选路径弧，备选路径弧对应的方向角最小值记为α_min， 最大值记为α_max，则从左往右第i条备选路径弧的方向角为：

${\alpha }_i=(i-\frac{N_a-1}{2})·\frac{{\alpha }_{\max }-{\alpha }_{\min }}{N_a-1}---\left(2\right)$

第i条备选路径弧的半径为：

R_i＝L_i/α_i    (3)

对于中间的直线型的备选路径弧，令其方向角为0，半径等于弧长。

另外，还应根据巡视探测器与目标点的距离对备选路径弧进行调整：当 巡视探测器到目标点的距离(记为D_g)小于设定的弧长L_i时，重新设置备选 路径弧，令备选路径弧的弧长等于当前巡视探测器到目标点的距离，则备选 路径弧的方向角为：

${\alpha }_i=\frac{D_g}{L_i}·(i-\frac{N_a-1}{2})·\frac{{\alpha }_{\max }-{\alpha }_{\min }}{N_a-1}---\left(4\right)$

(6)备选路径弧评价

对步骤(5)生成的备选路径弧，分别找出其在步骤(4)的适宜度地图 中所穿过的地图单元格，利用所穿过的地图单元格的适宜度对备选路径弧的 安全性进行评价，计算出备选路径弧的安全评价值。

假设备选路径弧所穿过的地图单元格的个数为n，记备选路径弧的安全评 价值为S，其计算方法如下式所示：

$S=\frac{\underset{k=1}{\overset{n}{\Sigma }}w\left(l_k\right)g_k}{\underset{k=1}{\overset{n}{\Sigma }}w\left(l_k\right)}---\left(5\right)$

式中，l_k为备选路径弧上从起始点到它所穿过的第k个单元格的那段弧长；g_k为的第k个单元格适宜度；w(l_k)是关于距离的分段线性函数，它的取值在[0，l_u] 区间为1，在[l_u，L_i]区间内线性减小到0，l_u的取值一般要大于备选路径弧弧长 L_i的一半。

设置合适的单元格适宜度阈值，适宜度小于该阈值的单元格视为“障碍”， 当某条备选路径弧连续经过若干个障碍单元格，则认为该备选路径弧为危险 的路径，需要在以后的评优筛选中直接否决掉，故令其安全评价值为0。

统计各备选路径弧末端到目标点的距离，得到最近距离d_min和最远距离 d_max。令距离目标点最近的备选路径弧的评价值最高，其它备选路径弧的评价 值按照标准正态分布的形式减小，从而得到备选路径弧的趋向目标评价值G。

对上述计算的备选路径弧的安全评价值和趋向目标评价值进行加权求 和，得到最终的备选路径弧评价值V：

V＝a·S+b·G    (6)

式中，a和b分别为安全评价值为S和趋向目标评价值G的加权值。

(7)选取避障路径

从所有备选路径弧中筛选出评价值V高于设定阈值的备选路径弧，然后从 中选择出评价值V最高的路径弧，作为本次避障规划的结果；将该路径弧的弧 长L_c和曲率C_c＝α_c/L_c输出给运动控制模块，并结束当前避障规划进程；如果 所有备选路径弧的评价值V都低于设定的阈值，则返回“无安全路径”的结果。

以中国空间技术研究院自主研发的月面巡视探测器原理样机为例进一步 说明本发明的具体实现过程。

该原理样机为六轮摇臂悬架式结构，六轮驱动，前后四个角轮提供转向， 可以实现前进、后退、原地转弯、行进中Ackerman转弯。用于避障规划的 双目立体视觉系统为一对安装于车体前方的一对避障相机。

原理样机的主要技术指标为：

外包络尺寸：1500mm×1000mm

车轮直径：300mm

最小转弯半径：1500m

最大越障高度：200mm

最大倾角：30°

避障相机视场角：120°

本发明局部路径规划方法的具体实现如下：

(1)生成局部地形的DEM数据

利用图像匹配得到的视差数据，结合原理样机当前的姿态信息，求得原 理样机周围地形的三维信息，即生成局部地形的DEM数据。

如图2所示为双目立体视觉的成像原理示意图，图中的P_L和P_R为立体视 觉系统相机对的左右两个成像平面。

假设场景中的某一点p在左右图像中的像素坐标分别为(X_L，Y_L)和 (X_R，Y_R)。由于两个成像平面在同一平面上，故Y_L＝Y_R，则由三角几何关系有：

$\left(\begin{array}{c}{X}_L=f\frac{x_c}{z_c}\\ X_R=f\frac{x_c-B}{z_c}\\ Y_L=Y_R=f\frac{y_c}{z_c}\end{array}\right)---\left(7\right)$

式中，f为避障相机的焦距，B为避障相机对的基线。

记点p在两个成像图像中的视差为D，根据视差的定义：D＝X_L-X_R，则 由式(7)可以计算出点p在相机坐标系下的坐标(x_c，y_c，z_c)为：

$\left(\begin{array}{c}{x}_c=\frac{B·X_L}{D}\\ y_c=\frac{B·Y_L}{D}\\ z_c=\frac{B·f}{D}\end{array}\right)---\left(8\right)$

根据相机系统在原理样机上的安装位置和当前的相机姿态，可以求得从 相机坐标系到原理样机本体坐标系的齐次坐标变换矩阵，记为根据 当前原理样机的姿态，可以求得从本体坐标系到水平投影坐标系的齐次 坐标变换矩阵，记为各坐标系的定义见图3。

对相机坐标系下的坐标(x_c，y_c，z_c)经过两次齐次坐标变换，求得场景中点p 在水平投影坐标系中的坐标(x_r，y_r，z_r)，即：

$\left(\begin{array}{c}{x}_r\\ y_r\\ z_r\\ ------\\ 1\end{array}\right)=T_b^r{T}_c^b\left(\begin{array}{c}{x}_c\\ y_c\\ z_c\\ ------\\ 1\end{array}\right)---\left(9\right)$

对于图像中经过稠密匹配处理的像素，利用式(8)和式(9)计算各像 素对应的水平投影坐标系坐标，从而得到局部地形的DEM数据。

(2)构建栅格地图，进行局部地形的DEM数据去噪处理

根据避障相机的视场范围，在原理样机前方构建一张6m×4m大小的栅 格地图，地图单元格的尺寸取为0.3m。

用(i，j)标记栅格地图中的单元格，其中i和j分别代表单元格所在地图中 的行数和列数。对于地图中的每一个单元格(i，j)，定义一个以该单元格为中心、 直径为1.8m的方形地形块(Patch)。

从步骤(1)得到的DEM中找出落在栅格地图单元格对应的地形块区域 内的数据点，统计这些数据点z_r坐标的平均值和方差σ_z。根据统计得到的z_r坐标的平均值和方差σ_z，对地形块中的DEM数据点进行噪点去除。

(3)地形可通过性分析，生成适宜度地图

针对步骤(2)的栅格地图，对地图中的每一个单元格(i，j)进行地形可通 过性分析。

具体分析单元格(i，j)的地形可通过性时，首先找到该单元格(i，j)对应的 Patch中包含的DEM数据点，然后这些DEM数据点的三维坐标进行平面拟 合。

设拟合的平面方程为：

Z＝k₁+k₂X+k₃Y    (10)

式中，k₁、k₂和k₃为待拟合的参数。

由平面方程易知拟合平面的法向量n为：

n＝[k₂ k₃ -1]^T    (11)

求得拟合平面的倾角为：

式中，b＝[0 0 1]^T，表示垂直向量。

平面拟合的残差r由下式计算：

$r=\sqrt{\frac{1}{N}\underset{n=1}{\overset{N}{\Sigma }}{d}_n^2}---\left(13\right)$

式中，d_n为第n个数据点到拟合平面的垂直距离。

由拟合平面的倾角计算单元格(i，j)的坡度适宜度(记为s_g)：

其中，根据原理样机的技术指标，取为30°。

由平面拟合的残差r计算单元格(i，j)的粗糙度适宜度(记为r_g)：

$r_g=1-\min (1,\frac{r}{3H_s})---\left(15\right)$

其中，根据原理样机的技术指标，H_s取为0.2m。

单元格(i，j)的高度适宜度(记为h_g)由下式计算：

$h_g=\left(\begin{array}{cc}0& \mathrm{\Delta h}\ge H_s\\ \frac{{H_s}^2-\Delta h^2}{{H_s}^2}& \mathrm{\Delta h}<H_s\end{array}\right)---\left(16\right)$

其中，Δh为Patch中每个DEM数据点到拟合平面垂直距离的高度差。

取s_g、r_g和h_g三者的最小值作为单元格(i，j)的地形适宜度g_i，j。对栅格地 图中的所有单元格进行上述计算，从而得到局部地形的适宜度地图。

(4)历史数据融合，得到融合后的适宜度地图

若当前规划是第一次规则，则执行步骤(5)；

若当前规划不是第一次规划，则将前一次规划获得的适宜度地图与当前 由步骤(3)获得的适宜度地图进行数据融合，得到融合后的适宜度地图；

这里主要说明进行适宜度地图数据融合的方法：

首先，根据避障相机的视场范围，取样机前方1m远、1.2m宽的区域作 为无数据区域；

其次，根据无数据区域的范围，计算出步骤(2)的栅格地图在该区域中 所包含的地图单元格；

然后，利用上一次规划得到的适宜度地图数据，对所找出的地图单元格 (i，j)的适宜度进行赋值，该步主要包含两部分计算：

i.计算单元格(i，j)中心点在上一次适宜度地图中的坐标(记为)

记当前原理样机的位置在上一次适宜度地图中的坐标为当前原理 样机行驶方向相对于上次规划时行驶方向的转角为Δψ(见图5左图)，则：

Δψ＝α_c·l_exe/L_c

$\left(\begin{array}{c}{\tilde{x}}_k=|R_c·\sin \left(\mathrm{\Delta \psi }\right)|\\ {\tilde{y}}_k=R_c·(1-\cos \left(\mathrm{\Delta \psi }\right))\end{array}\right)---\left(17\right)$

其中，l_exe为每次规划后原理样机的移动步长，L_c为上次规划所选择的路径弧 的长度，α_c和R_c为上次规划所选择的路径弧对应的方向角和半径。

单元格(i，j)在当前适宜度地图中的坐标记为(x_i，j，y_i，j)，则：

$\left(\begin{array}{c}{x}_{i,j}=X_{\min }+(0.5+i-1)l_{\mathrm{cell}}\\ y_{i,j}=Y_{\min }+(0.5+j-1)l_{\mathrm{cell}}\end{array}\right)---\left(18\right)$

其中，X_min＝0，Y_min＝-3，分别为适宜度地图的下边界和左边界；l_cell＝0.3，为 地图单元格的尺寸。

于是有：

$\left(\begin{array}{c}{\tilde{x}}_{i,j}={\tilde{x}}_k+x_{i,j}\cos \left(\mathrm{\Delta \psi }\right)-y_{i,j}\sin \left(\mathrm{\Delta \psi }\right)\\ {\tilde{y}}_{i,j}={\tilde{y}}_k+x_{i,j}\sin \left(\mathrm{\Delta \psi }\right)+y_{i,j}\cos \left(\mathrm{\Delta \psi }\right)\end{array}\right)---\left(19\right)$

ii.计算单元格(i，j)的适宜度

根据单元格(i，j)中心点在上一次适宜度地图中的坐标计算该点 在上一次适宜度地图中的浮点坐标(I+u，J+v)，其中，I和J为整数，u和v为 [0，1]区间内的浮点数。浮点坐标(I+u，J+v)的示意图见图5右图。

则融合后地图单元格(i，j)的适宜度g_i，j通过双线性插值的方法求得：

$g_{i,j}=(1-u)(1-v){\tilde{g}}_{I,J}+(1-u)v{\tilde{g}}_{I,J+1}---\left(20\right)$

$+u(1-v){\tilde{g}}_{I+1,J}+\mathrm{uv}{\tilde{g}}_{I+1,J+1}$

(5)在适宜度地图上生成备选路径弧

在步骤(4)得到的适宜度地图上均匀设置15条备选路径弧，备选路径 弧的弧长L_i＝3(m)，备选路径弧的方向角最小值为α_min＝-2(rad)，最大值记为 α_max＝2(rad)。

(6)备选路径弧评价

对步骤(5)生成的备选路径弧，分别找出其在步骤(4)的适宜度地图 中所穿过的地图单元格，利用所穿过的地图单元格的适宜度对备选路径弧的 安全性进行评价，计算出备选路径弧的安全评价值。

假设备选路径弧所穿过的地图单元格的个数为n，记备选路径弧的安全评 价值为S，其计算方法如下式所示：

$S=\frac{\underset{k=1}{\overset{n}{\Sigma }}w\left(l_k\right)g_k}{\underset{k=1}{\overset{n}{\Sigma }}w\left(l_k\right)}---\left(21\right)$

式中，l_k为备选路径弧上从起始点到它所穿过的第k个单元格的那段弧长；g_k为的第k个单元格适宜度；w(l_k)是关于距离的分段线性函数，它的取值在[0，2] 区间为1，在[2，3]区间内线性减小到0，w(l_k)函数的示意图见图8。

设置单元格适宜度阈值为0.25，适宜度小于该阈值的单元格视为“障碍”， 当某条备选路径弧连续经过4个障碍单元格，则令其安全评价值为0。

统计各备选路径弧末端到目标点的距离d_i，得到最近距离d_min和最远距离 d_max。令距离目标点最近的备选路径弧的评价值最高，其它备选路径弧的评价 值按照标准正态分布的形式减小，从而得到备选路径弧的趋向目标评价值G：

$G=e^{-\pi {\left(\frac{0.47(d_i-d_{\min })}{d_{\max }-d_{\min }}\right)}^2}---\left(22\right)$

对上述计算的备选路径弧的安全评价值和趋向目标评价值进行加权求 和，得到最终的评价值V：

V＝a·S+b·G    (23)

式中，a和b分别取为0.65和0.35。

(7)选取避障路径

设置备选路径弧评价值的阈值为0.5，从所有备选路径弧中筛选出评价值 高于设定阈值的备选路径弧，然后从中选择出评价值最高的路径弧，作为本 次避障规划的结果。将该路径弧的弧长L_c和曲率C_c＝α_c/L_c输出给运动控制模 块，并结束当前避障规划进程。如果所有备选路径弧的评价值都低于设定的 阈值，则返回“无安全路径”的结果。

本发明未详细阐述部分属于本领域公知技术。