CN115933648A - 一种机器人动态避障方法及系统 - Google Patents

Abstract

本发明公开了一种机器人动态避障方法及系统，包括：S1检测周围环境，初始化确定起始点和目标点，并生成点云；S2将点云输入同步定位与地图绘制模块，生成机器人位姿，并维护三维概率图；S3根据快速随机搜索树和三维概率图生成全局路径，根据点云生成局部地图与障碍物预测轨迹；S4根据障碍物预测轨迹和全局路径生成局部轨迹，并发送运动指令至机器人；S5机器人根据运动指令沿局部轨迹移动；本发明通过根据点云生成局部地图与障碍物预测轨迹，根据障碍物预测轨迹和全局路径生成局部轨迹的设置，预测了障碍物的轨迹，因此能够预先做出规避动作，具有更好的安全性；相比于模型预测控制+障碍物曲线拟合的方法，本发明的障碍物预测的准确性较高。

Description

一种机器人动态避障方法及系统

技术领域

本发明涉及机器人的运动规划领域，特别是涉及一种机器人动态避障方法及系统。

背景技术

针对路灯，路障等静态障碍物和行人，机动车，狗等日常生活中常见的动态障碍物，需要一个具有鲁棒性，安全性同时兼顾效率的路径规划算法来使得无人车能够规划一条无碰撞的路径从而避开这两类障碍物。因此，开发这种规避动态障碍物的路径规划算法，并将这种算法应用到无人车上去，将对商场，校园，机场，办公场所等场景具有极大的应用价值。

机器人的运动规划通常可以划分为全局规划和局部规划。全局规划是指在大尺度的静态场景(如大于100m*100m)下生成一条最优(如长度最短或耗时最少)的参考路径；局部规划是指在较小尺度的场景下(如小于100m*100m)，能够避开复杂的障碍物，并生成一条无碰撞的运动轨迹。相对来说，全局规划的参考轨迹的生成较为费时，因此更新频率较低，而局部规划确保了机器人的安全性，通过生成实时的运动指令驱动机器人运动。

局部规划根据实现方法的不同可以分为基于搜索的、基于采样的和基于优化的方法。基于搜索的算法包括：混合A-star算法，DWA算法(dynamic window approach)；基于采样的算法包括混合RRT算法(Rapidly-exploring Random Trees)，混合RRT-star算法等；基于优化的算法包括MPC算法(Model Predictive Control模型预测控制)，LQR算法(Linear–quadratic regulator，线性二次调解器)等。

现有技术公开了一种动态环境下基于改进DWA算法的AGV(Automated GuidedVehicle，自动导引车)路径规划方法，属于AGV路径规划技术领域；该方法首先通过获取AGV周围的局部地图信息设计局部DWA算法，对局部地图内的动态障碍物进行分类，评估动态障碍物速度，提升AGV对动态障碍物的避障判断能力，降低AGV偏离最短路径的程度；其次针对DWA算法移动到路径点需要调整方向的问题，通过优化DWA评价函数，缩短AGV在路径点的调整时间，并且通过改进的DWA算法对AGV全局路径的拐点进行优化，提高AGV运输效率；最后进行仿真验证，仿真结果表明该方法提出的改进DWA算法在动态情况复杂的环境下，能够保证AGV实时避障，同时缩短了AGV的运输时间，提升了运输效率。

但是DWA的算法原理主要是在速度空间中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹。由于采样的机制，并不能保证路径的最优性；对于动态障碍物状态的判断过于粗糙，不能很准确的得到障碍物在未来时域的速度和位置，这使得避障的时候安全性降低。

现有技术还提供了一种基于动态障碍物运动信息的室内移动机器人局部轨迹规划方法，其特征在于：在结合AMCL(adaptive Monte Carlo Localization，自适应蒙特卡洛定位)算法基础上通过单线激光雷达获取所有障碍物点云的位置信息，再对所有激光点云进行分割与直线拟合并去除地图中已知的障碍，并用合适大小的圆圈标记新出现的可能的动态障碍，再通过圆圈圆心坐标变化进行动态障碍物检测，不断获取动态障碍的圆心坐标并利用最小二乘求解其运动方程，最后扩大动态障碍物速度方向代价地图，通过结合DWA算法实现安全避开动态障碍。针对目前的局部轨迹规划算法在躲避动障碍物时都是将其当成瞬时的静态障碍物进行避障，缺乏主动性与安全性，利用该方法能够更安全的避障。

