一种基于地形可通过性的机器人控制方法及系统

1.本发明涉及二维位置或航道控制技术领域，特别是涉及一种基于地形可通过性的机器人控制方法及系统。


背景技术：

2.随着机器人技术与计算机技术的快速发展，具有自主导航能力的移动机器人在国民生产和生活中得到了广泛关注和应用。从家庭服务、灾难救援，到军事作战和行星探测，移动机器人扮演着越来越重要的角色。对于移动机器人来说，最大的技术挑战是自主导航，即对周围环境进行感知，并利用感知信息进行路径规划及运动控制。相较于结构化环境，移动机器人在非结构化的复杂环境中进行自主导航面临着更加严峻的挑战。
3.在结构化环境中，已有地理信息相对丰富，感知系统可以将对地面的感知信息转换为简单的边界结构形式，用以指导制定感知行为策略。而在非结构化环境中，缺少人工信标，甚至不存在地表人工道路，对地面的感知信息难以用结构化、规则化的形式进行描述，这极大地提高了移动机器人自主导航的难度。另外，结构化环境中多以水泥地面和沥青路面等刚性地形为主，而非结构化环境中存在沙地、草地、泥地等松软地形。由于松软地形中环境介质通常具有一定的流动性或可变形能力，因此，移动机器人在松软地形上运动时会产生沉陷和打滑，且受到的运动阻力往往大于通过刚性地形时受到的阻力。导致机器人的运动能力大大降低，进而影响机器人的自主导航能力。


技术实现要素：

