一种动态环境下趋势感知的机器人轨迹规划方法与流程

本发明属于机器人技术领域，尤其涉及一种动态环境下趋势感知的机器人轨迹规划方法，适用于机器人在动态场景下的实时路径规划与避障。

背景技术：

在近几十年的机器人导航的发展过程中，机器人轨迹规划是诸如自主运输和社会服务系统等实际应用中一项基础而必要的任务。已知静态地图中的路径规划技术在现阶段已经相对成熟与稳定，例如a*算法、d*算法、rrt算法和人工势场法等，然而机器人的真实运行环境往往充满了未知阻碍与动态障碍，在这种复杂情况下，纯路径规划方法通常会因为实时性不够高并无法预测动态障碍而与动态障碍碰撞。因此，规划带有位移、速度、加速度等有关时间信息的运动轨迹显得至关重要。

目前，轨迹规划方法的优劣一般由可行性、安全性、完备性、实时性和最优性度量。轨迹生成的可行性在于轨迹能否被机器人执行；安全性在于规划结果是否有效避开障碍物；完备性在于能否找到需要的轨迹；实时性在于轨迹规划耗时多少；最优性在于轨迹规划是否满足动力学约束、运动学约束和障碍约束等。

轨迹规划方法增加了时间维度，使得匀速直线运动的动态障碍一旦被获取速度信息，此规划方法即可预测该障碍的未来空间轨迹，从而规划生成一条避开该动态障碍未来路线的平滑轨迹。然而，避让一定密度的随机变速移动的动态障碍对于轨迹规划技术来说仍是一个艰难的挑战。特别是当机器人在运行轨道上遭遇随机穿越的障碍，尽管机器人自定位准确无误，普通的轨迹规划器仍不能保证能生成一个无碰撞的轨迹。主要有以下两个原因造成这种情况：

(1)目前仍难以获取未知障碍物精确的速度与加速度即使应用最先进的传感器与算法，其检测结果仍不可避免地会存在误差与噪声，以造成大部分轨迹规划器不能保持长期最优性而只能通过重新规划的方式应对最逼近的动态障碍。

(2)大多数轨迹规划方法只考虑障碍物目前的速度而忽略其他运动学信息。换句话说，障碍物的运动趋势是一个非常重要的信息，能应用于轨迹规划以避开碰撞。

技术实现要素：

针对目前在动态环境下机器人感知精度受限造成一般轨迹规划方法无法规划出一条安全并可行的轨迹，本发明的轨迹规划方法重视动态障碍物的运动学信息，旨在解决感知不精确与运动障碍带来的轨迹规划问题。

本发明的目的是提供一种动态环境下趋势感知的机器人轨迹规划方法，在维持机器人的运动动力学约束和障碍约束的基础上，其中运动动力学包括机器人运动学约束、速度约束和加速度约束，通过预测动态障碍物在一定时间范围内的运动趋势，在线修改全局路径规划器已经提前生成的初始可行路径，寻找出适于机器人执行的安全并最优的轨迹。

为了实现上述目的，本发明技术方案包括如下步骤：

一种动态环境下趋势感知的机器人轨迹规划方法，包括：

利用三维激光系统感知机器人状态和检测动态障碍物状态，在构形空间中，将机器人和动态障碍物作为一个质点，定位到同一个世界坐标系下；

对全局路径规划器规划的路径进行裁剪、离散化与时序化，使之变成一条离散的初始轨迹，通过感知估计动态障碍物的速度，预测动态障碍物的运动轨迹；

根据机器人的初始轨迹与动态障碍物的运动轨迹来构建两者的趋势轨迹，通过两者趋势轨迹的相交与重叠程度，构建约束条件；

将构建的约束条件映射到超图，转化为具有约束近似的无约束最小二乘优化问题并求解，对机器人轨迹进行优化。

进一步的，所述将机器人和动态障碍物作为一个质点，其中动态障碍物表示为一个向外膨胀的圆形。

进一步的，所述对全局路径规划器规划的路径进行裁剪、离散化与时序化，使之变成一条离散的初始轨迹，通过感知估计动态障碍物的速度，预测动态障碍物的运动轨迹，包括：

根据实时更新的机器人位置裁剪已经经过的路径，将剩余的路径作为构建初始轨迹的输入；

