移动机器人最小快照轨迹寻优方法研究

彭 伟，刘小方，吴玉彬，何 星，张宪宇

(火箭军工程大学， 西安 710025)

1 引言

移动机器人路径规划是集动态环境检测、动态规划与执行、行为决策与控制等数项功能于一体的综合化智能系统[1]。近年来已经成功地应用到工农业生产的各个领域，在我们生活的周围可以随处找到它的影子，因而移动机器人路径规划问题便引起了国内外众多学者的广泛关注，获得了一大批卓有成效的研究成果。

如何进行路径规划是无人飞行器、水下机器人[2]等移动机器人系统的一个重要问题，需要执行者跟踪移动目标，也就是要求移动机器人追逐另一个移动物体，例如潜艇[3]。除了机器人技术[4-5]之外，移动目标路径规划算法还可以解决诸如语音识别、路由、联网[6-7]和视频游戏[8-9]等各种问题。这些应用程序都依赖于能够在目标改变时快速重新规划路径。例如，在网络中，当必须向某个位置发送数据并且该位置不确定或改变时，使用移动目标路径规划器[10]。我们正在处理的运动目标路径规划问题在某些方面类似于通常应用于导弹[11-12]的闭环寻的制导算法。主要的区别在于，我们的场景除了机动目标之外还有潜在的迷宫式障碍。

D.Mellinger等[13]提出了最小快照轨迹生成算法，但是针对最小快照轨迹生成的原因并没有进行解释，以及最小快照在控制中的作用也鲜有涉猎。根据已有文献研究内容和其他一些相关学者的意见，平滑性标准一般可以转化为最小化输入变化率，这对于保持机载传感器测量的质量以及避免突然或过度的控制输入带来无法完成路径寻优任务是很有必要的。对于高动态车辆和机器人，多项式轨迹的选择是自然的[4-15]，因为作为约束QP的解决方案，这些轨迹可以有效地获得，并且约束QP使路径导数的成本函数最小化。该优化框架允许路径段的端点可选地固定到期望值或保持自由，并且多项式可以联合优化，同时将导数的连续性保持到任意顺序。对于导数，通过这些航路点的航点和时间进而给出这个QP问题的约束。

尽管近年来针对移动机器人路径规划问题已经取得了很多卓有成效的研究成果，但就目前作者关注情况而言，对于未知动态不确定环境下的移动机器人路径规划寻优问题还鲜有听闻，众所周知，未来的应用环境是瞬息万变的，这就要求移动机器人的路径规划寻优必须能够做到与时俱进地适应任何苛刻的动态环境。基于此，本文的研究重点就放在移动障碍物时实现最小快照轨迹的生成。大多数研究人员处理这一问题的常用方法是尝试添加路径点。基本过程如下：如果在优化后发现特定的轨迹段与障碍物相交，则在其两端之间简单地添加一个附加的路径点，将该路段分成两部分。这个中点被认为是无碰撞的，因为它位于由搜索算法返回的最优分段线性路径上。用附加的路径点重新优化多项式，必要时重复该过程，直到多项式轨迹无碰撞为止。显然，附加的路径点增加了计算复杂度，尤其是在室内密集环境中有移动障碍物存在时。因此，本文尝试分析添加路径点时的具体影响，并试图找出在避障成功的前提下可以降低计算复杂度的有效途径。

2 模型建立

针对持续时间段是已知的情况，路径可以描述如下。需要说明的是平坦输出空间中两点之间的单个轨迹段由独立多项式f(t)组成。

(1)

处理多项式f(t)导数平方的代价函数可以复写成如下：

(2)

其中c0,c1，…,cN是这些不同导数的系数。为了对这个快照之和进行最小化，除了c4之外，代价函数中的所有导数惩罚将被设置为零。

于是，一个段的成本函数可以描述为如下：

(f(4)(t))2=

