基于最优控制策略的复杂环境移动机器人轨迹规划

张泮虹 倪 涛 赵亚辉 翟海阳 赵泽仁

(燕山大学车辆与能源学院， 秦皇岛 066000)

0 引言

自主移动机器人主要包括前端感知、中间规划、终端控制3部分[1-3]。规划层主要是为移动机器人计算出一条由起点到终点的无障碍的可行路径[4]，是影响移动机器人移动行为质量的最直接因素。

目前，路径规划方法主要包括图搜索、势场、随机采样、曲线插值、机器学习以及优化方法[5-6]。

图搜索的方法是对环境进行网格化建模，然后在图中搜索符合任务要求的路径。在对建模要求精度不高的条件下，基于A*的图搜索[7-8]能够快速生成质量较高的移动路径，对于建模精度要求较高的运动规划任务需要对图搜索获得的粗糙路径进行精细化处理之后才能用于底层的控制环节[9]。

势场方法[10]是对环境中的障碍物和非障碍物建模为斥力场和引力场，通过对势场函数进行改进[11-12]可适应不同的应用环境。基于势场的算法是[13-15]通过修改传统人工势场法斥力场函数获得平顺且安全的无碰撞路径，但是易陷入局部最优[16]。

随机采样的方法是在环境中随机生成一系列采样点，然后筛选出满足任务要求的序列作为路径。相比于图搜索，随机采样可以处理较高维度的运动规划问题，具有概率完备性。主要包括概率路标算法(PRM)[17]和快速搜索随机树算法(RRT)[18]，其中RRT由于其规划出的路径可行性强而应用广泛。尽管随机采样的方法具有概率完备性，但是无法处理复杂的约束条件，其生成运动状态具有盲目性，导致最终解的不稳定，且解通常不是最优的[19]。

曲线插值[20]的方法是通过预设的路径点拟合生成连续性、平滑性较好的路径。常见的曲线插值方法包括杜宾曲线法、Reeds-Sheep曲线法、多项式样条曲线拟合法，等等。该方法生成的路径一般具有可跟踪的性质，因此通常是配合其他规划方法对轨迹进行平滑处理或生成初始路径，比如王明强等[21]用三次样条曲线获取道路基准线。

机器学习的方法是以作业需求、机器人的始末状态、环境设置等信息作为输入量，通过增强学习模型[22]、卷积神经网络模型[23]、隐式马尔可夫模型[24]、高斯混合模型[25]等进行学习，输出移动机器人的行驶路径/轨迹，然而机器学习需要大量的训练样本，并需要提前进行离线学习，不适合解决未知环境下实时性的路径规划问题[26]。

基于优化的方法是在动态系统模型的基础上加入约束条件以及任务需求，构成了最优控制问题，以最优控制问题的形式描述移动机器人的规划任务，具有直观、准确、客观的特点，这是前述几种方法所不具备的。目前计算最优控制方法已应用于车辆泊车[27]、避障[28]等结构化环境的驾驶情景中，基于最优控制形式建立的模型中一般包含复杂约束条件，一些研究将这些复杂的约束条件进行简化并利用现代智能优化算法(如粒子群[29]、随机分形搜索[30]等)进行求解。从目前的研究来看，基于最优控制思想去解决轨迹生成问题主要有以下难点：用于描述车辆运动学特性的方程是非凸的；用于限制车辆和障碍物碰撞的几何约束是高度非凸且不可微；避碰约束的维度和障碍物的密度是密切相关的，障碍物较多环境下会导致规划时间急剧增加；规划过程会产生大量的局部最优。因此，在复杂环境下的轨迹规划仍是一个需要研究的问题。

本文在上述研究的基础上，提出一种基于最优控制的复杂环境下移动机器人的轨迹规划方法，首先针对优化求解问题建立运动学模型、几何模型、变量极值点约束以及避障模型等约束，通过对控制变量进行拉格朗日离散化，针对离散化阶段的约束失效进行等距时间离散并引入惩罚函数，最后通过随机分形搜索算法对所构建的优化问题进行求解。

1 优化约束模型

1.1 移动机器人的数学模型

由于机器人在复杂路面上的行驶速度较小，轮胎的侧滑影响较小，因此移动机器人的运动学模型是基于自行车模型构建的。如图1所示，其数学表达式为

图1 移动机器人运动学模型Fig.1 Kinematic model of mobile robot

(1)

式中t——时间

tf——移动机器人整个运动过程总耗时

(x(t),y(t))——移动机器人后轮轴线中点坐标，表示机器人实时位置

v(t)——机器人后轮(点P)实时线速度