4.本发明的目的是提供一种基于地形可通过性的机器人控制方法及系统，能够在结构化环境合理对机器人进行路径规划和运动控制，提高了机器人路径规划的合理程度。
5.为实现上述目的，本发明提供了如下方案：
6.一种基于地形可通过性的机器人控制方法，所述方法应用于一种机器人，所述机器人上设置有内感知模块和外感知模块；
7.所述内感知模块用于获取所述机器人的姿态参数；
8.所述外感知模块用于获取所述机器人外部环境的深度图像和彩色图像；
9.所述方法包括：
10.获取机器人外部环境的深度图像和彩色图像，以及所述机器人的姿态参数；
11.根据所述机器人外部环境的深度图像和彩色图像，以及所述机器人的姿态参数，构建2.5维数字高程地图；
12.利用可通过性代价函数确定所述2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图；
13.根据所述可通过性地图，将机器人外部环境划分为多种通过程度区域；
14.根据多种通过程度区域规划所述机器人的路径。
15.可选的，所述根据所述机器人外部环境的深度图像和彩色图像，以及所述机器人
的姿态参数，构建2.5维数字高程地图，具体包括：
16.将所述彩色图像输入到地形类别识别模型中，得到彩色图像每个像素点的地形类别l
type
；所述地形类别识别模型是利用标记后的机器人外部环境的彩色图像，对神经网络进行训练得到的；
17.在彩色图像的每个像素点在彩色图像坐标系下的坐标(xc,yc)中添加地形类别l
type
，得到彩色图像坐标系下的语义坐标(xc,yc,l
type
)；
18.基于第一旋转矩阵，将深度图像的每个像素点在深度图像坐标系下的坐标(xd,yd,zd)，转化为在彩色图像坐标系下的坐标(xc,yc,zc)；
19.在彩色图像坐标系下每个像素点的语义坐标(xc,yc,l
type
)中添加深度信息zc，得到彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)；
20.基于第二旋转矩阵，将彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)，转化为每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)；
21.将机器人的姿态参数添加到每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)，得到2.5维数字高程地图。
22.可选的，所述利用可通过性代价函数确定所述2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图，具体包括：
23.通过滑动窗口，利用公式r＝1/v确定所述2.5维数字高程地图中每个栅格的粗糙度；
24.利用最小二乘法构建每个滑动窗口的拟合平面方程ax+by+cz+1＝0；
25.根据多个所述拟合平面方程，利用公式s＝cos-1
(c)确定每个滑动窗口内栅格的倾斜度；
26.利用滑动窗口和最大阶梯度值，确定每个滑动窗口内栅格的阶梯度；
27.根据机器人的足端沉陷量或足端变形量，利用公式确定每个点的法向松软程度；
28.确定栅格内所有点的法向松软程度平均值，为栅格的法向松软程度；
29.根据机器人的足端受到的法向力，利用公式根据机器人的足端受到的法向力，利用公式确定每个点的切向摩擦程度；
30.确定栅格内所有点的切向摩擦程度平均值，为栅格的切向摩擦程度；
31.根据粗糙度、倾斜度、阶梯度、法向松软程度和切向摩擦程度，确定每个栅格的可通过性，得到可通过性地图；
32.其中，r为栅格的粗糙度；v为向量的散度；v＝(n-1)/(n-r)；n为窗口内向量的个数；r为向量的模；数；r为向量的模；为栅格中所有点深度语义坐标中x坐标的平均值；为栅格中所有点深度语义坐标中y坐标的平均值；为栅格中所有点深度语义坐标中z坐标的平均值；a、b和c均为拟合平面方程的系数；s为倾斜度；δ为法向松软程度；k为所在地形的等效刚度系数；n为足端沉陷量或足端变形量的指数系数；f'n为机器人的额定接触力；f为切向摩擦程度，α和β均为权重，α+β＝1；fn为足端受到的法向力；μ为地面的滑动摩擦系
数；k为松软地面的剪切变形模量系数。
33.可选的，所述可通过性代价函数为：
[0034][0035]
其中，τ为地形的可通过性，τ∈[0,1]；ω1为地形倾斜度权重ω2为粗糙度权重，ω3为阶梯度权重，ω4为法向松软程度权重，ω5为切向摩擦程度权重，ω1+ω2+ω3+ω4+ω5＝1；s为地形倾斜度；h为阶梯度；δ为法向松软程度；f为切向摩擦程度；s
crit
为地形倾斜度临界值；r
crit
为粗糙度临界值；h
crit
为阶梯度临界值；δ
crit
为法向松软程度临界值；f
crit
为切向摩擦程度临界值。
[0036]
可选的，所述根据所述可通过性地图，将机器人外部环境划分为多种通过程度区域具体包括：
[0037]
确定所述可通过性地图中任一栅格为当前栅格；
[0038]
判断所述当前栅格的可通过性是否小于第一阈值，得到第一判断结果；
[0039]
若所述第一判断结果为是，则将所述当前栅格划分为低可通过性区域；
[0040]
若所述第一判断结果为否，则判断所述当前栅格的可通过性是否小于第二阈值，得到第二判断结果；第一阈值小于第二阈值；
[0041]
若所述第二判断结果为是，则将所述当前栅格划分为中可通过性区域；
[0042]
若所述第二判断结果为否，则将所述当前栅格划分为高可通过性区域。
[0043]
可选的，所述根据多种通过程度区域规划所述机器人的路径，具体包括：
[0044]
获取路径的起始点和目标点；
[0045]
根据所述起始点和所述目标点，在可通过性区域中规划所述机器人的路径；所述可通过性区域包括中可通过性区域和高可通过性区域；
[0046]
控制机器人沿规划的路径行驶。
[0047]
可选的，所述控制机器人沿规划的路径行驶具体包括：
[0048]
在机器人沿规划的路径通过中可通过性区域时，控制机器人以第一速度行驶；
[0049]
在机器人沿规划的路径通过高可通过性区域时，控制机器人以第二速度行驶；所述第一速度小于所述第二速度。
[0050]
一种基于地形可通过性的机器人控制系统，包括：
[0051]
数据获取模块，用于获取机器人外部环境的深度图像和彩色图像，以及所述机器人的姿态参数；
[0052]
2.5维数字高程地图构建模块，用于根据所述机器人外部环境的深度图像和彩色图像，以及所述机器人的姿态参数，构建2.5维数字高程地图；
[0053]
可通过性地图根据模块，用于利用可通过性代价函数确定所述2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图；
[0054]
通过程度区域划分模块，用于根据所述可通过性地图，将机器人外部环境划分为多种通过程度区域；
[0055]
路径规划模块，用于根据多种通过程度区域规划所述机器人的路径。
[0056]
可选的，所述2.5维数字高程地图构建模块具体包括：
[0057]
地形类别识别单元，用于将所述彩色图像输入到地形类别识别模型中，得到彩色
图像每个像素点的地形类别l
type
；所述地形类别识别模型是利用标记后的机器人外部环境的彩色图像，对神经网络进行训练得到的；
[0058]
语义坐标确定单元，用于在彩色图像的每个像素点在彩色图像坐标系下的坐标(xc,yc)中添加地形类别l
type
，得到彩色图像坐标系下的语义坐标(xc,yc,l
type
)；
[0059]
彩色图像坐标系下坐标确定单元，用于基于第一旋转矩阵，将深度图像的每个像素点在深度图像坐标系下的坐标(xd,yd,zd)，转化为在彩色图像坐标系下的坐标(xc,yc,zc)；
[0060]
深度语义坐标确定单元，用于在彩色图像坐标系下每个像素点的语义坐标(xc,yc,l
type
)中添加深度信息zc，得到彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)；
[0061]
深度语义坐标转换单元，用于基于第二旋转矩阵，将彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)，转化为每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)；
[0062]
2.5维数字高程地图确定单元，用于将机器人的姿态参数添加到每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)，得到2.5维数字高程地图。
[0063]
可选的，所述可通过性地图根据模块具体包括：
[0064]
粗糙度确定单元，用于通过滑动窗口，利用公式r＝1/v确定所述2.5维数字高程地图中每个栅格的粗糙度；
[0065]
拟合平面方程构建单元，用于利用最小二乘法构建每个滑动窗口的拟合平面方程ax+by+cz+1＝0；
[0066]
倾斜度确定单元，用于根据多个所述拟合平面方程，利用公式s＝cos-1
(c)确定每个滑动窗口内栅格的倾斜度；
[0067]
阶梯度确定单元，用于利用滑动窗口和最大阶梯度值，确定每个滑动窗口内栅格的阶梯度；
[0068]
法向松软程度确定单元，用于根据机器人的足端沉陷量或足端变形量，利用公式确定每个点的法向松软程度；
[0069]
栅格法向松软程度确定单元，用于确定栅格内所有点的法向松软程度平均值，为栅格的法向松软程度；
[0070]
切向摩擦程度确定单元，用于根据机器人的足端受到的法向力，利用公式确定每个点的切向摩擦程度；
[0071]
栅格切向摩擦程度确定单元，用于确定栅格内所有点的切向摩擦程度平均值，为栅格的切向摩擦程度；
[0072]
可通过性地图确定单元，用于根据粗糙度、倾斜度、阶梯度、法向松软程度和切向摩擦程度，确定每个栅格的可通过性，得到可通过性地图；
[0073]
其中，r为栅格的粗糙度；v为向量的散度；v＝(n-1)/(n-r)；n为窗口内向量的个数；r为向量的模；数；r为向量的模；为栅格中所有点深度语义坐标中x坐标的平均值；为栅格中所有点深度语义坐标中y坐标的平均值；为栅格中所有点深度语义坐标中z坐标的平均值；a、b和c均为拟合平面方程的系数；s为倾斜度；δ为法向松软程度；k为所在地
形的等效刚度系数；n为足端沉陷量或足端变形量的指数系数；f'n为机器人的额定接触力；f为切向摩擦程度，α和β均为权重，α+β＝1；fn为足端受到的法向力；μ为地面的滑动摩擦系数；k为松软地面的剪切变形模量系数。
[0074]
根据本发明提供的具体实施例，本发明公开了以下技术效果：
[0075]
本发明的目的是提供一种基于地形可通过性的机器人控制方法及系统根据机器人外部环境的深度图像和彩色图像，以及机器人的姿态参数，构建2.5维数字高程地图；利用可通过性代价函数确定2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图；根据可通过性地图，将机器人外部环境划分为多种通过程度区域；根据多种通过程度区域规划机器人的路径，通过构建2.5维数字高程地图，能够在结构化环境合理对机器人进行路径规划和运动控制，提高了机器人路径规划的合理程度。
附图说明
[0076]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0077]
图1为本发明实施例中基于地形可通过性的机器人控制方法流程图；
[0078]
图2为本发明实施例中坐标系建立示意图；
[0079]
图3为本发明实施例中语义-高程信息融合地图模型构建框架示意图；
[0080]
图4为本发明实施例中深度图像与语义图像融合示意图
[0081]
图5为本发明实施例中bekker的轮-地相互作用模型示意图；
[0082]
图6为本发明实施例中分层特征地图构建框架示意图；
[0083]
图7为本发明实施例中粗糙度矢量散度法测量示意图；
[0084]
图8为本发明实施例中滑动窗口示意图。
具体实施方式
[0085]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0086]
本发明的目的是提供一种基于地形可通过性的机器人控制方法及系统，能够在结构化环境合理对机器人进行路径规划和运动控制，提高了机器人路径规划的合理程度。
[0087]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0088]
如图1，本发明提供了一种基于地形可通过性的机器人控制方法，方法应用于一种机器人，机器人上设置有内感知模块和外感知模块；内感知模块用于获取机器人的姿态参数；外感知模块用于获取机器人外部环境的深度图像和彩色图像。
[0089]
方法包括：
[0090]
获取机器人外部环境的深度图像和彩色图像，以及机器人的姿态参数。
[0091]
根据机器人外部环境的深度图像和彩色图像，以及机器人的姿态参数。构建2.5维数字高程地图。
[0092]
利用可通过性代价函数确定2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图。
[0093]
根据可通过性地图，将机器人外部环境划分为多种通过程度区域。
[0094]
根据多种通过程度区域规划机器人的路径。
[0095]
其中，根据机器人外部环境的深度图像和彩色图像，以及机器人的姿态参数，构建2.5维数字高程地图，具体包括：将彩色图像输入到地形类别识别模型中，得到彩色图像每个像素点的地形类别l
type
；地形类别识别模型是利用标记后的机器人外部环境的彩色图像，对神经网络进行训练得到的；在彩色图像的每个像素点在彩色图像坐标系下的坐标(xc,yc)中添加地形类别l
type
，得到彩色图像坐标系下的语义坐标(xc,yc,l
type
)；基于第一旋转矩阵，将深度图像的每个像素点在深度图像坐标系下的坐标(xd,yd,zd)，转化为在彩色图像坐标系下的坐标(xc,yc,zc)；在彩色图像坐标系下每个像素点的语义坐标(xc,yc,l
type
)中添加深度信息zc，得到彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)；基于第二旋转矩阵，将彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)，转化为每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)；将机器人的姿态参数添加到每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)，得到2.5维数字高程地图。
[0096]
利用可通过性代价函数确定2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图，具体包括：通过滑动窗口，利用公式r＝1/v确定2.5维数字高程地图中每个栅格的粗糙度；利用最小二乘法构建每个滑动窗口的拟合平面方程ax+by+cz+1＝0；根据多个拟合平面方程，利用公式s＝cos-1
(c)确定每个滑动窗口内栅格的倾斜度；利用滑动窗口和最大阶梯度值，确定每个滑动窗口内栅格的阶梯度；根据机器人的足端沉陷量或足端变形量，利用公式确定每个点的法向松软程度；确定栅格内所有点的法向松软程度平均值，为栅格的法向松软程度；根据机器人的足端受到的法向力，利用公式确定每个点的切向摩擦程度；确定栅格内所有点的切向摩擦程度平均值，为栅格的切向摩擦程度；根据粗糙度、倾斜度、阶梯度、法向松软程度和切向摩擦程度，确定每个栅格的可通过性，得到可通过性地图；其中，r为栅格的粗糙度；v为向量的散度；v＝(n-1)/(n-r)；n为窗口内向量的个数；r为向量的模；r)；n为窗口内向量的个数；r为向量的模；r)；n为窗口内向量的个数；r为向量的模；为栅格中所有点深度语义坐标中x坐标的平均值；为栅格中所有点深度语义坐标中y坐标的平均值；为栅格中所有点深度语义坐标中z坐标的平均值；a、b和c均为拟合平面方程的系数；s为倾斜度；δ为法向松软程度；k为所在地形的等效刚度系数；n为足端沉陷量或足端变形量的指数系数；f'n为机器人的额定接触力；f为切向摩擦程度，α和β均为权重，α+β＝1；fn为足端受到的法向力；μ为地面的滑动摩擦系数；k为松软地面的剪切变形模量系数。
[0097]
具体的，可通过性代价函数为：
[0098][0099]
其中，τ为地形的可通过性，τ∈[0,1]；ω1为地形倾斜度权重ω2为粗糙度权重，ω3为阶梯度权重，ω4为法向松软程度权重，ω5为切向摩擦程度权重，ω1+ω2+ω3+ω4+ω5＝1；s为地形倾斜度；h为阶梯度；δ为法向松软程度；f为切向摩擦程度；s
crit
为地形倾斜度临界值；r
crit
为粗糙度临界值；h
crit
为阶梯度临界值；δ
crit
为法向松软程度临界值；f
crit
为切向摩擦程度临界值。
[0100]
此外，根据可通过性地图，将机器人外部环境划分为多种通过程度区域具体包括：确定可通过性地图中任一栅格为当前栅格；判断当前栅格的可通过性是否小于第一阈值，得到第一判断结果；若第一判断结果为是，则将当前栅格划分为低可通过性区域；通过程度区域包括：低可通过性区域、中可通过性区域和高可通过性区域；若第一判断结果为否，则判断当前栅格的可通过性是否小于第二阈值，得到第二判断结果；第一阈值小于第二阈值；若第二判断结果为是，则将当前栅格划分为中可通过性区域；若第二判断结果为否，则将当前栅格划分为高可通过性区域。
[0101]
根据多种通过程度区域规划机器人的路径，具体包括：获取路径的起始点和目标点；根据起始点和目标点，在可通过性区域中规划机器人的路径；可通过性区域包括中可通过性区域和高可通过性区域；控制机器人沿规划的路径行驶。
[0102]
控制机器人沿规划的路径行驶具体包括：在机器人沿规划的路径通过中可通过性区域时，控制机器人以第一速度行驶；在机器人沿规划的路径通过高可通过性区域时，控制机器人以第二速度行驶；第一速度小于第二速度。
[0103]
具体的，本发明提出的基于土壤力学的地形可通过性定义为地形几何特征、地形类别、地形物理特性和机器人状态的函数：
[0104][0105]
其中，表示三维环境中坐标处的地形可通过性；是时刻t坐标x处的地形几何特征集合；是时刻t坐标x处的地形类别集合；是时刻t坐标x处的地形物理特征集合，也即土壤力学模型；是具有n个自由度的机器人在时刻t的状态。函数f(
·
)是一个将考虑的所有元素映射到地形可通过性的模型，它的辨识是地形可通过性估计问题的最终目标。
[0106]
本发明提出的基于土壤力学的地形可通过性估计方法的流程图如图1所示。其中，外感知模块主要由深度相机或激光雷达等传感器组成，用于获取环境的彩色图像信息和点云信息。内感知模块由里程计和惯性测量单元组成，用于对机器人进行状态估计。由于不同因子之间单位不同，所以首先对各因子进行归一化处理，然后构建可通过性函数。最后根据可通过性代价函数绘制可通过性代价地图。基于量化的地形可通过性，为移动机器人通行控制策略、路径规划及运动控制提供参考。
[0107]
对未知环境进行远程建模是机器人预先进行运动规划(路径规划、步态规划)的前提。视觉具有信息丰富、感知范围大的特点，通过视觉进行远程环境建模是其它近距离传感器难以实现的。然而仅仅依靠视觉信息是不够的。移动机器人在行走过程中，移动装置(轮、足、履带等)与地面之间的相互作用影响着机器人的地面通过性能。移动装置与地形之间的
相互作用与地表的几何形状和地形介质的物理特性关系密切，仅建立环境的几何模型无法满足野外复杂非结构化环境下规避松软沙土等非几何危险的需求。因此在对环境进行几何建模的基础上，需要加入环境的物理特征，即考虑土壤力学特性对移动机器人运动性能的影响。
[0108]
为了对地形的可通过性进行更加可靠的分析，首先，在视觉构建环境模型方面，考虑传感器噪声和定位误差，建立了一种包含不确定度信息的2.5维环境高程图。其次，针对常见的野外地形类别，训练相应的地形分割模型，并构建语义地图建图框架，对未知环境进行分类。在环境物理特征提取方面，通过简化和统一软硬地面下的机器人与地形之间的相互作用模型，提取表征地面法向松软特性和切向摩擦特性的参数化指标。最终，融合地形几何特性和物理特性，构建一种基于土壤力学特性的地形可通过性模型。
[0109]
为进一步说明本发明提出的基于土壤力学模型的地形可通过性估计方法，接下来以足式移动机器人在复杂的非结构化环境中的运动为例对该方法的具体实施进行阐述，包括可通过性代价函数中各影响因子的获取及代价函数的构建。首先，采用深度相机或者激光雷达等外感知传感器建立机器人的环境感知系统，获取地形点云信息，构建数字高程地图，并提取地形的颜色、纹理等特征用于地形分类。然后，基于数字高程地图，提取地形的几何特征；基于地形分类信息，获取地形物理特性；通过内感知传感器对机器人进行状态估计。最后，对上述因子进行归一化处理，构建地形可通过性代价函数，绘制可通过性代价地图。
[0110]
1.2.5维数字高程地图构建
[0111]
移动机器人在未知环境中行走时感知环境是其自主运行的必要条件，而地形的几何信息是最基本的环境信息。提取几何信息的前提是建立环境的几何模型，本发明使用2.5维栅格地图进行环境几何建模。由于传感器具有噪声并伴随着机器人运动所带来的定位误差，因此在二维栅格图的基础上给每个栅格添加最大高度值和概率值信息，建立具有不确定度的2.5维栅格地图模型。这种采用扩展高程图的思路构建出的2.5维栅格地图，不仅具有三维栅格的表现效果，同时大大降低了栅格地图的存储与运算压力。
[0112]
本发明使用的2.5维栅格地图采用二维栅格地图的思路。首先，使用正方形小栅格对深度相机或激光雷达获取的点云数据进行栅格化，每个小栅格的信息由该栅格在地图中的行、列信息和栅格高程值组成。根据分析的需要，划定所需尺寸的栅格区域进行分析。本发明以机器人机体坐标系原点为中心，在向前5m、向后1m、左右各3m的范围内建立61
×
61的栅格图区域，后续地形可通过性分析只考虑世界坐标系下水平坐标落在此范围内的点云数据。
[0113]
首先要获得世界坐标系下三维点云的高度值。建立如图2所示的参考坐标系。图中i为世界坐标系，固定不动；b为机器人机体坐标系，随机器人运动；s为外感知传感器(深度相机或激光雷达等)坐标系。本发明通过深度相机获取机器人机体坐标系下的三维点云数据，每一个投影点对应一个三维坐标。通过对传感器进行外参数标定，可以得到传感器坐标系到机器人机体坐标系的平移与旋转矩阵(变换矩阵)，进而可将点云数据转换到机器人机体坐标系中。然后考虑移动机器人相对于地面的姿态信息，进一步将三维点云数据变换到世界坐标系下，点的竖直坐标即为对应高度值。
[0114]
在静止状态下，移动机器人机体保持在一个恒定的高度，可以认为相对水平面无
姿态变化，只需要进行机器人机体坐标系到世界坐标系的平移变换，测量出机器人中心相对于世界坐标系中心在各方向上的偏移量，将点云数据投影到世界坐标系下，得到稳定的高度值信息。
[0115]
在运动状态下，必须考虑移动机器人的姿态信息。机器人的姿态信息通过惯性测量元件测得，进而得到机器人机体坐标系相对于世界坐标系的位姿变换关系。这样，点云数据通过位姿变换从机器人机体坐标系投影到世界坐标系下，所得到的三维坐标的z轴坐标即为高度值。每个小栅格的高度值由实际落入单个正方形栅格区域的所有点的平均高度值来表示。当机器人相对于世界坐标系i运动时，高程图中的数据需要不断更新。
[0116]
2.地形分类
[0117]
(1)语义分割模型训练
[0118]
基于视觉信息的地形分类可以看作是一个语义分割问题，其中一个类标签被分配给输入图像中的每个像素。本发明采用liang-chieh chen等人于2017年发表的deeplabv2语义分割框架，该框架代表了目前基于深度学习进行语义分割的较高水平。训练的数据集采用微软发布的coco-stuff数据集，该数据集是由coco数据集进行像素级标注而成。这些标注可以用于语义分割、对象检测和图像字幕的场景理解任务。该数据集有164k张图片，图片中包含80类物体、91种材料类别和一个未标注类别。提取以下类别作为实验数据：硬质地面、沙地、草地、植被、碎石地面、泥地、沼泽、岩石、石头和其它地面一共10种类别，另外把其余的类别全部当作背景。
[0119]
(2)语义地图构建
[0120]
单纯经过语义分割后的二维图片无法直接用于导航，为了将语义信息与前期所建立的高程图相融合，设计了图3所示的融合框架。首先，通过语义分割模型，将深度相机获得的彩色图像处理为语义图片；然后，根据估计的机器人位姿和相机位姿，将二维语义图片计算为语义点云。另一方面，根据环境高程图构建框架，生成环境高程地图。最后，将语义点云投影到相应的高程地图栅格上，生成具有环境高程信息和语义信息的多信息融合环境地图模型。
[0121]
采用深度相机同时获取彩色图像和深度图像。设定彩色相机坐标系为c坐标系。彩色图像包含其坐标平面的(xc,yc,r,g,b)数据流。其中，(xc,yc)表示彩色图像坐标平面上某一点的位置，(r,g,b)代表该点处红(r)、绿(g)、蓝(b)三种颜色的亮度值。为了直接体现语义图像数据，将彩色数据流直接替换为语义数据流(xc,yc,l
type
)，l
type
表示地形类别，可通过前述语义分割训练模型获得。设定深度相机坐标系为d坐标系，深度相机图像可以解释为深度图像坐标平面上的(xd,yd,zd)数据流。
[0122]
如图4所示，将彩色相机坐标系的平面坐标(xc,yc)作为深度图像和语义图像连接的桥梁，通过深度相机和彩色相机之间的旋转矩阵可以将深度图像数据流转化到彩色图像(语义图像)坐标系下，为语义信息添加相应的zc坐标值。此时，通过估计的彩色相机坐标系到世界坐标系之间的旋转矩阵就能构成相应的世界坐标系下的语义点云。最后将语义点云投影到栅格地图上，完成语义地图的构建。
[0123]
3.基于土壤力学的地面物理特性表征方法
[0124]
足式移动机器人在野外环境下依靠足地之间的相互作用驱动其前进。在不同物理
特性的地面上，足式移动机器人的足地接触情况不尽相同。其可能在光滑的瓷砖上打滑，或在沙地、沼泽中发生沉陷。足地相互作用主要体现在法向力和切向力上。法向力表示地面为机器人提供的支撑力，间接反映地面的松软情况。当地面的承载能力不足时，足端会发生沉陷。而切向力则表示地面为机器人提供的驱动力，间接反映地面的切向摩擦程度。若地面可提供的切向力过小，则机器人容易发生打滑，影响其稳定地运动。因此，可利用足地之间的相互作用直观地表征地面的物理特性，并用于规避易发生打滑沉陷的地形，规划出安全的路径。
[0125]
(1)地面法向松软程度的表征
[0126]
1)法向接触模型
[0127]
根据足端类型和地面特性的不同，足地接触一般可分为以下四种形式：刚性足与松软地面接触；柔性足与松软地面接触；柔性足与硬质地面接触；刚性足与硬质地面接触。由于刚性足与硬质地面之间的相互作用会对机器人造成破坏性伤害，常见的移动机器人都会在足端添加弹簧或软质材料来规避破坏性伤害，本发明暂不考虑刚性足与硬质地面之间的碰撞接触情况。本发明考虑的足地之间的法向接触模型可统一为：
[0128]fn
＝kδn[0129]
其中，fn为机器人足端受到的法向接触力；k为不同地形的等效刚度系数；n为足端沉陷量/变形量的指数系数。在硬质地面上，δ表示足端变形量；在松软地形上，δ表示足端沉陷量。k和n的取值与地面类型和足的材料有关。对于给定的足式移动机器人，其足的设计和所用材料已经固定，则参数的变化仅与地面类型有关，因此可作为衡量地面法向松软程度的关键参数。
[0130]
需要注意的是，本发明假设地面类型确定以后，其对应的等效刚度系数和沉降量/变形量指数系数等地形物理特性参数即为已知量。该假设的合理性在于，对于常见的地形(硬质路面、干燥的沙地等)，其土壤特性参数已有大量研究数据可供参考并已建立相关参数库；对于复杂地形(湿沙、淤泥)的土壤特性参数，可以通过在线学习的方式快速进行参数辨识来获取，相关研究成果已见发表。另外，由于本发明提出的方法创新性在于考虑土壤与机器人的相互对机器人运动性能的影响，及非结构化复杂环境下地形可通过估计函数的构建，而非地形分类、土壤参数辨识等具体技术方法的创新。因此，在使用本发明提出的可通过性估计方法进行非结构化复杂环境地形可通过性估计时，采用某种成熟的技术手段完成土壤参数辨识是合理的，并不影响本发明提出的地形可通过性估计方法的创新性。
[0131]
2)法向松软程度表征
[0132]
地形的法向松软程度由等效刚度系数和沉陷量/变形量指数系数共同作用决定。本发明根据机体的重量和法向足地接触模型估计该工况下的足端沉陷量/变形量，以此衡量地形的法向松软程度。记机器人的额定接触力为f'n，该额定值依据机器人的重量、状态估计值和动力学模型计算得到。对应工况下的沉陷量/变形量δ用于度量地形的法向松软程度，公式如下：
[0133][0134]
需要说明的是，对于不同的行走装置(轮、足、履带等)，由于接触模型不同，故计算
公式有所不同，但法向松软程度的表征不受行走装置类型的制约。这里再以轮式移动机器人为例进行说明，图5为基于bekker土壤力学模型的轮-地相互作用模型。
[0135]
图5中各参数解释：r为轮子半径，w为轮子的垂直载荷，f'
rc
为作用在轮轴上的驱动力，θ1为轮子在沙地上运动时的进入角，σ为作用在轮子径向上的法向应力，z为轮子的沉陷量，zw(θ)为轮子在转角θ处的沉陷量。
[0136]
当车轮静置于沙地上时，由于车轮表面与沙子之间没有相对运动，所以车轮不受切向力的作用。其用于平衡垂直载荷的法向力fn仅由法向应力σ产生：
[0137][0138]
式中b为轮子宽度。
[0139]
运动阻力f
rc
由作用在轮轴上的驱动力f'
rc
克服：
[0140][0141]
如图5所示，由几何关系可得：
[0142]
x＝rsinθ
[0143]zw
(θ)＝r(cosθ-cosθ1)
[0144]
dx＝rcosθdθ
[0145]
dz＝-rsinθdθ
[0146]
当θ＝0时，x(0)＝0，zw(0)＝z；当θ＝θ1时，x(θ1)＝x1，zw(θ1)＝0。将代入前述公式可得垂直载荷w，运动阻力f
rc
和轮子的沉陷量z：
[0147][0148][0149][0150]
其中，ks为地形等效下沉模量，kc为地形内聚模量，为地形摩擦模量，n为地形的沉降指数。
[0151]
综上，法向松软程度表征适用于对不同行走装置在复杂地形上的可通过性进行分析。
[0152]
(2)地面切向摩擦程度的表征
[0153]
1)切向足地作用力学模型
[0154]
不同类型的足端与地面之间的切向相互作用可统一表示为
[0155]ft
＝-μ
·
th(k,1.5k)fn[0156]
[0157]
其中，f
t
为足端受到的切向力；fn为足端受到的法向力；μ为地面的滑动摩擦系数；k为松软地面的剪切位移量；k为松软地面的剪切变形模量系数。二者取值与地面类型和足的材料有关。同样，对于给定的足式移动机器人，其足的设计和所用材料已经固定，则参数的变化仅与地面类型有关，因此可作为衡量地面法向松软程度的关键参数。
[0158]
2)切向摩擦程度表征
[0159]
地面切向摩擦程度可以通过在接触过程中地面所能提供的最大切向力和达到最大切向力时的切向位移两个参数来共同度量。设置打滑系数来衡量地形的切向摩擦程度，具体表达如下：
[0160][0161]
其中，α、β为权重，α+β＝1。通过改变其数值可以调整最大切向力指标和饱和位移值(达到最大切向力时的切向位移)对打滑系数的重要程度；打滑系数f值越大代表足地之间越不容易打滑。对于给定的机器人，打滑系数只与地面参数有关，因此该参数可以作为单值量表征地面切向摩擦程度。类似地，对于轮式移动机器人，切向摩擦程度表征同样适用。
[0162]
4.融合地面环境几何-物理特征的可通过性代价地图构建
[0163]
环境信息通常存储在代价地图中用于路径规划。本发明构建如图6所示的分层特征地图。该地图由基础层和演绎层构成，基础层包含地形几何特征和物理特性基本要素，包括高度层、高度方差层、法向松软度层和切向摩擦程度层；演绎层则由基本层中的环境高程信息直接或间接地推导得来，包括但不限于粗糙度层、倾斜度层、阶梯度层等。
[0164]
(1)基础层构建
[0165]
1)高度层
[0166]
本发明构建高度层的方法为使用深度相机获取环境点云，然后将环境点云投影到所建立世界坐标系下的栅格平面，构建2.5维高程地图。每个小栅格的高度值由实际落入单个正方形栅格区域的所有点的平均高度值来表示。
[0167]
2)高度方差层
[0168]
由于传感器数据带有噪声且机器人的运动位置估计存在误差，使得所构建的地形高程图存在一定程度上的偏差。本发明在构建数字高程地图时，通过卡尔曼滤波计算不确定度，同时考虑了高度测量误差和机器人运动时产生的定位误差。
[0169]
3)法向松软度层
[0170]
根据机器人的质量等机体信息确定额定法向力和地形相关参数，可以计算出地形相应的法向松软程度层。
[0171]
4)切向摩擦程度层
[0172]
根据机器人的质量等机体信息确定额定法向力和地形相关参数，可以计算出地形相应的切向摩擦程度层。
[0173]
(2)演绎层构建
[0174]
1)粗糙度层
[0175]
粗糙度通过量化真实表面与其理想形状的垂直偏差来对地形表面纹理进行度量。粗糙表面的垂直偏差较大，而较小的偏差值则表示对应的表面相对光滑。本发明采用矢量
散度法来度量表面粗糙度。
[0176]
以矩阵形式输入数字高程图，通过三角网格的方法进行快速三维重建，然后计算每一个三角形的单位法向量(xi,yi,zi)，如图7-8所示。各栅格的粗糙度可通过滑动窗口的方法依次进行计算，如图所示，滑动窗口的尺寸d可以根据地形和机器人的尺寸进行调整。
[0177]
每个栅格的粗糙度可根据以下公式计算：
[0178][0179][0180]
v＝(n-1)/(n-r)
[0181]
r＝1/v
[0182]
其中，n为窗口内向量的个数；r表示向量的模；v表示向量的散度；r表示栅格的粗糙度；记为满足机器人运动时的稳定裕度要求，所允许的最大粗糙度值为r
crit
，该值可由机器人的运动学参数确定。
[0183]
2)倾斜度层
[0184]
地形的倾斜度是评价地形可通过性的另一个重要指标。地形的倾斜度越大，重力沿斜面向下的分量越大，机器人越容易打滑。因此，倾斜度越大越难满足机体稳定裕度的要求，机器人倾覆的可能性也相应越大。各栅格的地形倾斜度也可通过滑动窗口来计算。对于不同的规划目的，可以选择不同大小的滑动窗口。若滑动窗口的面积小于机器人的足端面积，则计算得到的倾斜度层适用于机器人落足点的选取；若滑动窗口的面积设置接近于机器人的体面积，则计算得到的倾斜度层更适用于机器人机体的运动规划。将滑动窗口中包含的栅格点的高度值通过最小二乘法估计得到相应的平面方程，拟合得到的一般平面方程表示为：
[0185]
ax+by+cz+1＝0
[0186]
其中a、b、c为相应平面系数。拟合平面的倾斜度可表示为：
[0187]
s＝cos-1
(c)
[0188]
记机器人运动时不发生倾翻所允许的最大倾斜度值为s
crit
，该值可通过对机器人进行受力分析确定。
[0189]
3)阶梯度层
[0190]
地形阶梯值表示地形的高度差，虽然倾斜度也可以在一定程度上代表地形的高度变化，但仍然无法准确衡量机器人是否能够通过这样的高度变化。例如，当局部地形的高度差较小而对应的倾斜度接近90度时，机器人仍能通过相应的地形；而对于倾斜程度而言，90度却是一个极度危险的角度。阶梯值可通过移动窗格的方法进行计算，整个过程分为两步：第一步，设置可接受的最大阶梯高度值h
crit
，该值由机器人足端的运动空间确定。计算每个窗格中各个栅格与窗格中心栅格的差值的绝对值，选取最大值作为临时阶梯度值h
max
，若h
max
大于h
crit
，则倾斜度s需要大于最大倾斜度值s
crit
；第二步，使用以下公式更新最终阶梯值:
[0191][0192]
其中，h为阶梯度值；n
st
表示在滑动窗格中高度差大于h
crit
的栅格的个数；n
scrit
表示高度差大于h
crit
滑动窗格中的有效栅格的个数。
[0193]
最终，基于地形几何特征和物理特性，建立可通过性代价函数：
[0194][0195]
式中，τ表示地形的可通过性，τ∈[0,1]。其值越大，表示地形的可通过性越强，τ＝0表示地形不可通过。ω1,ω2,ω3,ω4,ω5表示权重，权重值可根据机器人自身性能及运动要求进行设定，ω1+ω2+ω3+ω4+ω5＝1。
[0196]
s,r,h,δ,f分别表示地形倾斜度、粗糙度、阶梯度、法向松软程度和切向摩擦程度。s
crit
,r
crit
,h
crit
,δ
crit
,f
crit
为对应因子的临界值，也即受机器人结构参数、运动参数及地形物理特性所限制的理论最大值。其中，s
crit
是保证机器人不发生倾翻时允许的最大倾斜度值，由机器人的结构参数和运动参数确定。r
crit
是保证机器人运动时的稳定裕度所允许的最大粗糙度值，由机器人的结构参数和运动参数确定。h
crit
是机器人所能跨越的最大阶梯值，由机器人的工作空间确定。δ
crit
是保证机器人不丧失运动能力所允许的最大沉陷量/变形量。对于刚性足/柔性足在松软地形上运动的情况，δ
crit
是机器人在运动过程中所允许的最大足端沉陷量，当沉陷量超过该值时，机器人会陷入所处地形，丧失运动能力。对于柔性足在硬质地形上运动的情况，δ
crit
是机器人在运动过程中所允许的最大足端变形量，当变形量超过该值时，机器人足端将会受到破坏，丧失运动能力。
[0197]
s,r,h,δ,f任何一个值超过对应的临界值时，τ＝0，表示地形不可通过。
[0198]
基于本发明构建的地形可通过性代价函数，可进一步绘制地形可通过性代价地图。基于代价地图，可将地形分为不可通过(τ＝0)、低可通过性(0＜τ＜＜0.3)、中可通过性(0.3＜τ＜＜0.7)和高可通过性(0.7＜τ＜＜1)四种等级。
[0199]
对于不可通过地形和低可通过性地形，采用绕行的通行控制策略；对于中等可通过性的地形，对机器人进行减速通过；对于高可通过性的地形，机器人可在给定的车速下正常行驶，无需改变运动参数。
[0200]
此外，本发明还提供了一种基于地形可通过性的机器人控制系统，包括：
[0201]
数据获取模块，用于获取机器人外部环境的深度图像和彩色图像，以及机器人的姿态参数。
[0202]
2.5维数字高程地图构建模块，用于根据机器人外部环境的深度图像和彩色图像，以及机器人的姿态参数，构建2.5维数字高程地图。
[0203]
可通过性地图根据模块，用于利用可通过性代价函数确定2.5维数字高程地图中每个栅格的可通过性，得到可通过性地图。
[0204]
通过程度区域划分模块，用于根据可通过性地图，将机器人外部环境划分为多种通过程度区域。
[0205]
路径规划模块，用于根据多种通过程度区域规划机器人的路径。
[0206]
2.5维数字高程地图构建模块具体包括：地形类别识别单元，用于将彩色图像输入
到地形类别识别模型中，得到彩色图像每个像素点的地形类别l
type
；地形类别识别模型是利用标记后的机器人外部环境的彩色图像，对神经网络进行训练得到的；语义坐标确定单元，用于在彩色图像的每个像素点在彩色图像坐标系下的坐标(xc,yc)中添加地形类别l
type
，得到彩色图像坐标系下的语义坐标(xc,yc,l
type
)；彩色图像坐标系下坐标确定单元，用于基于第一旋转矩阵，将深度图像的每个像素点在深度图像坐标系下的坐标(xd,yd,zd)，转化为在彩色图像坐标系下的坐标(xc,yc,zc)；深度语义坐标确定单元，用于在彩色图像坐标系下每个像素点的语义坐标(xc,yc,l
type
)中添加深度信息zc，得到彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)；深度语义坐标转换单元，用于基于第二旋转矩阵，将彩色图像坐标系下的深度语义坐标(xc,yc,zc,l
type
)，转化为每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)；2.5维数字高程地图确定单元，用于将机器人的姿态参数添加到每个像素点在世界坐标系的深度语义坐标(xi,yi,zi,l
type
)，得到2.5维数字高程地图。
[0207]
可通过性地图根据模块具体包括：粗糙度确定单元，用于通过滑动窗口，利用公式r＝1/v确定2.5维数字高程地图中每个栅格的粗糙度；拟合平面方程构建单元，用于利用最小二乘法构建每个滑动窗口的拟合平面方程ax+by+cz+1＝0；倾斜度确定单元，用于根据多个拟合平面方程，利用公式s＝cos-1
(c)确定每个滑动窗口内栅格的倾斜度；阶梯度确定单元，用于利用滑动窗口和最大阶梯度值，确定每个滑动窗口内栅格的阶梯度；法向松软程度确定单元，用于根据机器人的足端沉陷量或足端变形量，利用公式确定每个点的法向松软程度；栅格法向松软程度确定单元，用于确定栅格内所有点的法向松软程度平均值，为栅格的法向松软程度；切向摩擦程度确定单元，用于根据机器人的足端受到的法向力，利用公式向力，利用公式确定每个点的切向摩擦程度；栅格切向摩擦程度确定单元，用于确定栅格内所有点的切向摩擦程度平均值，为栅格的切向摩擦程度；可通过性地图确定单元，用于根据粗糙度、倾斜度、阶梯度、法向松软程度和切向摩擦程度，确定每个栅格的可通过性，得到可通过性地图；
[0208]
其中，r为栅格的粗糙度；v为向量的散度；v＝(n-1)/(n-r)；n为窗口内向量的个数；r为向量的模；数；r为向量的模；为栅格中所有点深度语义坐标中x坐标的平均值；为栅格中所有点深度语义坐标中y坐标的平均值；为栅格中所有点深度语义坐标中z坐标的平均值；a、b和c均为拟合平面方程的系数；s为倾斜度；δ为法向松软程度；k为所在地形的等效刚度系数；n为足端沉陷量或足端变形量的指数系数；f'n为机器人的额定接触力；f为切向摩擦程度，α和β均为权重，α+β＝1；fn为足端受到的法向力；μ为地面的滑动摩擦系数；k为松软地面的剪切变形模量系数。
[0209]
本发明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0210]
本发明中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依
据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本发明书内容不应理解为对本发明的限制。