对裁剪后的路径进行离散化，机器人状态被表示为ci，其中ci由机器人位置xi，yi和朝向βi组成，离散化处理后的机器人状态的集合为：

r＝{ci}i＝0...n；

连续两个机器人状态通过时间间隔δti相互联系，t为时间间隔的集合，表示为：

t＝{δti}i＝0...n-1

获取的机器人初始轨迹p由机器人状态与时间间隔组成，表示为：

p＝{(c0，δt0，c1，δt1，c2，..，δtn-1，cn)}；

根据动态障碍物的初始位置与速度动态障碍物在t时刻的预测位置oj(t)为：

其中，n+1为初始轨迹中机器人状态离散点数量，i表示离散状态序号，j表示动态障碍物序号。

进一步的，所述根据机器人的初始轨迹与动态障碍物的运动轨迹来构建两者的趋势轨迹，通过两者趋势轨迹的相交与重叠程度，构建约束条件，包括：

根据机器人初始轨迹与动态障碍物运动轨迹，构建各自的趋势轨迹预测方程：

其中pi代表了某一时刻机器人在轨迹上的位置，由xi，yi组成，vi代表了机器人的速度，和分别是机器人与动态障碍物的趋势参数，和分别代表了预测轨迹后的机器人与动态障碍物的状态；

计算获取每个时刻的机器人状态与每个检测到的障碍物状态的距离：

δ(ci，oj(t))＝||oj(t)-pi||2；

该距离用于判断机器人与动态障碍物的远近程度，引出障碍约束zi：

zi(ci，t)≥0；

其中δmin是动态障碍物与机器人之间的最小空隙；

用σmin表示成机器人与障碍物之间保持安全间隙的一个最小交叉距离，构建障碍趋势约束：

构建运动动力学约束：

hi(ci+1，ci)＝0

vi(ci+1，ci，δti)≥0

αi(ci+1，ci，ci-1，δti+1，δti)≥0

其中hi是机器人的运动学约束，vi是速度约束，αi是加速度约束。

进一步的，将构建的约束条件映射到超图，转化为具有约束近似的无约束最小二乘优化问题，包括：

用p^*表示优化之后的轨迹，w^t表示衡量目标函数的权重因子，f(p)为目标函数，表示运动动力学约束、障碍约束和障碍趋势约束，所述具有约束近似的无约束最小二乘优化问题，表示为：

本发明提出的一种动态环境下趋势感知的机器人轨迹规划方法，与现有技术相比，本发明的有益效果如下：

地面机器人在广场或道路等场地自主执行任务时，会遭遇行人和行车等动态障碍物，此类动态障碍物有目标驱动的特性，容易发生穿越机器人预定轨迹的事件，因为其运动方向大致一定，所以通过机器人检测获取的障碍物速度可以预测其大致的运动轨迹。通过机器人与动态障碍物的趋势轨迹预测，引入障碍趋势约束，可以有效降低机器人与动态障碍物的碰撞概率与提高轨迹生成的稳定性，为自动驾驶、社会服务等情景提供了安全可靠的机器人运动轨迹。

附图说明

图1为本发明动态环境下趋势感知的机器人轨迹规划方法流程图；

图2为本发明实施例在动态环境下的实时轨迹规划结果；

图3为本发明实施例构建的局部超图结构。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅用以解释本申请，并不用于限定本申请。

如图1所示，给出了一种动态环境下趋势感知的机器人轨迹规划方法的实施例，包括：

步骤s1、利用三维激光系统感知机器人状态和检测动态障碍物状态，在构形空间中，将机器人和动态障碍物作为一个质点，定位到同一个世界坐标系下。

本实施例在动态未知环境下，部署一台机器人(也可以是其他形式的机器人，本申请对此不做限制)，该机器人上配置有一台桌面计算性能级的计算终端并在底盘中心支架装配有三维激光雷达，该激光雷达拥有360度水平及30度垂直视野，作为自身定位与障碍物感知系统的原始数据输入，其中机器人定位与二维地图构建由激光里程与建图算法提供，同时使用点云数据聚类的方法检测动态障碍物位置，根据连续两次对同个动态障碍物检测得出的位置与时间间隔，即可计算得到动态障碍物估计速度大小与朝向。本实施例机器人状态包括机器人的位置和朝向，动态障碍物状态包括动态障碍物的位置、速度和朝向。