θ(t)——机器人实时方向角

φ(t)——机器人前轮实时转向角

L——机器人前后轮之间的纵向距离

此外，为了保证后续规划路径在规避障碍物时更加有效，本文在运动学建模时加入了M、N两个量，分别表示移动机器人后悬长度和前悬长度，更加符合实际情况。

从模型的数学表达式可以看出，如果机器人的实时速度v(t)和方向角θ(t)已知，那么其位置P(x(t),y(t))和前轮的实时转向角φ(t)均可以通过数学积分来推导得出。因此将u=(v(t),θ(t))作为控制向量，χ=(x(t),y(t),φ(t))作为状态向量。

此外，由图1所示的几何关系，可以得出移动机器人的几何模型，即4个边角点的坐标，以便于后续避障模型的搭建。

(2)

式中B——前后轮之间的横向距离的一半

在移动机器人的路径规划中，最重要的原则是保证机器人避开环境中的所有障碍物；其次，也将移动机器人本身硬件所具有的最大速度和转向角加入优化条件中；此外，对移动机器人终点状态也做了相应的约束。

1.2 边界约束

在移动机器人的整个运动过程中，其线性速度不宜过高，转向角也存在极大值，即

(3)

1.3 环境约束

环境约束主要包括移动机器人的始末状态约束以及保证机器人在移动过程中不与环境中的障碍物发生碰撞。本文将障碍物建模为矩形，通过不规律的放置来使环境的复杂程度加大。

假设Q(X,Y)表示移动机器人的某一边角点，如图2所示。A、B、C、D分别表示矩形障碍物的4个边角点，设矩形的几何尺寸为m×n，其中，lAQ=(X-m,Y-n)，lAD=(-m,0)，lAB=(0,-n)。

图2 避障模型构建Fig.2 Obstacle avoidance model construction

若点Q在障碍物矩形区域内，即说明0

(4)

本文所述算法对移动机器人的终点状态做了相应的约束，在实际应用中，根据移动机器人不同的任务需求其终点状态也可以做相应的改变。本文定义机器人的终点是以v(tf)=0的状态进入到预先定义好的矩形框区域内。由于凸集特性，本文所定义的终端条件等效于移动机器人的4个角点位于终端矩形框内部的约束。移动机器人终端约束类似于矩形障碍物约束，此处不再赘述。

2 优化求解策略

本文将路径规划问题归结为一个最优控制问题，主要包括离散化和目标函数设计两部分。

2.1 离散化阶段

将[0,tf]区间平均分成Nf份，每一个子区间为[ti-1,ti](i=1,2,…,Nf)。如图3所示，在每个子区间内设置K+1个插值点{zi0,zi1,…,ziK}，并通过拉格朗日多项式来描述控制变量，以v(t)为例，即

图3 拉格朗日离散化Fig.3 Lagrange discretization

(5)

其中，τ∈[0,1]，τi表示高斯点，当K确定时可以离线计算。因此当插值点足够多时，就能很好地表示控制变量v(t)和φ(t)。此外，为了保证子区间之间变量的连续性，需要保证前后区间端点处的插值点相等，即

(6)

即

ziK=z(i+1)0

(7)

在用插值点描述控制变量之后，移动机器人的状态变量就可以通过数值积分的方式进行计算，如图4所示，在数值积分之前，把前述的分段拉格朗日多项式等距时间均分成Nsp(足够大)份，即v(t)可以表示为{v0,v1,…,vNsp-1}。

图4 等距时间离散化Fig.4 Isometric time discretization

2.2 目标函数设计

上一阶段仅仅是将连续变量通过拉格朗日插值的形式进行离散化，而第1节所述的约束条件也仅仅是针对插值点，但是插值点之间的时刻并没有被约束，这将引起如下问题：如图5所示，即便插值点1、2、3均满足控制变量约束条件，但由于控制变量的连续性，也仍然会存在某些不符合约束条件的时刻。此外，如图6所示，尽管在离散化情况下机器人的运动轨迹满足了障碍物约束，但在连续时间内，图示箭头所指机器人运动的下一时刻将会和障碍物发生碰撞，从而导致任务失败。

图5 连续变量越界Fig.5 Continuous variable out of bounds

图6 连续变量避障失效示意图Fig.6 Continuous variable obstacle avoidance failure

针对如图5所示的问题，在数值积分之前， 将重新均分的各个离散化点，即集合{v0,v1,…,vNsp-1}中的每个元素都进行边界约束检查，若vi超出边界线，则令vi=vmax或者令vi=vmin，将有效减少控制变量超出边界的情况。

