一种基于线性时序逻辑的移动机器人运动规划算法

技术领域

本发明涉及机器人运动规划技术领域，特别涉及一种基于线性时序逻辑的移动机器人运动规划算法。

背景技术

近年来，智能移动机器人得到快速发展，应用领域越来越广泛，如应用于家庭、餐厅、医院的服务机器人，应用于智能仓储的物流机器人以及运用于军事场景的特种机器人等。移动机器人的研究内容具体有：基于视觉或雷达的环境感知与SLAM定位、自主运动规划、行为控制等。其中，运动规划一直都是移动机器人研究的热点方向，运动规划包含路径规划和轨迹规划两部分，高效的运动规划算法能够使机器人高效准确地完成任务。

许多学者对移动机器人的路径规划展开了深入的研究，且取得许多成果，但是针对的大都是一些简单的问题，即根据路程最短、时间最短等评价标准，规划出一条从起点运动到终点的可行路径，并且能够避开环境中的障碍物，这称为点到点问题。传统的路径规划算法因其任务模式单一，无法应用于含有时序性、循环性任务的路径规划问题，故已经无法满足人们对移动机器人的应用需求。人们需要移动机器人去完成一些复杂的时序任务，如工厂安防巡逻、军事侦察、快递配送、火灾救援等任务。因此，需结合一种可描述复杂时序任务的语言，设计出可根据时序任务规划出可行路径的运动规划算法框架。

近十年来，作为机器人和人工智能领域的前沿交叉方向，基于模型检测方法的机器人规划算法研究得到了一批国内外知名学者的重视和开创性研究，其中以线性时序逻辑(Linear Temporal Logic,LTL)语言为任务描述语言的规划方法成为该方向的主要研究内容。线性时序逻辑可以方便地表达事物的行为，能够描述多种时序任务要求，能方便地与现有规划框架结合。如何设计一种基于线性时序逻辑的运动规划算法对扩大移动机器人应用领域具有十分重要的意义。

发明内容

本发明提供了一种基于线性时序逻辑的移动机器人运动规划算法，以解决现有技术中机器人运动路径规划很难适用于复杂时序任务的技术问题。

为达到上述目的，本发明的技术方案是这样实现的：

本发明提供了一种基于线性时序逻辑的移动机器人运动规划算法，包括如下步骤：

步骤S1、构建栅格地图，并根据机器人半径对栅格地图做膨胀处理；构建机器人时序任务，并将其转换为Büchi自动机；

步骤S2、依据Büchi自动机构建前缀采样树T

步骤S3、依据增量构建后的前缀采样树T

步骤S4、依据Büchi自动机和候选前缀路径列表traj

步骤S5、依据增量构建后的后缀采样树T

步骤S6、依据安全走廊规划机器人运动轨迹。

进一步地，所述步骤S2中的前缀采样树T

采样树根据Büchi自动机的有限集合S

进一步地，所述步骤S1具体包含如下步骤：

步骤S11、采用开源算法Cartographer进行栅格地图构建，并根据机器人半径对地图做膨胀处理；

步骤S12、构建机器人时序任务，将机器人的时序任务使用LTL公式表示，并通过开源功能包spot将其转换为Büchi自动机。

Büchi自动机可用元组

步骤S13、将Büchi自动机中同时满足LTL公式的多个原子命题的转换关系删去。

进一步地，所述步骤S2具体包含如下步骤：

步骤S211、将初始节点

步骤S212、将初始状态s

第一列表表示前缀采样树T

前缀采样树T

步骤S213、设置前缀采样的迭代增量Δiter

步骤S214、令迭代计数值i

步骤S215、若i

步骤S216、给定概率λ

