有向外力场作用下机器人路径规划方法*

2019-03-22陶永明

沈阳工业大学学报 2019年2期

王婷，陶永明，阎岩

(1. 大连海洋大学应用技术学院，辽宁大连 116300； 2. 东北财经大学管理科学与工程学院，辽宁大连 116025)

机器人路径规划问题一直是研究的热点，在移动机器人[1]、水面舰艇[2]、水下机器人[3]以及物流运输[4]等领域有着许多研究成果.传统的地面机器人路径规划侧重于分析障碍物或者危险区域来保证机器人的安全，同时使得路径最短.在一些新的应用场景中，如轻量级旋翼无人机在风场中或者水下机器人在流场中运行，机器人的行为明显受到空间内有向外力的影响，这种有向外力会对机器人的行为起到促进或阻碍的作用.因此机器人在路径规划时需要考虑外力存在的情况，合理利用外力对机器人的某项指标进行优化.

Petres等[8]提出了一种基于快速行进算法的连续路径规划方法，方法定义了各向异性的代价函数，其中环境中的有向外力场作为优化问题的约束条件.该方法是有向外力场作用下机器人基于网格点的路径规划方法的突破，但仍存在很大的问题，主要在于它只适用于线性代价函数，导致在过强的外力场中无法规划出可行的路径.Lolla等[9]提出了海洋环境中在流场影响下的水下机器人路径规划问题，突破了路径规划问题基于格点搜索的限制，并且在理论上保证了在强海流中路径的可行性.但是由于在计算机中数值计算还是采用离散化的方式，所以受到数值方法的约束，在实际强海流中路径的可行性并没有得到保证.

本文有以下两点贡献：首先是通过重新定义问题，将海流的情况推广到有向外力的作用情况，使得方法有更广泛的意义；另外是改进了数值方法，使得在强外力作用下，规划出路径的可行程度更高.

1 有向力作用下的机器人路径规划

1.1 路径规划问题描述

(1)

(2)

该模型不考虑机器人具体的动力学，在机器人的运动空间远远大于机器人尺寸的应用场景中，这种规约是完全合理的.在机器人本体更高层面上做规划有着非常重要的现实意义，本文也将在这个层面展开对问题的阐述.

1.2 Level Set方法

(3)

式中，H为速度函数.

经过一段时间的演化后，曲线可能发生拓扑结构变化(例如断裂或者合并等).曲线演化一般会涉及到曲线的参数化，而曲线参数化最大的缺点就是不易于计算曲率、法向矢量等曲线几何参数，且难以处理曲线的拓扑结构变化.

图1 曲线按法线方向的演化示意图Fig.1 Schematic evolution of curvealong normal direction

水平集方法是一种用于界面跟踪和形状建模的数值方法，其最大的优点是可以在笛卡尔网格中对演化的曲线(面)进行数值计算，而不必对曲线(面)进行计算；水平集方法一个重要的理论前提是隐函数的概念，在曲线演化理论中引入水平集的目的就是为曲线提供一种隐式表达方式，从而避免参数化这种显式表达所带来的一系列问题.其主要思想是，将移动变形的曲面嵌入到高一维的函数中，即将曲线的拓扑结构变化表示成一个连续变化的曲面与一个固定的平面(如高度为零的平面)的交线变化，曲面本身可以不发生拓扑结构的变化，从而使得复杂的曲线运动过程变为高维函数的演化过程.

高维曲面φ选取应该是越简单越好，如果该曲面和某一简单的函数之间为一一对应的关系，则选取这种简单的函数来替代“高维曲面”.在水平集问题中，φ通常被选取为“符号距离函数”.假设要跟踪曲线(即零水平集)为∂ω，距离函数d(X)为曲线外围或内部某点到曲线的最短距离，在计算机计算过程中，∂ω本身是由一系列的离散点组成的，所以d(X)的数学表述为d(X)=min|X-Xi|，其中，Xi为组成∂ω的所有点.符号距离函数定义为

(4)

对于∂ω上的所有点Xi均有φ(Xi)=0，选取这种“符号距离函数”作为“高维曲面”存在诸多好处.首先它是光滑曲线，可以避免等高线上出现很陡的梯度，便于后面的计算使用.φ(X)是和时间相关的，所以可被写作φ(X，t)，又因为其包含封闭曲线C，所以也可写作φ(C(t)，t)，简写为φ.