针对图6所示的问题，本文在目标函数设计中加入了惩罚函数，对于机器人运动过程中避障，基于式(4)，得到障碍物的惩罚值

Qc=min((lABlAB-lAQlAB),lAQlAB,
(lADlAD-lAQlAD),lAQlAD)

(8)

式(8)保证了点Q是以最小的代价位于障碍物外的，对于移动机器人终端约束也可以用类似的思想，以保证最终机器人位于终点所定义的矩形框内。

因此，最终惩罚项

(9)

为了将求解轨迹的可行解与不可行解区分开，本文引入一个足够大的定值N来保证可行解的惩罚值绝对小于不可行解的惩罚值。显然，若路径能保证无障碍和满足最终状态，那么其惩罚值为0。

3 优化求解器

根据前文所述，将离散化的插值点和移动机器人的运动时间tf作为决策变量。然后，通过积分得到状态变量，检查其边界约束，并将障碍物惩罚函数加入优化准则中。考虑到元启发算法具有全局优化的能力，本文采用了随机分形搜索算法[31]作为整个优化过程的求解器。

分形指的是具有自相似性的现象、图像或者物理过程等，随机分形表现在结构或复杂度上具有统计意义上的自相似性，一般是通过莱维飞行、高斯游走等随机规则来产生。随机分形主要包括扩散和更新两个过程，扩散过程是基于粒子的当前位置，因此有利于寻找全局最优解。

首先是种群的初始化，在这个过程中，基于约束条件随机初始化种群中的每一个个体，第j个个体的初始化方程为

Pj=LB+ε(UB-LB)

(10)

其中，LB和UB分别是求解问题向量的上下边界，本文指的是控制变量转向角和速度的上下限值，ε是在区间[0,1]上服从均匀分布的随机数。

初始化种群之后，计算个体的适应度函数以获得最佳个体BP，然后所有个体都围绕当前的位置游走以搜索环境空间，随机搜索算法采用高斯游走作为唯一的游走方式，即

(11)

式中Pi——第i个个体的位置

ε、ε′——区间[0,1]上服从均匀分布随机数

μBP、μP——高斯函数均值

σ——高斯参数标准差

μBP=|BP|，μP=|Pi|，高斯参数中的标准差为

(12)

第1次更新过程，首先根据个体的适应度进行排序，然后赋予每个个体性能级别，个体适应度为

(13)

式中N——种群的个体数量

rank(Pi)——个体Pi在种群中的排序

之后判断是否满足Pai<ε，若满足则更新个体Pi的第j个分量，否则保持不变，更新公式为

P′i(j)=Pr(j)-ε(Pt(j)-Pi(j))

(14)

式中P′i——个体Pi更新后的位置

Pr、Pt——种群中随机选择的个体

第2次更新过程主要是根据个体的分量进行，对于更新后的P′i，判断是否满足P′ai<ε，若满足则更新P′i的当前位置，否则保持不变，更新公式为

(15)

式中P′r、P′t——第1次更新后种群中选择的个体

若P″i对应的适应度优于P′i对应的适应度，则用P″i更新替换掉P′i。

4 仿真实验

4.1 实验设置

仿真实验在Matlab 2019b中展开，计算机环境是Windows10,处理器Intel(R) Core(TM) i7-10870H CPU @ 2.20 GHz 2.21 GHz，RAM 16 GB，本文所述算法中提到的一些参数设置如表1所示。为了验证本文所述算法的有效性，鉴于现在大多数的规划算法均是将机器人建模为质点，与真实情况相差较大，在本仿真实验中，引入了“足迹”的概念，即表示各个时刻移动机器人在地面上的投影，以足迹和障碍物不发生交集来作为避障有效的判定，如1.3节所述。将建好的模型经拉格朗日离散化处理，然后代入SFS算法中进行迭代求解，最终得到最优解。

表1 基于优化算法的参数Tab.1 Parameter based on optimization algorithm

4.2 障碍物较少场景验证

为验证本文所述算法的有效性，场景1随意设置10个矩形障碍物，用户可根据环境或者任务不同随意指定起点，本文取x1(0)=-40 m,y1(0)=15 m，终点设置在以(0,0)为中心，长为2.5 m、宽为1.5 m的终端盒子内。初始方位角为0°，初始转向角为0°，初始速度为0。从图7a可以看出，所规划的轨迹可以实现准确避障，即说明优化模型中的避障模型是有效的，此外终端约束也满足，图7b中的速度最大值控制在3.0 m/s，前轮转角控制在0.714 rad内，即边界约束有效，同时可以看出移动机器人的足迹和障碍物矩形的交集为空，因此针对离散化导致的避障失效问题也得到了有效解决。