但是采用最小二乘的算法拟合出障碍物的方法缺少鲁棒性，当障碍物的位置噪声比较大的时候，常常会出现预测不准确的现象；直接将障碍物轨迹放进代价地图再使用DWA的算法进行规划的方案过于保守，忽略了障碍物的实时性，能够确保安全却减少了路径的最优性。

发明内容

本发明的目的在于解决提高机器人动态避障安全性的问题。

为此，本发明提出一种机器人动态避障方法，包括如下步骤：S1、检测周围环境，初始化确定起始点和目标点，并生成点云；S2、将所述点云输入同步定位与地图绘制模块中，从而生成机器人位姿，并维护三维概率图；S3、根据快速随机搜索树和三维概率图生成全局路径，根据所述点云生成局部地图与障碍物预测轨迹；S4、根据所述障碍物预测轨迹和所述全局路径生成局部轨迹，并发送运动指令至机器人；S5、所述机器人根据所述运动指令沿所述局部轨迹移动。

在本发明的一些实施例中，步骤S3还包括根据点云生成局部地图；通过聚类将障碍物参数化到所述局部地图上，生成障碍物最小包络椭圆；通过卡尔曼滤波，估计和预测所述障碍物最小包络椭圆的状态，在前向时域内生成障碍物预测轨迹。

在本发明的一些实施例中，步骤S4还包括根据所述障碍物预测轨迹和所述全局路径进行运动规划，将动态控制障碍函数和模型预测控制算法相结合，生成所述局部轨迹，并发送运动指令。

在本发明的一些实施例中，所述障碍物最小包络椭圆的状态包括形状和位置；对所述障碍物最小包络椭圆的位置协方差R_p提出修正参数k，表达式如下：

其中R_p，max和R_p，min表示位置方差的边界，Ξ_min，crit和Ξ_max，crit表示障碍物最小包络椭圆位置置信度的指标Ξ_p的边界；根据估计的所述障碍物最小包络椭圆的状态和方差值，预测未来时域的所述障碍物最小包络椭圆的状态，表达式如下：

x_k＝Ax_k-1.

P_k＝AP_k-1A^T+Q.

其中A为障碍物运动方程，x_k为障碍物在k时刻的状态，x_k-1为障碍物在k-1时刻的状态，P_k为障碍物在k时刻的方差，P_k-1为障碍物在k-1时刻的方差，Q为系统噪声的协方差。

在本发明的一些实施例中，定义用于评估MBE位置置信度的指标Ξ_p，表达式如下：

其中x_ob，t-i，t和

分别表示时间t时，r为上标，表示真实的，向后时域第i步处障碍物的最小包络椭圆位置和真实位置；定义Ξ_η来评估最小包络椭圆的形状变化程度，时域设置为m步，Ξ_η的估计如下：

其中

表示η的平均值，Ξ_η用于估计Ξ_p，公式如下：

其中，κ和γ分别是比例系数和幂次系数。

在本发明的一些实施例中，用不确定度扩展椭圆，椭圆

在两个轴上被σ放大到

其中σ为椭圆轴长的扩展量；定义r＝r_p+r_η来描述椭圆的不确定度，其中r_p和r_η分别由椭圆的位置和形状的协方差得到；为了找到包络闵可夫斯基和的最小椭圆，通过求解以下方程得到σ的最小值：

对于预测时域内的障碍物O(t：t+N|t)，当k从0增加到N时，障碍物的不确定性增加，相应的椭圆也相应扩大。

在本发明的一些实施例中，所述动态控制障碍函数表达式如下：

其中X为变量，h是连续可微函数，表达式如下：

安全集C为连续可微函数的下水平集，表达式为

其中

为状态集；所述动态控制障碍函数对于所有

存在一个扩展类

函数γ，表达式如下：

其中u为输入，s.t.为使得。

