基于遍历测度理论的机器人空间搜索无碰撞轨迹规划

周 浩，唐 倩*，李代杨，李志航，撒利军

（1.重庆大学 机械传动国家重点实验室，重庆 400044；2.重庆长安汽车股份有限公司，重庆 400023）

0 引言

在机器人的应用领域中，有许多应用需要高效控制策略用于空间搜寻任务，比如监控领域、目标定位等领域[1]。机器人直接对空间进行逐点搜寻效率较低，并且考虑到机器人的供电续航问题，逐点空间搜寻的策略明显不符合实际。此外，机器人在空间搜寻的过程中还要避免与环境的障碍发生碰撞。所以本文针对机器人在搜索空间无碰撞的运动轨迹的规划算法进行研究。

基于模型的空间无碰撞轨迹规划方法由于其可靠性高得到了广泛关注与研究，目前常见的算法是人工势场法和快速随机树算法。早期KHATIB[2]通过虚拟力建立了环境的人工势场，采用梯度下降的方法得到机器人的运动轨迹。最近，张强等[3]基于刘建华提出的轨迹规划算法[4]提出了改进人工势场的蚁群算法，从而实现了移动机器人的最优路径规划。此外，随机树扩展方法在高维空间的高效率规划而广泛应用于机器人轨迹规划领域[5]，该算法主要包括随机路径规划法（RRT[6]）及其改进算法RRT-Connect[7]。虽然上述算法都能用于移动机器人运动规划，但是上述算法只能实现初始位置到终点位置的无碰撞路径规划，不能实现空间搜寻的轨迹。随着测度理论的不断发展，Mathew[8]提出了遍历性的度量标准，并通过最小化哈密顿函数得到遍历控制的最优控制量的范围，Todd D.Murphey[9]基于遍历度量提出了遍历迭代控制算法（Ergodic iterative-Sequential Action Control,EISAC），并利用该方法对无人机的目标轨迹追踪进行了研究。

本文针对机器人空间搜寻无碰撞轨迹规划问题，尝试建立空间搜寻无碰撞轨迹的目标函数。并通过求解建立的优化目标函数得到机器人的控制策略，从而得到机器人空间搜寻的无碰撞轨迹。

1 空间搜寻策略的数学模型

针对机器人的空间搜寻无碰撞轨迹规划问题，本文拟基于遍历测度理论对空间搜寻无碰撞轨迹进行建模。

1.1 搜寻策略模型

机器人空间搜寻无碰撞轨迹通过遍历测度理论建立目标函数。该标函数需要满足系统状态方程和控制量的约束，通过约束的优化目标函数可以得到最优控制策略。空间搜寻可避障轨迹规划的数学模型如式(1)所示。

其中ε为目标函数，μ*为最优控制，x为规划系统的状态，f为系统状态方程。

1.2 搜寻轨迹度量标准

遍历测度理论表明，搜索空间区域的轨迹分布与输入空间信息的密度成正比（如图1所示），规划轨迹在空间的分布可以通过式(2)求解：

图1 遍历轨迹与输入信息关系图

其中：δ表示狄拉克函数，s为空间[0,L1]×[0,L1]×…×[0,L1]上的点，v为空间的维度，xv(t)表示t时刻轨迹位于空间的位置，因子1/T是为了保证搜索轨迹分布满足概率和为1的性质。

本文利用傅里叶系数量化空间信息分布与均匀时间的轨迹分布，空间信息分布Φ(s)的傅里叶系数φk以及空间均匀时间的轨迹分布C(s)的傅里叶系数Ck可以通过式(3)与式(4)得到：

对于空间信息和轨迹均匀时间分布计算中的傅里叶基函数Fk可以通过式(5)求得：

其中hk为归一化系数，k=0,1,2…，k越大越能反映空间信息分布，从图2中可以得到：随着的增加，在空间中的信息分布越精确，但是k值过大会影响计算效率。

图2 空间信息与的关系图

结合式(3)～式(5)可得到遍历性度量目标函数，如式(6)：

式(6)中的∧k=1/(1+||k||2)s，主要目的是使输入空间低频信息放大。

2 空间搜索无碰撞轨迹求解

