非平坦地面环境下的机器人导航规划方法、装置及机器人

1.本发明属于自主无人系统技术领域，尤其涉及非平坦地面环境下的机器人导航规划方法、 装置及机器人。


背景技术：

2.在许多具有挑战性的应用场景(如搜救、探索、检查或运输)中，自主导航对于无人车 的安全高效运行至关重要，它让无人车得以执行或协助对人类来说是重复的、疲惫的甚至是 危险的任务。除了定位和建图，轨迹生成是任何自主导航系统的关键功能。
3.无人车轨迹生成技术，指无人车根据自身传感器对环境感知，从当前点到目标点，自动 规划一段安全的运动轨迹。针对地面机器人轨迹生成主要解决三大问题：1)连通性，即轨迹 必须能连接起点和终点；2)可执行性，即轨迹满足机器人自身的约束，如最小转弯半径(最 大曲率)、最大速度、最大加速度等；3)最优性，即在完成以上任务的前提下，轨迹的长度、 对机器人的能耗应被尽量优化。
4.现有算法为了方便计算，大多是在二维环境进行导航规划；但实际上地面机器人的工作 环境不能用二维环境简单描述，诸如野外环境(崎岖的草坪、充满土包的山路)、具有多层结 构的人造建筑物等环境，现有技术往往只在二维平面将环境区分为障碍物和可通行区域，例 如使用高度阈值二分法，这样做没有考虑地形对无人车的影响，更是略去了大量实际可通行 的地面，因此计算出的轨迹既不能保证可执行性，也不能保证在一定标准下(如轨迹的长度、 对机器人的能耗)的最优性，甚至在复杂的环境中无法规划出合理轨迹。
5.有一些新技术，允许在三维点云上直接进行车辆的导航规划，但是，直接在三维点云上 计算的方法通常需要在线地对点云进行knn取近邻，拟合局部地形特征，运算量巨大以至 于不能实时计算。并且，基于局部拟合的算法不能体现更大范围内点云的特征，在规模更大 的复杂环境中鲁棒性不高。更为重要的一点是，直接在点云上进行导航规划所生成的轨迹受 限于点云信息的离散性，往往表现为不带有时间信息的折线路径，轨迹的可执行性不强、能 耗也很高。


技术实现要素：