本实施例将机器人与动态障碍物在构形空间中表示，构形空间是机器人系统所有可能构形的空间，其中机器人在构形空间中表示为一个质点，动态障碍物在该空间中表示为一个向外膨胀的圆形，即在动态障碍物上附加一个膨胀层，膨胀层的厚度可以设置，例如0.35米。在这个构形空间中在动态障碍物上附加一个膨胀层，可以简化机器人外形的异同带来的计算复杂化，忽略其外形以及尺寸来简化问题。

本实施例将机器人的三维激光雷达定位的初始坐标系作为世界坐标系，机器人和动态障碍物的位置与朝向将通过定位信息转换到同一个世界坐标系中。

如图2所示，本实施例的机器人r在构形空间，也就是执行任务的区域遭遇动态障碍物a、b时示意图，利用三维激光系统感知机器人状态和检测动态障碍物状态，在构形空间中，将机器人位置和动态障碍物实时位置作为一个质点，定位到同一个世界坐标系下。

步骤s2、对全局路径规划器规划的路径进行裁剪、离散化与时序化，使之变成一条离散的初始轨迹，通过感知估计动态障碍物的速度，预测动态障碍物的运动轨迹。路径规划是机器人导航最基本的环节，它指的是机器人在有障碍物的工作环境中，如何找到一条从起点到终点适当的运动路径，使机器人在运动过程中能安全、无碰撞地绕过所有障碍物。全局路径规划器是机器人进行路径规划的装置，给机器人规划一条路径，机器人根据规划的路径从一个点移动到另一个点。

本实施例机器人周围的环境信息作为静态障碍物表示在二维地图上，全局路径规划器根据用户给定的目标点与静态障碍物规划出一条可通行的路径，其中全局路径规划器的核心为a*算法，a*算法能在二维地图中找到一条从起始位置到目标位置并避开障碍物的最优路径。关于全局路径规划器进行路径规划，已经是比较成熟的技术，这里不再赘述。

本实施例中，对全局路径规划器规划的路径进行裁剪、离散化与时序化，使之变成一条离散的初始轨迹，包括：

步骤s2.1、根据实时更新的机器人位置裁剪已经经过的路径，将剩余的路径作为构建初始轨迹的输入。

在机器人前往目标地点的过程中，全局路径规划器规划有对应的路径，本实施例根据实时更新的机器人位置裁剪已经经过的路径，将剩余的路径作为构建初始轨迹的输入。

步骤s2.2、对裁剪后的路径进行离散化。

本实施例机器人状态被表示为ci，其中ci由机器人位置xi，yi和朝向βi组成，将步骤s2.1中裁剪后的路径以一定的距离离散化，并在各个离散点上添加默认的机器人朝向，从而构成离散的机器人状态，其机器人状态的集合为：

r＝{ci}i＝0...n

上述公式表示有n+1个机器人状态组成机器人轨迹，n+1为初始轨迹中机器人状态离散点数量，i表示离散状态序号。

步骤2.3、对裁剪后的路径进行时序化。

连续两个机器人状态通过时间间隔δti相互联系：

t＝{δti}i＝0...n-1

其中t为时间间隔的集合。

步骤2.4、获取机器人初始轨迹p为：

p＝{(c0，δt0，c1，δt1，c2，..，δtn-1，cn)}

获取的机器人初始轨迹由机器人状态与时间间隔组成。

步骤2.5、动态障碍物的轨迹预测。

假设在此动态场景中有n个动态障碍物，这些障碍物的绝对位置随着时间变换。时刻t表示为：

所以动态障碍物在t时刻的预测状态能被表示为：

oj(t)，j＝0，...，n

当动态障碍物的初始位置与速度被步骤s1中的方法测量得到，可以得到动态障碍物在t时刻的预测状态：

其中，j表示动态障碍物序号。即对于每一个被检测到的障碍物来说，在未来一段时间内，其运动趋势可以被简单预估，所以动态障碍物预测状态oj(t)与时间间隔δti可组成动态障碍物的运动轨迹，而且动态障碍物的初始位置会被实时更新以应对复杂的动态环境。

本实施例初始轨迹与运动轨迹的时间同步并保持一定时间范围t，初始轨迹与障碍运动轨迹都是在同一时刻开始预测的轨迹，预测的时间范围t由用户指定。预测的时间越长，轨迹越能保持最优，但计算能力需求越大，所以该时间范围将维持在适当的范围。