对于所有分段，m个多项式分段可以通过以块对角格式连接它们的成本矩阵来联合优化。多项式优化中的约束施加在每个轨迹段的端点上。这些约束允许将端点固定到空间中的已知位置，或者指定特定的速度、加速度和阶跃等。

(4)

利用多项式的系数和端点导数之间的映射矩阵(Aj)，对第段轨迹的导数约束进行公式化：

(5)

其中dj代表给定的第j段的位置信息。第j阶导数表示路径点约束。可以描述为如下。

(6)

对于联合优化问题，这些多段约束可以编译为单个线性等式约束集。

(7)

两段之间的连续性约束[16]：

如果特定导数未知，则必须施加连续性约束，以确保第j段末尾的导数与第j+1段开始处的导数匹配，然后，尝试使用合适的方法来解决由此产生的约束QP。

虽然上述方法适用于单个分段和小的联合优化问题，但是这个公式在数值上是不稳定的，并且对于多个分段、高阶多项式以及涉及广泛变化的分段时间时将会变得陷入病态。

AT, jpj=A0, j+1pj+1

(8)

对上述解的改进与处理是利用替换技术[17-18]将问题转化为无约束QP，直接用端点导数作为决策变量来求解，而不是用多项式系数求解。

(9)

首先，将约束替换为原始成本函数。

(10)

现在，这个新的二次成本函数中的决策变量是段的端点导数。通过一个组合零点矩阵和零点(C)将这些变量重新排序，即将固定的衍生项(dF)和自由的衍生项组合在一起(dP)。于是可以得到:

(11)

为了简化符号，把块对角矩阵写为和。总成本的表达式可以在分区后写入。

(12)

对J进行微分，将J等同于零，利用固定导数和代价矩阵给出自由导数的最优值向量。

(13)

现在可以从适当约束方程的个别评估中恢复多项式，将导数映射到系数空间。

3 时间分配

在前面的导数处理中，设置T固定，这意味着机器人通过每个轨迹段的时间相同。前期没有考虑时间分配问题。这些分段时间限制了解决方案的质量，但是可以允许在成本函数方面改进以提高总体解决方案的质量。在一些实际应用中，调整时间分配可以优化轨迹。在本节中，将会考虑时间分配。通过仿真给出考虑时间分配的移动机器人最小拍照轨迹生成实验结果。

如图1所示，由上述解决方案生成简单多段最小拍照轨迹。

图1 简单的最小拍照轨迹示意图

4 通过添加航路点避免障碍物

对于最小拍照轨迹生成，避障也是实际应用中需要解决的问题。这个问题的共同解决方案是增加航路点[19-20]。如果在优化后发现特定的轨迹段与障碍物相交，则在其两端之间简单地添加一个附加的航路点，将该段分割成两个。这个中点是已知的无碰撞的，因为它位于由搜索算法返回的最优分段线性路径上。使用附加的航路点重新优化多项式，如果需要，重复该过程，直到多项式轨迹无碰撞为止。在存在移动障碍物的密集和动态环境中，轨迹可能需要许多附加的航路点来修复碰撞，因此需要多次重新求解优化问题直到找到可行的解决方案。附加路径点可能增加QP的计算复杂度，需要在每次迭代中求解。

为了评估障碍如何影响到整个算法复杂度的增加，本文在各种情况下进行试验，例如区分固定的障碍和运动障碍以及它们的不同分布等情形。

通过增加控制路径点，保持原始最小总拍照约束是否会带来不同的解决方案？如果我们想保持这个约束，当发现障碍物时，试图通过添加路径点后修改原始轨迹来解决它。基于最小拍照轨迹的完整性生成，以保持最小的差分推力。解决方案是通过搜索算法增加路径点，寻找可行的无碰撞轨迹。如果我们考虑保持时间约束以及不花时间分配，它将花费大量的时间来获得可行的解决方案，甚至导致无效解或无解。

