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

本发明专利技术公开了非平坦地面环境下的机器人导航规划方法、装置及机器人，该方法包括：获取原始点云，对所述原始点云进行可通行区域过滤；对原始点云进行下采样，生成占据栅格地图；对过滤后的点云进行下采样，构建前端路径搜索空间；在生成的前端路径搜索空间中，进行基于A*的图搜索，得到前端路径；基于得到的前端路径生成轨迹初值，根据占据栅格地图构造惩罚函数，利用凸优化算法生成最终轨迹，其中惩罚函数包括用于使得轨迹平滑的平滑性惩罚函数、用于控制轨迹总时长的总时间惩罚函数、用于使得轨迹上任意时刻的机器人期望速度、期望加速度满足机器人自身的动力学限制的动力学惩罚函数以及用于使得轨迹避开危险区域且不产生碰撞的安全性惩罚函数。撞的安全性惩罚函数。撞的安全性惩罚函数。

【技术实现步骤摘要】
非平坦地面环境下的机器人导航规划方法、装置及机器人


[0001]本专利技术属于自主无人系统
，尤其涉及非平坦地面环境下的机器人导航规划方法、 装置及机器人。

技术介绍

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

技术实现思路

[0006]本申请实施例的目的是提供非平坦地面环境下的机器人导航规划方法、装置及机器人， 以解决相关技术中存在的针对二维平面规划存在的环境表示不准确问题，直接点云上进行导 航存在的计算量大实时性不强、轨迹平滑性差、缺乏时间信息且能耗高的问题，利用结构化 栅格地图，利用基于优化的轨迹方法，通过构建符合地面机器人的特殊安全性约束，实现了 针对地面机器人在非平坦地面环境下的光滑无碰能量最优轨迹规划。
[0007]根据本申请实施例的第一方面，提供一种非平坦地面环境下的机器人导航规划方法，包 括：
[0008]步骤(1)：获取原始点云，对所述原始点云进行可通行区域过滤；
[0009]步骤(2)：对原始点云进行下采样，生成占据栅格地图；
[0010]步骤(3)：对过滤后的点云进行下采样，构建前端路径搜索空间；
[0011]步骤(4)：在步骤(3)中生成的前端路径搜索空间中，进行基于A*的图搜索，得到前端 路径；
[0012]步骤(5)：基于步骤(4)得到的前端路径生成轨迹初值，根据所述占据栅格地图构造惩 罚函数，利用凸优化算法生成最终轨迹，其中所述惩罚函数包括用于使得轨迹平滑的平滑性 惩罚函数、用于控制轨迹总时长的总时间惩罚函数、用于使得轨迹上任意时刻的机器人期望 速度、期望加速度满足机器人自身的动力学限制的动力学惩罚函数以及用于使得轨迹避开危 险区域且不产生碰撞的安全性惩罚函数。
[0013]进一步地，所述步骤(1)包括：
[0014](1.1)遍历原始点云中的每一个点m，在该点m附近搜索半径为r的领域点；
[0015](1.2)遍历邻域点构成的集合Dr中的每一个点d，求出点d与点m的高度差z
dm
以及连 线与水平面的夹角α，计算方式如下：
[0016]z
dm
＝d3‑
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]进一步地，所述平滑性惩罚函数J
e
用于使得轨迹尽可能平滑，保证能量最优性：
[0029][0030]其中p(t)是轨迹方程，M为轨迹段数。
[0031]进一步地，所述总时间惩罚函数J
t
用于使得轨迹的总时间尽可能短：
[0032][0033]其中T
i
为第i段轨迹的总时长，M为轨迹段数。
[0034]进一步地，所述动力学惩罚函数J
d
用于使得轨迹上任意时刻的机器人期望速度、期望加速 度满足机器人自身的动力学限制：
[0035][0036]其中C(.)＝max{.,0}3为立方罚函数，为期望速度，为期望加速度，M为轨迹段数， v
max
和a
max
分别是最大速度限制和最大加速度限制，k
i
为第i段轨迹的采样个数：
[0037][0038][0039]式中，β为多项式轨迹p(t)＝c
T
β(t)的基，c为多项式系数。
[0040]进一步地，所述安全性惩罚函数J
c
用于使得轨迹远离危险区域，并且不会产生碰撞，所述 安全性惩罚函数的构造过程为：
[0041]在原始点云构建的占据栅格地图上构建ESDF地图，并选取一个合适的截断距离，生成 TSDF地图：
[0042][0043]其中d
thr
是安全距离阈值，d(p)是TSDF函数值，用来惩罚轨迹上距离障碍物太近的点，为第j段轨迹第i个采样点；
[0044]基本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种非平坦地面环境下的机器人导航规划方法，其特征在于，包括：步骤(1)：获取原始点云，对所述原始点云进行可通行区域过滤；步骤(2)：对原始点云进行下采样，生成占据栅格地图；步骤(3)：对过滤后的点云进行下采样，构建前端路径搜索空间；步骤(4)：在步骤(3)中生成的前端路径搜索空间中，进行基于A*的图搜索，得到前端路径；步骤(5)：基于步骤(4)得到的前端路径生成轨迹初值，根据所述占据栅格地图构造惩罚函数，利用凸优化算法生成最终轨迹，其中所述惩罚函数包括用于使得轨迹平滑的平滑性惩罚函数、用于控制轨迹总时长的总时间惩罚函数、用于使得轨迹上任意时刻的机器人期望速度、期望加速度满足机器人自身的动力学限制的动力学惩罚函数以及用于使得轨迹避开危险区域且不产生碰撞的安全性惩罚函数。2.根据权利要求1所述的方法，其特征在于，所述步骤(1)包括：(1.1)遍历原始点云中的每一个点m，在该点m附近搜索半径为r的领域点；(1.2)遍历邻域点构成的集合Dr中的每一个点d，求出点d与点m的高度差z
dm
以及连线与水平面的夹角α，计算方式如下：Z
dm
＝d3‑
m3其中，d3和m3分别指点d和m坐标的z轴分量；(1.3)判断点m是否符合可通行区域条件，其中判断条件为：对Dr中任意点d，都不存在z
dm
＜0且α＜θ的情况，如果满足该条件，则点m在可通行区域Ms内，否则过滤掉点m，其中θ是机器人最大安全站立倾斜角。3.根据权利要求1所述的方法，其特征在于，所述步骤(2)包括：计算所述原始点云中每个点对应的栅格下标：其中，floor函数表示向下取整，res
occ
为栅格地图的分辨率，bound
min
和bound
max
为原始点云在三维空间的边界范围；根据每个点对应的栅格下标，判断落入每个栅格中的点的个数，如果落入一栅格中的点数大于预定阈值，该栅格的值设为1，否则设为0，以完成对栅格的重新赋值，从而生成占据栅格地图。4.根据权利要求1所述的方法，其特征在于，所述步骤(3)包括：利用与步骤(2)相同的方式，对过滤后的点云Ms构建占据栅格地图；对每个过滤后的点云Ms中被占据的栅格进行遍历，判断每个被占据的栅格的上方栅格是否为非占据栅格，如果是，则将该非占据栅格划定在前端路径搜索空间中，从而完成前端路径搜索空间的构建。5.根据权利要求1所述的方法，其特征在于，所述平滑性惩罚函数J
e
用于使得轨迹尽可能平滑，保证能量最优性：
其中p(t)是轨迹方程，M为轨迹段数。6.根据权利要求1所述的方法，其特征在于，所述总时间惩罚函数J
t
用于使得...

【专利技术属性】
技术研发人员：高飞，王敬平，徐隆，付皓然，
申请(专利权)人：浙江大学，
类型：发明
国别省市：