在建立了空间轨迹与空间信息的遍历性度量的待优化目标函数后，需要优化目标函数得到当前时域的最优控制，从而实现空间搜索无碰撞轨迹规划。本文拟采用遍历迭代控制算法（EISAC）对目标函数进行求解，得到当前时域最优控制解析解。通过遍历度量标准，可以得到搜索轨迹的待优化目标函数：

其中terg0为自定义的遍历搜索的初始时间，x∈x2为系统的状态变量，Q∈R为系统的代价系数，其余系数见前述内容。本文基于目标函数(7)，采用遍历迭代控制策略，在每个时域内优化目标函数得到最优的控制策略u*解析解：

式(8)中ψ=h(x)TρρTh(x)，ρ为最优控制策略求解过程的中间变量，为系统状态方程的状态转移矩阵，u1为上一时域的最优控制策略，αd∈R-为调节系数且为常数，一般较小。上述变量具体求解方法见文献[10,11]。

遍历迭代控制算法流程如图3所示。

图3 EISAC算法流程图

EISAC算法的主要流程如下：1）通过输入空间计算输入空间信息的傅里叶系数，2）基于输入空间信息的傅里叶系数执行遍历迭代控制算法，3）利用微分方程求解系统的状态x，计算遍历迭代控制算法的中间变量，4）利用最优控制策略求解空间搜索轨迹的最优控制策略u*，5）然后计算遍历迭代控制策略的作用时间λA和时刻τA，6）最后输出空间搜索无碰撞轨迹。

3 空间搜索无碰撞轨迹仿真

为了验证遍历迭代控制算法能够实现空间搜索无碰撞轨迹规划，本文在Matlab2018中进行仿真实验。因为二阶系统的轨迹连续性较一阶系统好，所以本文采用二阶系统对空间搜索可避障轨迹进行仿真规划。二阶系统的空间状态方程：

其中x，y分别为二阶系统在笛卡尔空间下的坐标，u1，u2为对应时域控制输入量。

本文采用遍历迭代控制算法对二阶系统的空间搜索无碰撞轨迹进行数值仿真，其中遍历迭代控制算法（EISAC）仿真的主要参数如表1所示。

表1 EISAC算法仿真参数表

3.1 搜索无碰撞轨迹仿真

本文基于遍历迭代控制算法，得到了3类环境的空间搜索无碰撞轨迹规划仿真，环境1是为了证明算法能够实现不同障碍下的遍历轨迹规划，环境2是为了证明算法能够实现半封闭障碍下的轨迹规划，环境3是为了证明算法能够实现复杂环境的轨迹规划。遍历迭代控制算法（EISAC）的MATLAB仿真结果如图4所示。

图4中输入空间信息为图中输入环境行。本文根据遍历迭代控制算法，在MATLAB中得到了10、20、40、60时间步长的空间搜索无碰撞轨迹规划的仿真结果（如图4中的输出轨迹行所示）。

图4 遍历迭代控制（EISAC）算法仿真结果

仿真结果表明，1）机器人搜索空间的无碰撞轨迹覆盖面积随着时间的增加而增加；2）本文采用的遍历迭代控制算法（EISAC）能够适用于不同环境甚至复杂环境下的空间搜索任务；3）本文采用的遍历迭代控制算法不仅能够实现机器人在空间中的搜索任务，而且还能实现避障功能。

3.2 目标函数收敛性分析

仿真过程中目标函数的变化情况如图5所示。图5表明，本文基于遍历测度理论建立的空间搜索模型能够较快的收敛，从而进一步证明了算法的可行性。

图5 遍历迭代控制的代价变化趋势图

4 结语

针对机器人空间搜索无碰撞轨迹规划问题，本文基于遍历测度理论建立了空间搜索轨迹与空间信息的度量标准，并基于该遍历度量标准的模型建立了待优化的目标函数。然后通过遍历迭代控制算法优化目标函数得到了最优控制量。最后，不同环境的空间搜寻无碰撞轨迹规划仿真表明：基于遍历测度理论建立的空间搜索算法不仅能用于不同环境机器人的无碰撞搜索轨迹，而且目标函数能较快的收敛，从而证明了算法的有效性。