如图2所示，再生轨迹进入第三障碍物的区域，这意味着该轨迹不是最佳的解决方案。在仿真部分中，轨迹被视为多个点。在决策算法中，这些轨迹点不在障碍物区域内，因为离该障碍物最近的两点不在障碍物区域内。即使通过改进轨迹点的分布解和更新决策算法，即所生成的轨迹是否在障碍物的影响区域内，也无法解决这个问题，因为即使这样做也会发生其他无效解，而且这意味着计算复杂度的增加从而带来更大的问题。

图2 通过添加航路点生成全最小快照轨迹示意图

假设障碍物与位于路点1和路点2之间的一个参数相对应，则该部分放弃了最小抓拍轨迹要求的完整性。机器人检测到机器人到达A点时的障碍物，该点受到传感器检测范围的限制。然后保持轨迹的其他部分不变，通过增加控制点，重新计算点A与最近路点之间的无碰撞轨迹。修改后的路径仍将在有效约束中生成，如图3所示。

图3 通过添加航路点避开区间障碍物轨迹示意图

通过在几个递归步骤之后来比较结果的成本函数，可以搜索最优解。在具体应用中，避开障碍物时会有一些具体的要求。然而，本文选择采用在点A和点B之间新轨迹长度最短的轨迹作为该问题的目标解。

在该算法中，传感器的检测范围应大于安全范围，并事先做出重新规划轨迹的决定。由于区间最小抓拍轨迹很容易进入障碍物影响范围，增加远离原轨迹的控制点会产生较多的计算时间和奇异形状的轨迹。在检测障碍物时，避障是主要考虑的问题，因为基于当前传感器信息生成整个轨迹，所以由其他算法生成的后续轨迹可能是无效的。在确定障碍物不会发生之前规划整个下一个轨迹可能会导致一些无意义的计算工作发生，特别是在存在移动障碍物的快速变化的环境中。

最后，立足于实验室现有条件，对所设计算法进行实景在线实验验证。图4为课题研究的模拟实验室场景图，图5所示为自身携带的BUMBLEBEE2传感器的移动机器人。具有全局快门曝光功能的双目相机自带BUMBLEBEE2，图像分辨率以及帧率分别设定为：640*480ppi与30fps，基线设定为12 cm。从图6可以清晰看出，基于最小快照轨迹生成算法，移动机器人携带BUMBLEBEE2型双目相机在实验室中进行路径寻优规划，其轨迹为路线图，总共进行了3圈实景在线路径寻优运动，能够顺利无碰撞地有效完成路径规划任务并成功地进行了在线定位。

图4 模拟实验室场景图

图5 携带BUMBLEBEE2传感器的移动机器人实物图

图6 在各种情况下最小快照轨迹生成示意图

图7为移动机器人实景在线实验效果图，其中，图7(a)是移动机器人路径规划及在线地图定位实景图，其采用的观察视角与图5模拟实验室场景图中一致，图7(b)为采用MATLAB Spacedyn工具箱仿真得到的移动机器人轨迹优化的俯视图。由于实验室的实际模拟环境中存在大面积白墙、玻璃等，因而对BUMBLEBEE2型双目相机的采集环境信息造成相当大的压力。

但是从图7可以明显看出，移动机器人的三圈寻优轨迹之间并没有出现非常大的错位现象，并且都能性能俱佳无碰撞地顺利完成路径规划寻优任务，从一个侧面成功地反映了所提算法的有效性与优越性。

图7 移动机器人实景在线实验验证效果图

5 结论

为了解决增加路径点会产生大量的不确定性，使计算复杂度提高，且在特定区间内局部增加控制点生成的路径性能不好，提出一种全新的移动机器人路径规划寻优方法，获得最小快照轨迹生成。首先建立模型，提出最小导数的代价函数并给出约束条件；其次把时间分配问题应用到路径规划中，与动态轨迹规划相结合，在优先考虑避障的同时，获得了保证100%输入平滑度的完整最小快照轨迹生成。