步骤S217、根据转换对(s

步骤S218、遍历前缀采样树T

步骤S219、判断||X

步骤S220、遍历采样子树

步骤S221、求解从X

步骤S222、对dubins曲线Q进行碰撞检测，如无碰撞，则执行步骤S223，否则返回步骤S215；

步骤S223、构建新节点N

步骤S224、将新节点N

步骤S225、判断s

步骤S226、将新节点N

步骤S227、构建一系列节点

步骤S228、更新列表第一列表、第二列表，且每当第二列表新增加一对转换对时，迭代值iter

步骤S229、通过采样调整转换列表P

进一步地，所述步骤S3具体包含如下步骤：

步骤S31、遍历前缀采样树T

步骤S32、将每个遍历节点通过父指针向上访问至根节点，从而得到一系列候选前缀路径，并添加至候选前缀路径列表traj

进一步地，所述步骤S4具体包含如下步骤：

步骤S411、将前缀采样树T

步骤S412、将s

第三列表表示后缀采样树T

后缀采样树T

步骤S413、设置后缀采样的迭代增量Δiter

步骤S414、令迭代计数值i

步骤S415、若i

步骤S416、给定概率λ

步骤S417、根据转换对(s

步骤S418、遍历后缀采样树T

步骤S419、判断||X

步骤S420、遍历采样子树

步骤S421、求解从X

步骤S422、对Q进行碰撞检测，如无碰撞，则执行步骤S423，否则返回步骤S415；

步骤S423、构建新节点N

步骤S424、将N

步骤S425、判断s

步骤S426、将N

步骤S427、遍历候选前缀路径traj

步骤S428、将traj

步骤S429、通过采样调整列表P

进一步地，所述步骤S5具体包含如下步骤：

步骤S51、遍历后缀采样树T

步骤S52、根据候选后缀路径，选取代价最小的前缀路径和后缀路径，从而完成路径规划部分；

步骤S53、将步骤S52中获取的路径按照Büchi自动机状态划分，每段路径的Büchi自动机状态相同；

步骤S54、每段路径按一定步长均匀分成多个线段，根据栅格地图在每个线段周围生成一个凸多边形，最终得到多个凸多边形，该凸多边形内部无障碍物，多个凸多边形构成安全走廊。

进一步地，所述步骤S6具体包含如下步骤：

步骤S61、以Bézier曲线作为轨迹的表达式，运用移动机器人微分平坦模型，构建优化问题；n阶m段Bézier轨迹f

其中函数表达式

μ维第j段Bézier曲线的第k阶导数(k＝0,1,2)的第i+1个控制点c

其中，i＝0,1,...,n-k，j＝1,2,...,m；

本优化问题的优化变量为每段轨迹的控制点坐标以及总时间，每段轨迹时间占比为该段轨迹对应的初始路径长短与总路径长短的比值，优化问题如下：

s.t.A

式(3a)为最小化目标函数J，有多个优化目标，第一项为轨迹jerk积分最小，以保持移动机器人运动的平稳性，第二项为时间最短，第三项为惩罚泛函代价最小；式中ω

令

其中

将每段轨迹等间距划分得到q个离散点(不包含轨迹末端点)，令

其中，惩罚函数ψ(x)定义见式(10)，正交系数

步骤S62、将以上优化问题用开源工具Cppad和ipopt进行数值优化求解，得到最终可行的轨迹规划结果。

本发明的有益效果：

1、路径规划相对现有的算法更加符合运动学特性

在路径规划部分，本发明采用dubins曲线进行采样节点状态的连接，相比采用直线连接，更加符合移动机器人的运动学特性，更有利于后续轨迹的生成，因为带运动学约束的路径会大大减少后续轨迹规划出现时间分配不合理的概率，使得生成的轨迹质量更优。另外，不同时序状态路径的交点(即任务指定地点)的航向角不同，可增大在空间受限环境下路径规划求解成功的概率，在移动机器人到达任务指定地点并完成相应任务后，可通过原地转向调整姿态，再到下一任务指定地点；

2、路径规划鲁棒性高

不同的LTL公式转换得到的Büchi自动机规模可能不同，若路径规划采样算法中的迭代次数为常值，则可能造成过度求解或求解失败的情况。对于规模较大的Büchi自动机，应选择较大的迭代次数，反之选择较小的迭代次数。基于此问题，本发明的路径规划采样算法中的迭代次数会自动调整，当采样过程中出现新的Büchi自动机状态转换对时，迭代次数会增加一个给定常量，这使得对于不同的LTL公式都有合理的迭代次数，增大求解成功的概率，鲁棒性高；

3、轨迹规划求解质量高

本发明所提出的轨迹规划算法，运用了移动机器人的微分平坦模型，在较低维度下可根据移动机器人的运动学和动力学对轨迹进行约束，使得生成的轨迹质量更高。

附图说明

图1是本发明基于线性时序逻辑的移动机器人运动规划算法的总体流程图；

图2是LTL公式φ

图3是采样树结构图；

图4是四个LTL公式对应的路径规划结果；

图5是四个LTL公式对应的轨迹规划结果；

图6是LTL公式φ

具体实施方式

下面结合附图及具体实施例对本发明再作进一步详细的说明。

在本发明中的“第一”、“第二”等描述，仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量或顺序。由此，限定有“第一”、“第二”的特征可以明示或隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个、三个等，除非另有明确具体的限定。

参照图1，本申请实施例提供了一种基于线性时序逻辑的移动机器人运动规划算法，包括如下步骤：

步骤S1、构建栅格地图，并根据机器人半径对栅格地图做膨胀处理；构建机器人时序任务，并将其转换为Büchi自动机；

步骤S2、依据Büchi自动机构建前缀采样树T

步骤S3、依据增量构建后的前缀采样树T

步骤S4、依据Büchi自动机和候选前缀路径列表traj

步骤S5、依据增量构建后的后缀采样树T

步骤S6、依据安全走廊，构建优化问题规划机器人运动轨迹。

在本实施例中，前缀采样树T

前缀采样树T

在本实施例中，所述步骤S1具体包含如下步骤：

步骤S11、采用开源算法Cartographer(谷歌开源的建图定位算法)进行栅格地图构建，并根据机器人半径对地图做膨胀处理；

步骤S12、构建机器人时序任务，将机器人的时序任务使用LTL公式表示，并通过开源功能包spot将其转换为Büchi自动机；

Büchi自动机可用元组

在本实施例中，所述步骤S12的LTL公式的应用例子为四个，其分别如下：

φ

φ

φ

φ

其中，φ

步骤S13、将Büchi自动机中需同时满足LTL公式的多个原子命题的转换关系删去。

在本实施例中，所述步骤S2具体包含如下步骤：

步骤S211、将初始节点

步骤S212、将初始状态s

第一列表表示前缀采样树T

前缀采样树T

步骤S213、设置前缀采样的迭代增量Δiter

步骤S214、令迭代计数值i

步骤S215、若i

步骤S216、给定概率λ

步骤S217、根据转换对(s

步骤S218、遍历前缀采样树T

步骤S219、判断||X

步骤S220、遍历采样子树

步骤S221、求解从X

步骤S222、对dubins曲线Q进行碰撞检测，如无碰撞，则执行步骤S223，否则返回步骤S215；

步骤S223、构建新节点N

步骤S224、将新节点N

步骤S225、判断s

步骤S226、将新节点N

步骤S227、构建一系列节点

步骤S228、更新列表第一列表、第二列表，且每当第二列表新增加一对转换对时，迭代值iter

步骤S229、通过采样调整转换列表P

备注：算法进行过程中，迭代次数iter

在本实施例中，所述步骤S3具体包含如下步骤：

步骤S31、遍历前缀采样树T

步骤S32、将每个遍历节点通过父指针向上访问至根节点，从而得到一系列候选前缀路径，并添加至候选前缀路径列表traj

在本实施例中，所述步骤S4具体包含如下步骤：

步骤S411、将前缀采样树T

步骤S412、将s

第三列表表示后缀采样树T

后缀采样树T

步骤S413、设置后缀采样的迭代增量Δiter

步骤S414、令迭代计数值i

步骤S415、若i

步骤S416、给定概率λ

步骤S417、根据转换对(s

步骤S418、遍历后缀采样树T

步骤S419、判断||X

步骤S420、遍历采样子树

步骤S421、求解从X

步骤S422、对Q进行碰撞检测，如无碰撞，则执行步骤S423，否则返回步骤S415；

步骤S423、构建新节点N

步骤S424、将N

步骤S425、判断s

步骤S426、将N

步骤S427、遍历候选前缀路径traj

步骤S428、将traj

步骤S429、通过采样调整列表P

在本实施例中，所述步骤S5具体包含如下步骤：

步骤S51、遍历后缀采样树T

步骤S52、根据候选后缀路径，选取代价最小的前缀路径和后缀路径，从而完成路径规划部分；将式(1)-(4)的路径规划结果在RVIZ仿真平台可视化，分别对应图4(a)-(d)，其中栅格地图尺寸为15m x 15m；

步骤S53、将步骤S52中获取的路径按照Büchi自动机状态划分，每段路径的Büchi自动机状态相同；

步骤S54、每段路径按一定步长均匀分成多个线段，根据栅格地图在每个线段周围生成一个凸多边形，最终得到多个凸多边形，该凸多边形内部无障碍物，多个凸多边形构成安全走廊。

在本实施例中，所述步骤S6具体包含如下步骤：

步骤S61、以Bézier曲线作为轨迹的表达式，运用移动机器人微分平坦模型，构建优化问题；n阶m段Bézier轨迹f

/>

其中函数表达式

μ维第j段Bézier曲线的第k阶导数(k＝0,1,2)的第i+1个控制点c

其中，i＝0,1,...,n-k，j＝1,2,...,m；

本优化问题的优化变量为每段轨迹的控制点坐标以及总时间，每段轨迹时间占比为该段轨迹对应的初始路径长短与总路径长短的比值，优化问题如下：

s.t.A

式(3a)为最小化目标函数J，有多个优化目标，第一项为轨迹jerk积分最小，以保持移动机器人运动的平稳性，第二项为时间最短，第三项为惩罚泛函代价最小；式中ω

令

/>

其中

将每段轨迹等间距划分得到q个离散点(不包含轨迹末端点)，令

其中，惩罚函数ψ(x)定义见式(10)，正交系数

步骤S62、将以上优化问题用开源工具Cppad(用于自动微分)和ipopt(用于求解非线性优化问题)进行求解，得到最终可行的轨迹规划结果。设置v

本发明采用线性时序逻辑公式描述机器人任务需求，通过开源工具箱Spot将公式转化为Büchi自动机。该自动机可用来引导路径规划采样算法进行采样，采样树不断扩展过程中，拥有的Büchi自动机状态会不断增多，待扩展的Büchi自动机状态转换对也会随着增多，直到Büchi自动机的所有状态转换对被探索完。采样过程中碰撞检测所依据的静态2D地图是通过开源算法cartographer采集的，而节点之间采用dubins曲线进行连接。之后在采样获得的满足时序任务要求的最短路径上进行轨迹优化，首先根据路径进行安全走廊构建，随后运用移动机器人微分平坦模型，以Bézier曲线作为轨迹的表达式，构建优化问题，最后采用开源工具Cppad和ipopt进行求解，获得满足移动机器人运动学和动力学的轨迹。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。并且，本发明各个实施方式之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。