步骤s3、根据机器人的初始轨迹与动态障碍物的运动轨迹来构建两者的趋势轨迹，通过两者趋势轨迹的相交与重叠程度，构建约束条件。

本实施例引入障碍趋势约束，机器人轨迹将一并满足运动学约束、速度约束、加速度约束、障碍约束等，本实施例对具体的约束条件不做限制，本领域技术人员可以适当增加约束条件或减少约束条件。其中运动学约束、速度约束、加速度约束也可以统称为运动动力学约束。

步骤s3.1、根据机器人初始轨迹与动态障碍物运动轨迹，构建各自的趋势轨迹预测方程：

其中pi代表了某一时刻机器人在轨迹上的位置，由xi，yi组成，vi代表了机器人的速度，和分别是机器人与动态障碍物的趋势参数，其决定了趋势大小与朝向，和分别代表了预测趋势轨迹后的机器人与动态障碍物的状态。

步骤3.2、计算获取每个时刻的机器人状态与每个检测到的障碍物状态的距离：

δ(ci，oj(t))＝||oj(t)-pi||2；

距离用于判断机器人与动态障碍物的远近程度，引出障碍约束zi：

zi(ci，t)≥0；

其中δmin是动态障碍物与机器人之间的最小空隙，满足该不等式约束以保持机器人轨迹的安全性。

将两者之间每个状态的距离应用于如下激活函数：

其中γ∈[0，1]用于条件判断，δreq是一个经验的设定值，κ表示一个尺度因子。当γ不等于零时，意味着该预测时刻的机器人与障碍物之前的距离已经抵达设定阈值，需要进行下一阶段的双趋势轨迹相交程度判断以防止潜在的碰撞风险。然而，在判断双趋势轨迹相交性之前，通过预先检测机器人趋势轨迹与障碍物趋势轨迹是否平行与共线，在排除此种情况以后，轨迹相交情况可被计算为：

如果和大于等于零，该轨迹规划方法则认为机器人与障碍物存在潜在的碰撞。

只有满足障碍趋势约束：

在满足障碍趋势约束时，机器人产生碰撞的几率可被大量减少，其中σmin表示成机器人与障碍物之间保持安全间隙的一个最小交叉距离。

步骤s3.3、稳定可靠的轨迹优化方法还需要满足以下不等式约束：

hi(ci+1，ci)＝0

vi(ci+1，ci，δti)之0

αi(ci+1，ci，ci-1，δti+1，δti)≥0

其中hi是机器人的运动学约束，主要通过限制两个相邻的机器人状态都处在恒定曲率的共同弧长上来实现，vi是速度约束，限制机器人速度在一定范围内，αi是加速度约束，限制机器人加速度在正常范围内。

步骤4、将构建的约束条件映射到超图，转化为具有约束近似的无约束最小二乘优化问题并求解，对机器人轨迹进行优化。

本实施例机器人轨迹优化问题映射为一种超图的表达方式，如图3为简化的一部分超图，其中包括两个圆形的动态障碍o0(0)，o0(δt0)、两个机器人状态c0，c1和一个时间间隔δt0。动态障碍物的绝对位置随时间变化，但是其在超图里作为固定顶点，也就意味着其不能被优化程序所更改。趋势参数τi作为超图的边连接两个顶点作为目标的约束。

具体的，机器人轨迹优化问题被映射到超图成为一个具有约束近似的无约束最小二乘优化问题之后，此近似优化问题可以表示为：

其中p^*为优化之后的机器人可执行轨迹，w^t代表着衡量目标函数的权重因子。f(p)代表着目标函数包含了运动动力学约束、障碍约束和障碍趋势约束。此优化问题可由高效的非线性最小二乘求解器求解，例如levernberg-marquardt法。上述约束的条件都只依赖于很少的变量与参数，所以求解过程中的海森矩阵是稀疏的，以便于实时求解。

在每一个轨迹优化步骤中，该算法会动态地添加新的机器人状态或移除机器人状态，以便于调整空间与时间的分辨率到剩余的轨迹长度或规划范围。轨迹优化器会根据权重优化调整整体的轨迹形状，使之达到安全、平滑，可行和最优的目标。

根据以上步骤，可以得到如图2所示机器人轨迹结果，可以看出该轨迹能让机器人安全避让随机穿越的动态障碍物，并能同时保持平滑的机器人轨迹。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。