在本发明的一些实施例中，将机器人的中心与障碍物最小包络椭圆连接，计算所述障碍物最小包络椭圆轮廓与所述机器人的距离；所述障碍物最小包络椭圆轮廓与机器人的距离集合扫过所述控制障碍函数区域，并受到所述控制障碍函数边界的约束，防止机器人过快接近障碍物；

所述动态控制障碍函数表达式如下：

Δh(X_k，u_k)≥-γh(Xk₎，0＜γ≤1；

Δh(X_k，u_k)：＝h(X_k+1)-h(X_k)；

其中u_k为控制器输入，X_k为状态变量，γ为扩展类函数；每个障碍物表示为区域

的最小包络椭圆，表达式如下：

在k时刻，计算所述机器人和所述障碍物最小包络椭圆的外围的距离，表达式如下：

其中l_i(k)为机器人和所述障碍物最小包络椭圆的外围的距离，a_i(k)为障碍物椭圆半长轴，b_i(k)为障碍物椭圆半短轴，δ表示直线与椭圆的长轴形成的角度；所述动态控制障碍函数的二次形式表示如下：

其中p(k)为机器人k时刻的状态，xⁱ _ob(k)为障碍物最小包络椭圆k时刻的，l_i(k)为，d_safe为安全距离。

在本发明的一些实施例中，步骤S3中所述快速随机搜索树采用基于平面拟合的非平坦地形导航框架中基于平面拟合的快速随机搜索树。

在本发明的一些实施例中，采用模型预测控制算法结合所述动态控制障碍函数建立模型预测控制算法和动态控制障碍函数框架，所述模型预测控制算法和动态控制障碍函数框架表达式如下：

s.t.x_k+1＝f(x_k，u_k)，k＝0，...，N-1

x_t|t＝x_t，

Δh(X_k，u_k)≥-γh(X_k)，k＝0，...，N-1

其中

为状态集，

为输入状态集，

为终端状态集，J为优化目标函数，x_t+1＝f(x_t，u_t)为机器人的动态模型，全局规划器给出的参考路径为

终端代价为

过程代价为

其中P，Q，R，S为权重矩阵，

是系统在

处的状态，

代表机器人的控制输入。

本发明还提供一种机器人动态避障系统，包括：传感器，能实现上述的一种机器人动态避障方法，还包括：同步定位与地图绘制模块、全局规划模块、局部感知模块、局部规划模块；所述传感器用于检测周围环境，生成点云；所述同步定位与地图绘制模块根据所述点云生成机器人位姿，并维护三维概率图；所述全局规划模块根据快速随机搜索树和三维概率图生成全局路径；所述局部感知模块根据所述点云生成局部地图与障碍物预测轨迹；所述局部规划模块根据所述障碍物预测轨迹和所述全局路径生成局部轨迹，并发送运动指令至机器人。

本发明具有如下有益效果：

本发明通过根据点云生成局部地图与障碍物预测轨迹，根据障碍物预测轨迹和全局路径生成局部轨迹的设置，预测了障碍物的轨迹，因此能够预先做出规避动作，具有更好的安全性；相比于模型预测控制+障碍物曲线拟合的方法，本发明的障碍物预测的准确性较高。

本发明实施例中的其他有益效果将在下文中进一步述及。

附图说明

图1是本发明实施例中工作流程示意图；

图2是本发明实施例中系统组成示意图；

图3是本发明实施例中局部感知流程图；

图4a是本发明实施例中拟合结果示意图；

图4b是本发明实施例中圆柱障碍物在静止条件下置信的变化曲线图；

图4c是本发明实施例中圆柱障碍物在运动条件下置信的变化曲线图；

图5是本发明实施例中MPC-D-CBF示例图示意图；

图6a是本发明实施例中算法机器人部署前视图；

图6b是本发明实施例中算法机器人部署侧视图；

图7是本发明实施例中机器人避障效果图。

附图标记如下：

1为RS-Helios 32线激光雷达、2为WiFi、3为机载电脑、4为电源

具体实施方式

下面对照附图并结合优选的实施方式对本发明作进一步说明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

需要说明的是，本实施例中的左、右、上、下等方位用语，仅是互为相对概念，或是以产品的正常使用状态为参考的，而不应该认为是具有限制性的。

局部规划根据实现方法的不同可以分为基于搜索的、基于采样的和基于优化的方法。基于搜索的算法包括：混合A-star算法，DWA算法(dynamic window approach)；基于采样的算法包括混合RRT算法(Rapidly-exploring Random Trees)，混合RRT-star算法等；基于优化的算法包括MPC算法(Model Predictive Control模型预测控制)，LQR算法(Linear–quadratic regulator，线性二次调解器)等。相对来说，基于搜索的算法较为耗时，但是能够得到一个路径的最优解；基于采样的算法通常不具有路径的最优性，但耗时较少，其中混合RRT-star算法可以通过不断的迭代路径从而使得算法具有渐进最优性；基于优化的算法通过梯度下降法，得到机器人的未来时域的轨迹，同时通过界定优化参量的范围，可以避开各种障碍物。随着计算机算力的提升，这类算法近年来应用越来越多。

在介绍具体实施方式之前，先对本发明的思路介绍如下：

近几十年来，移动机器人安全临界最优规划与控制的研究一直在积极开展。通过改进现有自主导航解决方案的鲁棒性和效率，机器人自主导航已被广泛应用于许多领域，但在动态和非结构化环境下的自主仍然是一个挑战。难点主要体现在以下几个方面：

1、非结构化环境中障碍物的稳定检测与预测；

2、障碍的参数表示与不确定性分析；

3、障碍物实时运动轨迹的运动规划算法。

本发明通过结合全局规划和局部规划，生成一种两层(Two-Level)的运动控制的框架，可以使得机器人在运动过程中，能够保证路径最优的同时，能够避开障碍物从而确保安全性。本发明提出的机器人动态避障方法能实现以下功能：

1、实时的对障碍物进行探测跟踪的能力；

2、实时的估计障碍物的形状和位置，速度的能力；

3、实时预测障碍物的状态的能力；

4、实时的在跟随全局路径的同时，具有躲避静态和动态障碍物的能力；

5、根据具体应用场景，改变机器人和障碍物的最小安全距离的能力。

本发明下述实施例提出了一种机器人动态避障方法。

本发明实施例机器人动态避障方法的工作流程如图1所示：

S1、检测周围环境，初始化确定起始点和目标点，并生成点云；

S2、将所述点云输入同步定位与地图绘制模块中，从而生成机器人位姿，并维护三维概率图；

S3、根据快速随机搜索树和三维概率图生成全局路径，根据所述点云生成局部地图与障碍物预测轨迹；

S4、根据所述障碍物预测轨迹和所述全局路径生成局部轨迹，并发送运动指令至机器人；

S5、所述机器人根据所述运动指令沿所述局部轨迹移动。

本发明实施例提供一种机器人动态避障系统，包括同步定位与地图绘制模块(SLAM)模块，全局规划模块，局部规划模块，局部感知模块。如图2所示，从左至右：激光雷达传感器检测周围环境，生成点云。SLAM模块接收点云生成机器人位姿实时维护三维概率图。全局规划模块基于PUTN算法中的PF-RRT*和地图生成参考路径。点云用于生成局部地图。通过聚类将障碍物参数化到局部地图上，生成MBE。通过卡尔曼滤波，估计出MBE的当前运动状态，进而在前向时域内生成相应的运动轨迹。预测轨迹和参考路径被发送到局部规划模块进行运动规划。该模块将D-CBF和MPC相结合，生成机器人的安全轨迹，并实时发送运动指令。UGV接收运动指令沿轨迹移动。

本发明实施例还提供一种机器人动态避障系统，包括：传感器，能实现上述实施例中的机器人动态避障方法，还包括：同步定位与地图绘制模块、全局规划模块、局部感知模块、局部规划模块；所述传感器用于检测周围环境，生成点云；所述同步定位与地图绘制模块根据所述点云生成机器人位姿，并维护三维概率图；所述全局规划模块根据快速随机搜索树和三维概率图生成全局路径；所述局部感知模块根据所述点云生成局部地图与障碍物预测轨迹；所述局部规划模块根据所述障碍物预测轨迹和所述全局路径生成局部轨迹，并发送运动指令至机器人。

下面结合附图对本发明实施例的机器人动态避障方法进行进一步说明：

一、障碍物预测

如图3所示，局部感知在考虑观测的不确定性的情况下，通过预测障碍物轨迹，实时的划分出安全区域和障碍物区域。在这个过程中，通过实时点云生成高层图，在高层图中经过梯度处理和步高处理来得到局部地图。在局部地图中，进行障碍识别和数据关联。

由于点云噪声和障碍物检测角度的变化，MBE的形状η_ob＝[a，b，θ]，其中a，b，θ分别表示椭圆的半长轴，半短轴和偏移角度。会发生快速而巨大的变化，导致MBE的位置x_ob＝[x_ob，y_ob]偏离真实值。针对这一问题，本发明实施例提出了一个用于评估MBE位置置信度的指标Ξ_p。

其中x_ob，t-i，t和

分别表示时间t时(r为上标，表示真实的)，向后时域第i步处障碍物的MBE位置和真实位置。本发明实施例提出了Ξ_η来评估MBE的形状变化程度，时域设置为m步。在一般情况下，障碍物的形状不会迅速改变(例如，快速膨胀)。因此，在局部地图的有限区域内，可以认为障碍物的形状保持不变。Ξ_η的估计如下：

其中

表示η的平均值。

由于

在实际环境无法准确测量，Ξ_η可以用来估计Ξ_p。通过数据分析，本发明实施例使用公式

来完成这个估计任务，κ和γ分别是比例系数和幂次系数。为了验证

和Ξ_p的相关性，本发明实施例对不同速度和大小的障碍物进行了实验，如图4a所示，可以看出，当[κ，γ]＝[5.5，1.3]时，该公式可以很好地估计半径小于1.5m，速度小于0.9m/s的障碍物。

图4a显示κ为5.5，γ为1.3时

和Ξ_p的相关系数随障碍物速度和大小的变化而变化，其中横坐标表示障碍物的大小，纵坐标表示障碍物的速度，竖坐标表示

和Ξ_p的相关系数。图4b、图4c分别表示圆柱障碍物在静止和运动条件下置信的真值Ξ_p和估价值

的变化，其中横坐标T为时间间隔，纵坐标为方差。实验中圆柱体直径设置为1m，运动状态设置为0.5m/s的均匀线性运动。Ξ_p和

在障碍物处于静止状态时的相关系数为0.9133，在障碍物处于恒速度状态时的相关系数度为0.9059。

为了减小点云噪声的影响，本发明实施例使用KF更新障碍物的状态，同时使用MBEξ作为观测。状态变量被设置为一个向量

为9维变量状态转移矩阵和观测矩阵分别为：

其中T为时间间隔，I与O分别为单位矩阵和0矩阵。MBE位置的变化有两个原因：(a)障碍物本身的运动、(b)由于MBE形状η_ob的异常变化而导致的位置偏差。在之前分析的基础上，对位置协方差R_p提出了修正参数k，以减小异常的偏差：

其中R_p，max和R_p，min表示位置方差的边界，Ξ_min，crit和Ξ_max，crit表示Ξ_p的边界。这样，当MBE的形状发生快速变化时，R_p增加，以减小MBE位置的振动。根据当前估计的状态和相应的方差值，可以用x_k＝Ax_k-1和P_k＝AP_k-1A^T+Q来预测未来时域的状态，其中Q为系统噪声的协方差。

为了提高安全性，本发明实施例用不确定度来扩展椭圆。椭圆

在两个轴上被σ放大到

其中σ为椭圆轴长的扩展量。定义r＝r_p+r_η来描述椭圆的不确定度，其中r_p和r_η分别由椭圆的位置和形状的协方差得到。为了找到包络闵可夫斯基和的最小椭圆，通过求解以下方程得到σ的最小值

对于预测时域内的障碍物O(t：t+N|t)，当k从0增加到N时，障碍物的不确定性增加，相应的椭圆也相应扩大。

二、局部规划算法

已经证明CBF(Control Barrier Functions)可以用于UGV避障。然而，对于动态障碍物，传统CBF的安全性仍然是一个挑战。与CBF不同，D-CBF(动态控制障碍函数)认为障碍是可移动的。定义机器人位置，障碍物位置，障碍物形状为

对于临界安全控制，集合C定义为连续可微函数h：

的下水平集：

在该发明中，本发明实施例将C称为安全集。参考CBF的定义，D-CBF可以定义为：

对于所有

并且存在一个扩展类

函数γ：

根据定义，D-CBF是在

上的一个CBF。由于CBF定理，安全集C是前向不变且渐近稳定的。因此，C中机器人位置对应的分量集也是前向不变且渐近稳定的，这意味着控制系统是安全的。

如图5所示，机器人x(t：t+N|t)避免与预测障碍O_i(t：t+N|t)在未来N步的最优路径。将机器人的中心与椭圆连接，椭圆轮廓与机器人的距离可计算为d_i(t：t+N|t)。该距离集合扫过CBF区域，并受到CBF边界的约束，防止机器人过快接近障碍物。在离散系统中，D-CBF可以表述为

Δh(X_k，u_k)≥-γh(X_k)，0＜γ≤1

其中，Δh(X_k，u_k)：＝h(X_k+1)-h(X_k)。

每个动态障碍物表示为区域

的椭圆，可描述

和

在时间步骤t，所有障碍的集合描述为

由上一节的卡尔曼预测的推导可知，可以将障碍物O_i的正向时域N状态预测为O_i(t：t+N|t)。为了简化符号，本发明实施例用k代替t+k|t。通过连接O_i(k)的中心和机器人p(k)＝[x(k)，y(k)]，在k时刻，通过解直线和椭圆联立方程，计算l_i(k)从O_i(k)到其外围的距离：

其中δ表示直线与椭圆的长轴形成的角度。因此，D-CBF以二次形式表示如下：

其中d_safe为安全距离。

为了规划机器人未来的姿态，实现无碰撞的运动轨迹，本发明实施例采用MPC来实现，并结合D-MPC建立了MPC-D-CBF框架。为了建立MPC，本发明实施例首先用离散时间方程x_t+1＝f(x_t，u_t)来描述机器人的动态模型，其中

是系统在

处的状态。

代表机器人的控制输入

s.t.x_k+1＝f(x_k，u_k)，k＝0，...，N-1

x_t|t＝x_t，

Δh(X_k，u_k)≥-γh(X_k)，k＝0，...，N-1

其中

和

分别是状态集、输入状态集和终端状态集，J为优化目标函数，跟踪的全局规划器给出的参考路径为

终端代价

过程代价为

其中P，Q，R，S为权重矩阵。

三、机器人部署

本发明实施例的算法在ROS Melodic操作系统下工作。本地感知模块工作频率为10hz～20hz，本地规划模块工作频率为10hz。如图6a、图6b所示，Scout2.0是一个配备了电源4和Wi-Fi2的四轮差动移动机器人。唯一使用的传感器是RS-Helios 32线激光雷达1，垂直视场70°，视场低于地平线55°，以消除盲区。两台机载电脑3为Intel@NUC(i52.4GHz CPU和16GB内存)分别用于运行规划算法和SLAM算法。

如图7所示，带点线表示PF-RRT*生成的全局路径。方框分别代表MPC-D-CBF规划的机器人的姿态。场景中演示了两个行人和电动汽车的轨迹，MBE的不确定性从小到大被表示为颜色变深。椭圆柱序列表示障碍物轨迹的预测。

在这种情况下，当机器人遇到障碍物(行人、电动汽车)时，本发明实施例的算法会根据障碍物的预测轨迹生成一个轨迹。例如，在(a)中，当行人仍然在机器人的左侧时，本发明实施例的算法预测它在接下来的N步中的状态将是向右的轨迹。所以机器人采用从左侧回避的策略。这样，机器人不仅可以提高运动的安全性，而且可以在更短的时间内到达目标点。

本发明实施例具有以下优点：

1、机器人能够在本发明实施例算法的控制下完成对于复杂动态障碍物的规避；

2、机器人能够在规避障碍物的前提下尽可能的减少时间优化路径；

3、预测了障碍物的轨迹和机器人的轨迹提高了安全性。

相对于传统的模型预测控制和基于传统控制障碍函数的模型预测控制方法，本发明预测了障碍物的轨迹，因此能够预先做出规避动作，具有更好的安全性；相比于模型预测控制+障碍物曲线拟合的方法，本发明的障碍物预测的准确性较高。

本发明实施例还具有以下优点：

1、提出D-CBF(动态控制障碍函数)扩展了传统CBF的安全性。

2、使用D-CBF和模型预测控制结合完成动态避障的任务。

3、通过带有不确定性的卡尔曼滤波来估计和预测障碍物的形态和位置。

4、提出了一个函数通过MBE形态的变化来估计MBE位置的不确定性。

5、将pf-RRT*算法和MPC-D-CBF结合完成了全局规划和局部规划的配合。

本发明实施例涉及的缩略语和关键术语定义如下：

同步定位与地图绘制(Simultaneous LocalizationAnd Mapping，简称SLAM)

基于平面拟合的非平坦地形导航框架(Plane-fitting based Uneven TerrainNavigation Framework，简称PUTN)

基于平面拟合的快速随机搜索树(Plane-Fitting based Optimal Rapidly-Exploring Random Trees，简称PF-RRT*)

控制障碍函数(Control Barrier Function，简称CBF)

动态控制障碍函数(Dynamic Control Barrier Function，简称D-CBF)

模型预测控制(Model Predictive Control，简称MPC)

快速激光里程计(Fast lidar Odometry，简称FAST-LO)

八叉树(Octomap)

最小包络椭圆(minimumbounding ellipses，简称MBE)

无人驾驶地面车辆(Unmanned GroundVehicle，简称UGV)

卡尔曼滤波器(Kalman Filter，简称KF)

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (11)

1.一种机器人动态避障方法，其特征在于，包括以下步骤：

S1、检测周围环境，初始化确定起始点和目标点，并生成点云；

S2、将所述点云输入同步定位与地图绘制模块中，从而生成机器人位姿，并维护三维概率图；

S3、根据快速随机搜索树和三维概率图生成全局路径，根据所述点云生成局部地图与障碍物预测轨迹；

S4、根据所述障碍物预测轨迹和所述全局路径生成局部轨迹，并发送运动指令至机器人；

S5、所述机器人根据所述运动指令沿所述局部轨迹移动。

2.如权利要求1所述的方法，其特征在于，步骤S3还包括根据点云生成局部地图；通过聚类将障碍物参数化到所述局部地图上，生成障碍物最小包络椭圆；通过卡尔曼滤波，估计和预测所述障碍物最小包络椭圆的状态，在前向时域内生成障碍物预测轨迹。

3.如权利要求1所述的方法，其特征在于，步骤S4还包括根据所述障碍物预测轨迹和所述全局路径进行运动规划，将动态控制障碍函数和模型预测控制算法相结合，生成所述局部轨迹，并发送运动指令。

4.如权利要求2所述的方法，其特征在于，所述障碍物最小包络椭圆的状态包括形状和位置；对所述障碍物最小包络椭圆的位置协方差R_p提出修正参数k，表达式如下：

其中R_p，max和R_p，min表示位置方差的边界，Ξ_min，crit和Ξ_max，crit表示障碍物最小包络椭圆位置置信度的指标Ξ_p的边界；

根据估计的所述障碍物最小包络椭圆的状态和方差值，预测未来时域的所述障碍物最小包络椭圆的状态，表达式如下：

x_k＝Ax_k-1；

P_k＝AP_k-1A^T+Q；

其中A为障碍物运动方程，x_k为障碍物在k时刻的状态，x_k-1为障碍物在k-1时刻的状态，P_k为障碍物在k时刻的方差，P_k-1为障碍物在k-1时刻的方差，Q为系统噪声的协方差。

5.如权利要求4所述的方法，其特征在于，定义用于评估MBE位置置信度的指标Ξ_p，表达式如下：

其中x_ob，t-i，t和

分别表示时间t时，r为上标，表示真实的，向后时域第i步处障碍物的最小包络椭圆位置和真实位置；

定义Ξ_η来评估最小包络椭圆的形状变化程度，时域设置为m步，Ξ_η的估计如下：

其中

表示η的平均值，Ξ_η用于估计Ξ_p，公式如下：

其中，κ和γ分别是比例系数和幂次系数。

6.如权利要求5所述的方法，其特征在于，用不确定度扩展椭圆，椭圆

在两个轴上被σ放大到

其中σ为椭圆轴长的扩展量；

定义r＝r_p+r_η来描述椭圆的不确定度，其中r_p和r_η分别由椭圆的位置和形状的协方差得到；为了找到包络闵可夫斯基和的最小椭圆，通过求解以下方程得到σ的最小值：

对于预测时域内的障碍物O(t：t+N|t)，当k从0增加到N时，障碍物的不确定性增加，相应的椭圆也相应扩大。

7.如权利要求3所述的方法，其特征在于，所述动态控制障碍函数表达式如下：

其中X为变量，h是连续可微函数，表达式如下：

安全集C为连续可微函数的下水平集，表达式为

其中

为状态集；

所述动态控制障碍函数对于所有

存在一个扩展类

函数γ，表达式如下：

其中u为输入，s.t.为使得。

8.如权利要求6所述的方法，其特征在于，将机器人的中心与障碍物最小包络椭圆连接，计算所述障碍物最小包络椭圆轮廓与所述机器人的距离；所述障碍物最小包络椭圆轮廓与机器人的距离集合扫过所述控制障碍函数区域，并受到所述控制障碍函数边界的约束，防止机器人过快接近障碍物；

所述动态控制障碍函数表达式如下：

Δh(X_k，u_k)≥-γh(X_k)，0＜γ≤1；

Δh(X_k，u_k)：＝h(X_k+1)-h(X_k)；

其中u_k为控制器输入，X_k为状态变量，γ为扩展类函数；

每个障碍物表示为区域

的最小包络椭圆，表达式如下：

在k时刻，计算所述机器人和所述障碍物最小包络椭圆的外围的距离，表达式如下：

其中l_i(k)为机器人和所述障碍物最小包络椭圆的外围的距离，a_i(k)为障碍物椭圆半长轴，b_i(k)为障碍物椭圆半短轴，δ表示直线与椭圆的长轴形成的角度；

所述动态控制障碍函数的二次形式表示如下：

其中p(k)为机器人k时刻的状态，xⁱ _ob(k)为障碍物最小包络椭圆k时刻的，l_i(k)为，d_safe为安全距离。

9.如权利要求1所述的方法，其特征在于，步骤S3中所述快速随机搜索树采用基于平面拟合的非平坦地形导航框架中基于平面拟合的快速随机搜索树。

10.如权利要求9所述的方法，其特征在于，采用模型预测控制算法结合所述动态控制障碍函数建立模型预测控制算法和动态控制障碍函数框架，所述模型预测控制算法和动态控制障碍函数框架表达式如下：

s.t.x_k+1＝f(x_k，u_k)，k＝0，...，N-1

x_t|t＝x_t，

Δh(X_k，u_k)≥-γh(X_k)，k＝0，...，N-1

其中

为状态集，

为输入状态集，

为终端状态集，J为优化目标函数，x_t+1＝f(x_t，u_t)为机器人的动态模型，全局规划器给出的参考路径为

终端代价为

过程代价为

其中P，Q，R，S为权重矩阵，

是系统在

处的状态，

代表机器人的控制输入。

11.一种机器人动态避障系统，包括：传感器，其特征在于，能实现如权利要求1-10任一项所述的一种机器人动态避障方法，还包括：同步定位与地图绘制模块、全局规划模块、局部感知模块、局部规划模块；

所述传感器用于检测周围环境，生成点云；

所述同步定位与地图绘制模块根据所述点云生成机器人位姿，并维护三维概率图；

所述全局规划模块根据快速随机搜索树和三维概率图生成全局路径；

所述局部感知模块根据所述点云生成局部地图与障碍物预测轨迹；

所述局部规划模块根据所述障碍物预测轨迹和所述全局路径生成局部轨迹，并发送运动指令至机器人。

Images