6.本技术实施例的目的是提供非平坦地面环境下的机器人导航规划方法、装置及机器人， 以解决相关技术中存在的针对二维平面规划存在的环境表示不准确问题，直接点云上进行导 航存在的计算量大实时性不强、轨迹平滑性差、缺乏时间信息且能耗高的问题，利用结构化 栅格地图，利用基于优化的轨迹方法，通过构建符合地面机器人的特殊安全性约束，实现了 针对地面机器人在非平坦地面环境下的光滑无碰能量最优轨迹规划。
7.根据本技术实施例的第一方面，提供一种非平坦地面环境下的机器人导航规划方法，包 括：
8.步骤(1)：获取原始点云，对所述原始点云进行可通行区域过滤；
9.步骤(2)：对原始点云进行下采样，生成占据栅格地图；
10.步骤(3)：对过滤后的点云进行下采样，构建前端路径搜索空间；
11.步骤(4)：在步骤(3)中生成的前端路径搜索空间中，进行基于a*的图搜索，得到前端 路径；
12.步骤(5)：基于步骤(4)得到的前端路径生成轨迹初值，根据所述占据栅格地图构造惩 罚函数，利用凸优化算法生成最终轨迹，其中所述惩罚函数包括用于使得轨迹平滑的平滑性 惩罚函数、用于控制轨迹总时长的总时间惩罚函数、用于使得轨迹上任意时刻的机器人期望 速度、期望加速度满足机器人自身的动力学限制的动力学惩罚函数以及用于使得轨迹避开危 险区域且不产生碰撞的安全性惩罚函数。
13.进一步地，所述步骤(1)包括：
14.(1.1)遍历原始点云中的每一个点m，在该点m附近搜索半径为r的领域点；
15.(1.2)遍历邻域点构成的集合dr中的每一个点d，求出点d与点m的高度差z
dm
以及连 线与水平面的夹角α，计算方式如下：
16.z
dm
＝d
3-m3[0017][0018]
其中，d3和m3分别指点d和m坐标的z轴分量；
[0019]
(1.3)判断点m是否符合可通行区域条件，其中判断条件为：对dr中任意点d，都不存 在z
dm
＜0且α＜θ的情况，如果满足该条件，则点m在可通行区域ms内，否则过滤掉点m， 其中θ是机器人最大安全站立倾斜角。
[0020]
进一步地，所述步骤(2)包括：
[0021]
计算所述原始点云中每个点对应的栅格下标：
[0022][0023]
其中，floor函数表示向下取整，res
occ
为栅格地图的分辨率，bound
min
和bound
max
为 原始点云在三维空间的边界范围；
[0024]
根据每个点对应的栅格下标，判断落入每个栅格中的点的个数，如果落入一栅格中的点 数大于预定阈值，该栅格的值设为1，否则设为0，以完成对栅格的重新赋值，从而生成占据 栅格地图。
[0025]
进一步地，所述步骤(3)包括：
[0026]
利用与步骤(2)相同的方式，对过滤后的点云ms构建占据栅格地图；
[0027]
对每个过滤后的点云ms中被占据的栅格进行遍历，判断每个被占据的栅格的上方栅格是 否为非占据栅格，如果是，则将该非占据栅格划定在前端路径搜索空间中，从而完成前端路 径搜索空间的构建。
[0028]
进一步地，所述平滑性惩罚函数je用于使得轨迹尽可能平滑，保证能量最优性：
[0029][0030]
其中p(t)是轨迹方程，m为轨迹段数。
[0031]
进一步地，所述总时间惩罚函数j
t
用于使得轨迹的总时间尽可能短：
[0032][0033]
其中ti为第i段轨迹的总时长，m为轨迹段数。
[0034]
进一步地，所述动力学惩罚函数jd用于使得轨迹上任意时刻的机器人期望速度、期望加速 度满足机器人自身的动力学限制：
[0035][0036]
其中c(.)＝max{.,0}3为立方罚函数，为期望速度，为期望加速度，m为轨迹段数， v
max
和a
max
分别是最大速度限制和最大加速度限制，ki为第i段轨迹的采样个数：
[0037][0038][0039]
式中，β为多项式轨迹p(t)＝c
t
β(t)的基，c为多项式系数。
[0040]
进一步地，所述安全性惩罚函数jc用于使得轨迹远离危险区域，并且不会产生碰撞，所述 安全性惩罚函数的构造过程为：
[0041]
在原始点云构建的占据栅格地图上构建esdf地图，并选取一个合适的截断距离，生成 tsdf地图：
[0042][0043]
其中d
thr
是安全距离阈值，d(p)是tsdf函数值，用来惩罚轨迹上距离障碍物太近的点，为第j段轨迹第i个采样点；
[0044]
基于所述tsdf地图，构造无碰惩罚函数j
ct
：
[0045][0046]
根据机器人的位姿透射函数，根据地形带来的额外危险区域分析，构造地形惩罚函数：
[0047][0048]
其中，机器人的位姿透射函数表示在空间位置p处，机器人底盘中 心坐标为x0,y0,z0，机器人倾斜角为λ
sp
和λ
se
是权重系数；
[0049]
根据所述tsdf地图和地形惩罚函数，构造安全性惩罚函数：
[0050]
[0051]
根据本技术实施例的第二方面，提供一种非平坦地面环境下的机器人导航规划装置，包 括：
[0052]
过滤模块，用于获取原始点云，对所述原始点云进行可通行区域过滤；
[0053]
生成模块，用于对原始点云进行下采样，生成占据栅格地图；
[0054]
构建模块，用于对过滤后的点云进行下采样，构建前端路径搜索空间；
[0055]
路径搜索模块，用于在构建模块中生成的前端路径搜索空间中，进行基于a*的图搜索， 得到前端路径；
[0056]
路径优化模块，用于基于路径搜索模块得到的前端路径生成轨迹初值，根据所述占据栅 格地图构造惩罚函数，利用凸优化算法生成最终轨迹，其中所述惩罚函数包括用于使得轨迹 平滑的平滑性惩罚函数、用于控制轨迹总时长的总时间惩罚函数、用于使得轨迹上任意时刻 的机器人期望速度、期望加速度满足机器人自身的动力学限制的动力学惩罚函数以及用于使 得轨迹避开危险区域且不产生碰撞的安全性惩罚函数。
[0057]
根据本技术实施例的第三方面，提供一种机器人，包括：
[0058]
一个或多个处理器；
[0059]
存储器，用于存储一个或多个程序；
[0060]
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现 如第一方面所述的方法。
[0061]
本技术的实施例提供的技术方案可以包括以下有益效果：
[0062]
(1)本发明生成的轨迹安全平滑、具有能量最优性和良好的执行性，且满足地面机器人 的运动规律。
[0063]
(2)本发明直接利用原始点云生成三维空间的地图表示，最大程度地保留了可通行区域 的信息，避免了传统二维算法对环境信息的压缩与舍弃。
[0064]
(3)本发明生成的轨迹，在获得初始轨迹后，对轨迹进行了全局优化和局部优化，保证 了所得轨迹的最优性。
[0065]
(4)轨迹生成可使用局部点云地图，可以在全局环境未知的情况下使用。
[0066]
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制 本技术。
附图说明
[0067]
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本技术的实施例，并 与说明书一起用于解释本技术的原理。
[0068]
图1是根据一示例性实施例示出的一种非平坦地面环境下的机器人导航规划方法的整体 框架图。
[0069]
图2是根据一示例性实施例示出的点云可通行区域过滤示意图。
[0070]
图3是根据一示例性实施例示出的地形危险性分析效果图。
[0071]
图4是根据一示例性实施例示出的前端搜索效果示意图。
[0072]
图5是根据一示例性实施例示出的最终轨迹效果示意图。
[0073]
图6是根据一示例性实施例示出的一种非平坦地面环境下的机器人导航规划装置的框图。
[0088][0089]
其中，d3和m3分别指点d和m坐标的z轴分量。
[0090]
(1.3)判断点m是否符合可通行区域条件，该条件描述为：对dr中任意点d，都不存 在z
dm
＜0且α＜θ的情况。如果满足该条件，则m点在可通行区域ms内，否则过滤掉点 m。θ是机器人最大安全站立倾斜角，根据机器人的实际性能确定。
[0091]
基于上述三步，我们利用原始点云筛选出了机器人可通行区域的点云，筛选掉了属于障 碍物的点云，这在后续前端路径搜索中起到了至关重要的作用。
[0092]
在一实施例中，图2展示了本步骤中的点云可通行区域过滤，图2中的(a)图中点a是满 足可通行性的点，应当被保留，点b，c，d由于红线与水平面的夹角超过阈值，均不满足可 通行性，图2中的(b)图展示了遍历完成之后各点的判定情况，图2中的(c)图为完成过滤后 ms中的点。
[0093]
在步骤(2)的具体实施中，首先指定栅格地图的分辨率res
occ
，随后遍历原始点云中的 点，确定原始点云在三维空间的边界范围bound
min
和bound
max
，以这两个点为体对角线 构造的长方体将完全包裹原始点云。随后再次遍历点云，判断落入每个栅格中的点的个数， 每个点对应的栅格下标计算方式如下：
[0094][0095]
floor函数表示向下取整。进行点云遍历操作后，最后遍历所有栅格，重新对栅格赋值： 如果落入该栅格中的点数大于某个阈值，该栅格的值设为1，否则设为0。通常，对于slam 算法直出的点云地图，阈值往往大于1，对于已经经过初步下采样后的点云地图，阈值可以 设为1。此步骤完成后即完成了栅格地图的生成。
[0096]
在步骤(3)的具体实施中，利用与步骤(2)相同的方式，对滤波后的点云ms构建占 据栅格地图。由于ms是mo的子集，因此无需额外申请内存空间来分别存放mo和ms的占 据栅格地图。随后对每个ms中被占据的栅格进行遍历，判断其上方栅格是否为非占据栅格， 如果是，则此非占据栅格被划定在前端路径搜索空间中，完成ms被占据栅格遍历后即完成 了前端路径搜索空间的构建。不难看出，前端路径搜索空间是紧贴着物体上表面的一层空间， 这可以保证前端路径基本符合地面机器人的运动规律，为后续的轨迹优化提供良好的初值。
[0097]
在步骤(4)的具体实施中，采用基于a*的图搜索，首先确定搜索的起始点和终点，假 设机器人此时的位置为p，终点位置为pe，为了保证起点和终点都在前端路径搜索空间内，需 要对p和pe进行一次向下投射，即确定p和pe的栅格id，如果该处栅格不在前端路径搜索空间 内，则向下移动一格栅格并再次判断，直到该处栅格在前端路径搜索空间内为止。完成起点 终点的投射后，进行一次a*搜索，a*是一种成熟的路径搜索算法，具体过程不在此赘述。
[0098]
在步骤(5)的具体实施中，轨迹的表示形式采用了minco轨迹类的表示方式，具体定 义如下：
[0099]
[0100]
该轨迹类采用参数q,t来表示多项式，含义为：经过中间点q，并且每段的时间为t的 满足能量最优性的多项式轨迹。其中c为多项式系数，m为q,t参数到c参数的映射。
[0101]
常用的多项式表示方法是使用系数c来表示多项式：
[0102]
p(t)＝c
t
β(t)
[0103]
β是多项式的基。minco的表示方法与系数c表示方法之间存在一个映射c＝m(q,t)， 利用该映射，构建在多项式上的惩罚函数j(c,t)可以便捷地转换为构建在minco轨迹上的 惩罚函数：h(q,t)＝j(m(q,t),t),且各项偏导数也能快速求出。
[0104]
轨迹优化的惩罚函数包括平滑性惩罚函数、总时间惩罚函数、动力学惩罚函数、安全性 惩罚函数。优化问题的形式为：
[0105]
min
c,t
λeje+λ
tjt
+λdjd+λ
sjs
[0106]
其中λ均为对应的权重系数，在具体实施中应当根据经验设置。
[0107]
(ⅰ)平滑性惩罚函数用于使得轨迹尽可能平滑，保证能量最优性：
[0108][0109]
其中p(t)是轨迹方程，m为轨迹段数。
[0110]
(ⅱ)总时间惩罚函数使得轨迹的总时间尽可能短：
[0111][0112]
其中ti为第i段轨迹的总时长，m为轨迹段数。
[0113]
(ⅲ)动力学惩罚函数使得轨迹上任意时刻期望的机器人速度、加速度满足机器人自身 的动力学限制，由于轨迹具有时间信息，轨迹上某一点的期望速度和加速度分别可计算如下：
[0114][0115][0116]
动力学惩罚函数如下：
[0117][0118]
其中c(.)＝max{.,0}3为立方罚函数，m为轨迹段数，v
max
和a
max
分别是最大速度限 制和最大加速度限制，ki为第i段轨迹的采样个数(用离散的数值采样近似计算数值积分)。
[0119]
(ⅳ)安全性惩罚函数使得轨迹远离危险区域，并且不会产生碰撞。对于地面机器人来 讲，除了将物体视作障碍物之外，还需要考虑地形带来的额外危险区域，由于地面机器人不 能飞行，并且平衡能力有限，因此，地形带来的额外危险区域包括以下两种：1.太过倾斜的 地面，机器人站在上方会导致倾覆；2.陡峭的边缘，机器人在边缘移动可能会发生坠落或 者姿态剧烈变化。因此，安全性惩罚函数需要考虑上述所有的危险情况。
[0120]
首先需要使轨迹远离物体，为了实现这一点，在原始点云构建的占据栅格地图上构建 esdf(euclidean signed distance functions)地图，并选取一个合适的截断距离，生成tsdf (truncated signed distance function)地图：
[0121][0122]
其中d
thr
是安全距离阈值，d(p)是tsdf函数值，为第j段轨迹第i个采样点；
[0123]
基于所述tsdf地图，构造无碰惩罚函数j
ct
：
[0124][0125]
除此之外，为了判定地形危险区域，对原始点云各处进行地形分析：利用ransac算法 对每一处位置的机器人投射位姿进行计算，所谓投射位姿，是指空间某一点向下投射到最近 的表面处的机器人位姿，这样做可以计算出整个三维空间中的地形惩罚函数，从而与轨迹的 维度相同，而地面机器人由于不能飞行，它的运动空间是整个三维空间的子集，因此上述算 法可以完全覆盖机器人的运动空间。
[0126]
设机器人的投射位姿函数为：
[0127][0128]
它表示在空间位置p处，机器人的投射位姿为其中x0,y0,z0表示机器人底 盘中心坐标，表示机器人倾斜角。得到位姿投射函数后，根据地形带来的额外危险区域分 析，可构造地形惩罚函数：
[0129][0130]
该函数的第一项，使得轨迹远离太过倾斜的区域，而第二项，使得轨迹远离陡峭的边缘 和导致机器人位姿剧烈变化的区域，λ是权重系数，在具体实施中，依照机器人的需求和经验 取值。
[0131]
最终，安全性惩罚函数如下：
[0132][0133]
惩罚函数构造完成之后，使用数值优化器对轨迹参数进行数值优化，这里我们选取 lbfgs数值优化算法。完成数值优化后，即可得到最终的轨迹。
[0134]
图3展示了某场景下危险性分析的结果，可以看出，在没有护栏的斜坡上，算法自动分 析出了危险的边缘地带。
[0135]
图4展示了前端搜索的效果，可以看出前端是几乎贴合地面的，能够给予后端优化一个 良好的初值。
[0136]
图5展示了最终轨迹规划的效果，得益于后端安全性约束与前端的良好初值，最终轨迹 是平滑安全易于执行的地面机器人轨迹。
[0137]
与前述的非平坦地面环境下的机器人导航规划方法的实施例相对应，本技术还提
供了非 平坦地面环境下的机器人导航规划装置的实施例。
[0138]
图6是根据一示例性实施例示出的一种非平坦地面环境下的机器人导航规划装置框图。
[0139]
参照图6，该装置可以包括：
[0140]
过滤模块21，用于获取原始点云，对所述原始点云进行可通行区域过滤；
[0141]
生成模块22，用于对原始点云进行下采样，生成占据栅格地图；
[0142]
构建模块23，用于对过滤后的点云进行下采样，构建前端路径搜索空间；
[0143]
路径搜索模块24，用于在构建模块中生成的前端路径搜索空间中，进行基于a*的图搜 索，得到前端路径；
[0144]
路径优化模块25，用于基于路径搜索模块得到的前端路径生成轨迹初值，根据所述占据 栅格地图构造惩罚函数，利用凸优化算法生成最终轨迹，其中所述惩罚函数包括用于使得轨 迹平滑的平滑性惩罚函数、用于控制轨迹总时长的总时间惩罚函数、用于使得轨迹上任意时 刻的机器人期望速度、期望加速度满足机器人自身的动力学限制的动力学惩罚函数以及用于 使得轨迹避开危险区域且不产生碰撞的安全性惩罚函数。
[0145]
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施 例中进行了详细描述，此处将不做详细阐述说明。
[0146]
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的 部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单 元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单 元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其 中的部分或者全部模块来实现本技术方案的目的。本领域普通技术人员在不付出创造性劳动 的情况下，即可以理解并实施。
[0147]
相应的，本技术还提供一种机器人，包括：一个或多个处理器；存储器，用于存储一个 或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处 理器实现如上述的非平坦地面环境下的机器人导航规划方法。如图7所示，为本发明实施例 提供的一种非平坦地面环境下的机器人导航规划方法所在任意具备数据处理能力的设备的一 种硬件结构图，除了图7所示的处理器、内存以及网络接口之外，实施例中装置所在的任意 具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括 其他硬件，对此不再赘述。
[0148]
相应的，本技术还提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处 理器执行时实现如上述的非平坦地面环境下的机器人导航规划方法。所述计算机可读存储介 质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或 内存。所述计算机可读存储介质也可以是外部存储设备，例如所述设备上配备的插接式硬盘、 智能存储卡(smart media card，smc)、sd卡、闪存卡(flash card)等。进一步的，所述 计算机可读存储介还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存 储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力 的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
[0149]
本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本技术的其
它实施 方案。本技术旨在涵盖本技术的任何变型、用途或者适应性变化，这些变型、用途或者适应 性变化遵循本技术的一般性原理并包括本技术未公开的本技术领域中的公知常识或惯用技术 手段。
[0150]
应当理解的是，本技术并不局限于上面已经描述并在附图中示出的精确结构，并且可以 在不脱离其范围进行各种修改和改变。