图7 障碍物较少场景Fig.7 Scenarios with fewer obstacles

4.3 狭窄区域场景验证

场景2设置了10个障碍物并设置了一个狭窄区域，起点设置为x2(0)=-50 m,y2(0)=5 m，终点设置在以(0,0)为中心，长为2.5 m、宽为1.5 m的终端盒子内。初始方位角为0°，初始转向角为0°，初始速度为0。从图8a可以看出，所规划的轨迹可以实现准确避障，即说明优化模型中的避障模型是有效的，此外终端约束也严格满足，图8b中的速度最大值控制在3.0 m/s，前轮转角控制在0.714 rad内，即边界约束有效，同时可以看出移动机器人的足迹和障碍物矩形的交集为空，因此针对离散化导致的避障失效问题也得到了有效的解决，因此本文所述方法在狭窄区域内仍具有一定的稳定性。

图8 狭窄区域场景Fig.8 Narrow area scenario

4.4 复杂场景验证

场景3设置了30个障碍物矩形区域，起点设置为x3(0)=-40 m,y3(0)=0 m，终点设置在以(0,0)为中心，长为2.5 m、宽为1.5 m的终端盒子内。初始方位角为0°，初始转向角为0°，初始速度为0。从图9a可以看出，所规划的轨迹可以实现准确避障，即说明优化模型中的避障模型是有效的，此外严格满足终端约束，图9b中的速度最大值控制在3.0 m/s，前轮转角控制在0.714 rad内，即边界约束有效，同时可以看出移动机器人的足迹和障碍物矩形的交集为空，因此针对离散化导致的避障失效问题也得到了有效的解决，在复杂环境下进一步证明了本文所述方法具有较强的鲁棒性。

图9 障碍物较多场景Fig.9 Scenarios with many obstacles

4.5 仿真实验对比分析

采用Matlab随机生成了10个场景，由于混合A*算法考虑了机器人的运动学模型，而且算法中也加入了非线性优化和非参数插值，因此将本文所述优化算法与混合A*算法作比较，结果如表2所示，成功率是指移动机器人足迹不与矩形障碍物产生交集，如图10虚线圆处所示，计算第4障碍物和第6障碍物与混合A*算法所规划轨迹的最近距离，分别为0.312 m和0.965 m，而本文设定的移动机器人的宽为2 m，长为4 m，也就说明移动机器人在实际移动中会与障碍物发生碰撞，导致避障失效，即混合A*算法在狭窄区域内并不有效，同时，基于优化算法所得到的轨迹较短，时间也相对较小，效率更高。这里的轨迹长度是对求得的离散轨迹点求和，即

图10 本文算法与混合A*算法运行轨迹Fig.10 Proposed algorithm and hybrid A* algorithm running track

表2 本文算法与混合A*算法性能对比Tab.2 Performance comparison between proposed algorithm and hybrid A* algorithm

(16)

此外，与现有文献中的基于优化的规划算法比较，用于验证本文所提算法的求解质量。选取文献[32]中提到的优化算法作为基准，用于验证其余算法的最优性损失，假设算法X以代价值cX取得最优解，文献[33]算法以cbaseline为代价值取得最优解，那么算法X的最优性损失计算式为[34]

(17)

很明显最优性损失越小，说明求解质量越高。通过计算，本文所述算法最优性损失为19.72%，文献[32]算法为45.1%，文献[33]算法为34.61%，因此本文所述算法在轨迹规划的求解上质量更高，输出的轨迹具有更好的性能。

此外，验证SFS算法中迭代次数对于求解结果的影响。选择场景1，对不同迭代次数下求得的目标函数值作比较，如表3所示，从表3可以看出，迭代次数在大于500以后，目标函数值的降低率趋缓，因此迭代次数并不是越大越好，选择合适的迭代次数会增加求解效率。

表3 不同迭代次数SFS算法的目标值Tab.3 Target values of different iterations of SFS algorithm

5 结论

(1)将轨迹规划转化为最优控制问题，通过设置控制变量极值、障碍物避碰、起点终点状态等优化限制条件，采用随机分形搜索算法的元启发策略进行求解，得到了移动机器人精确的移动轨迹，本文所述方法在复杂、狭窄区域均具有较好的表现。

(2)通过等距时间离散，解决了由离散化导致的离散点之间约束失效的问题，Matlab仿真实验显示移动机器人的线速度不超过最大速度3.0 m/s，转向角不超过最大转角0.714 rad，均满足变量的边界约束。