对于t时刻，演化得到的曲线C(t)和零水平集φ(C(t)，t)=0相等，将其对时间求导可得

(5)

在微分几何中，曲线或曲面上的内单位法线和梯度关系为

(6)

由此可见梯度和法线方向一致，将式(5)、(6)联立可得

(7)

2 基于Level Set的路径规划方法

(8)

假设零水平集在初始时刻为一条极小的封闭曲线，即该曲线所包含的面积趋近于0，但可以保证在曲线上每一点上的法向存在，则式(8)初始条件为

(9)

由于T(y)时刻的零水平集是由t=0时刻演化所得，所以如果将T(y)时刻位置看作与y重合，则可以得到上一时刻的位置，按此思路可以逆推出t=0时刻的位置就是出发点.反向路径逆推过程满足

(10)

将所有这些逆推得到的点相连，即可得到路径γ，可以发现T(y)刚好是从起点到终点所需的时间.

3 算法数值实现

算法总共分为两部分，分别称为前向零水平集演化和后向路径追踪.

3.1 前向零水平集演化

定义四个差分算子，分别为x方向和y方向上的向前差分和向后差分，对应着连续x和y方向上的偏导数，即

(11)

根据式(11)定义如下两个梯度，即

(12)

将式(11)、(12)代入式(10)可得

(13)

通过式(13)的演化机制，水平集函数可以不断向前演化.由于数值计算中使用了有限差分法，因此需要注意时间步长Δt的选择，在给定空间网格间距Δh的条件下，需满足

HΔt≤Δh

(14)

3.2 后向路径追踪

(15)

4 仿真结果与分析

在有向外力作用下机器人路径规划问题的仿真实验中，配置空间规约在二维平面上，空间的两个坐标轴x、y都在[-1，1]之间.为进行数值计算，x和y方向上被均分成m×n个网格，分别按i和j来索引.空间内的外力速度场Vf在这些网格上取值，其中第(i，j)个网格点上的外力速度为Vf(i，j).在以下的仿真实验中，机器人的速度保持恒定，在仿真中设定为vn=0.28 m/s.

为验证算法的基本功能以及数值方法的精度，本文对恒定外力场中机器人的路径进行规划，空间内存在恒定的外力速度场Vf=0.2 m/s，仿真结果如图2所示.由图2可知，算法的基本功能已经实现，并且数值算法的精度可以保证最优路径的生成.在外力影响下，机器人向左运动速度减慢，向右速度加快，在曲线上的表现为单位时间原点左侧的曲线被挤压，而右侧曲线被拉伸.

图2 在恒定外力场中机器人的路径规划Fig.2 Path planning for robot in constantexternal force field

不同速度下机器人的路径规化如图3所示.图3中空间存在着带状速度场，同样结构的外力场中，通过调整速度场的大小进行了6组实验.仿真结果表明，路径规划算法可以有效利用外力来加快自己的运动，并且在强流场中算法仍能保持原有的精度.在较小的速度场中，穿过速度场的路径与x轴的夹角较大，表示机器人可以更容易地穿越速度场；在较大的速度场中，该角度变小，表示在较大的速度场中，机器人可以移动的范围变小，同时算法可以精准地确定进入速度场的位置，以确定在较强的外力作用下所规划出的路径仍然保持可行性.

弯曲封闭的外力场在自然界中也很常见，如大气和海洋中存在的涡旋.图4是模拟涡旋生成的一种弯曲外力场，模拟场为同心圆结构，距圆心越远，速度越大.圆心处速度最小，为0 m/s，边界处最大，为1 m/s.仿真选取了3组起始点和终点作为比较，同时以虚线给出了A*算法得到的路径结果.结果显示，在弯曲外力场下，路径可以有效利用外力场，并且生成的路径突破了传统的节点取点方式，形成了连续光滑的曲线路径.A*路径由于必须在网格点上取点，因此要牺牲一些距离来取网格点.从时间上来看，Level Set路径完成后，A*路径还没有到达终点，图4中以“×”表示与Level Set相同时间的A*所能到达的点.