基于改进粒子群优化的水下机器人足端轨迹规划

摘 要：针对多项式插值轨迹规划阶次高、面对多个限制条件时优化运算量大等问题，提出水下机器人关节空间基于改进粒子群优化方法，设计以时间最优为目标的5次多项式插值轨迹规划算法。用Fluent求得腿部水阻力系数后，建立多个约束条件方程，通过更改搜索目标降低搜索维度，减少计算量。改进粒子群优化采用可变权重，相较于标准粒子群优化，改进粒子群优化较好地平衡了全局收敛性和收敛速度。通过仿真实验得到各关节的角度、角速度、角加速度曲线，验证了所提出方法的可行性与有效性。

关键词：水下机器人；轨迹规划；粒子群优化；多项式插值

中图分类号：TP242.3 文献标志码：B 文章编号：1671-5276（2024）04-0208-06

Trajectory Planning of Underwater Robot Foot Based on Improved Particle Swarm Optimization

ZHI Chenghao， ZHAO Dongbiao

（College of Mechanical and Electrical Engineering， Nanjing University of Aeronautics and Astronautics， Nanjing 210016， China）

Abstract：Regarding the high order of polynomial interpolation trajectory planning and the large amount of optimization operations in the face of multiple constraints， a 5-time polynomial interpolation trajectory planning algorithm based on an improved particle swarm optimization method in the joint space of underwater robots with the goal of time optimality is proposed. With the leg water resistance coefficient calculated by Fluent， a number of constraint equations are established. The search target is changed to reduce the search dimension and calculation amount. Compared with standard particle swarm optimization， the improved particle swarm optimization is trade-off between global convergence and computation speed. The angle， angular velocity and angular acceleration curves of each joint are obtained by simulation experiments， which verifies the feasibility and effectiveness of the proposed method.

Keywords：underwater robot; trajectory planning; particle swarm optimization; polynomial interpolation

0 引言

相较于轮式、履带式机器人，四足机器人在行走越障能力、环境适应能力上有较好的表现，因此在机器人领域是一个热度较高的研究方向。四足机器人通过规划行走步态和足端运动轨迹，可以在越障能力、工作效率和能量消耗上进行有效提升，从而在机器人携带有限电源的情况下，大大增加工作时间^[1]。尤其在水下工作环境中，水阻力使得行走消耗增高。因此，对四足机器人的行走步态和足端轨迹进行规划和优化是很有必要的。

在足端轨迹规划方面，通常会采用各类轨迹函数进行轨迹、速度等各方面的规划，如多项式插值、正弦法、NURBS（B样条）曲线等。刘吉成等^[2]在轮腿混合式机器人研究中使用了修改后的复合摆线轨迹规划，该方法实现了机器人足端曲线的平滑变换，解决了机器人足底与地面的滑动及拖地问题；李满宏等^[3]为仿生六足机器人在不同路况下设计了抛物线、直线-抛物线组合轨迹，提高越障能力；柏龙等^[4]根据动物足端轨迹特性，选择椭圆曲线作为足端运动轨迹，具有轨迹圆滑、无突变、导数连续的优点；王立鹏等^[5]在研究液压四足机器人中，根据复合摆线轨迹，改进为足端零冲击轨迹规划，并使用对角步态行走路；韩宝玲等^[6]在液压四足机器人研究中，以Bézier曲线为基础，通过改进步高和步长，来提高机器人工作过程中的平衡能力。

为了获得更好的速度、加速度性能，通常做法是增加规划轨迹函数的阶次或复杂度，但这样会增加轨迹优化时的计算量。在机器人实际运行中有一些运动学、动力学约束，例如位置、速度、加速度、加加速度和转矩的限制。为了提高效率，机器人应在满足约束条件下尽可能快速地运动。但由于机器人动力学中的非线性和耦合，该最小时间控制问题可能计算复杂，解决困难^[7]。因此常会结合一些智能优化算法对一个或多个指标求取最优方案，如遗传算法、粒子群算法（PSO）、人工鱼群算法等以及各类混合算法。这些算法都是通过模拟某些自然现象和过程，或生物群体的智能行为，满足一定条件下，在众多方案或数值中寻找最优值。相较于传统数学推导优化，这类优化方法具有简单、通用、便于并行处理等特点，而PSO只是通过内部速度进行更新，因此原理更简单、参数更少、实现更容易^[8]。

标准粒子群优化算法采用固定惯性权重，收敛速度较慢，且可能在局部极值收敛。本文提出基于改进PSO的时间最优5次样条插值的轨迹规划方法，采用动态惯性权重，平衡收敛速度和全局收敛性，以轨迹运动时间为待优化对象，将搜索空间降低到一维，从而不必推导多项式系数与优化目标间复杂的函数关系，大幅降低了计算量和计算时间，并且结合水阻力仿真系数和建立的腿部运动模型，使得优化结果限制在实际运动条件的范围内，可靠性更高。最终在Matlab中模拟验证优化结果。

1 步态规划

在研究四足机器人行走步态时，常会参考自然界四足动物的行走动作，其常用步态包括：行走、对角小跑、对侧步、疾驰或跳跃。四足动物会根据行走的速度不同选择不同步态^[9]，如图 1所示。

行走步态速度最慢，行走过程中始终保持3只支撑足，1只摆动足，属于静态行走步态，需要保持质心投影在支撑三角内来保持稳定。其余步态属于动态步态，相较于行走步态，速度较快，依靠机器人自身质量惯性保持动态平衡。其中，对角步态保持对角线上的两足保持同相，对侧步态保持身体同侧的两足同相。疾驰或跳跃步态速度最快，都是以身体前后划分为两组规划步态，但在同组足的相位差上有所差别。一般自然界中体型较大的动物使用疾驰步态，如马，狗等，体型较小的动物使用跳跃步态，如兔等。

在研究四足机器人步态时，最常用的步态为对角小跑步态。该步态相较行走步态速度快，两足支撑对角线通过质心，易于保持平衡，而其余步态均需要在动态行走过程中额外调整关节，如对侧步态，质心会随着步态相位变化往身体两侧来回变换，容易造成机器人侧翻，需要调节髋关节进行姿态调整；疾驰或跳跃步态则在前后方向上质心有较大变化，落地冲击力较大，需要肘关节、踝关节进行缓冲。

但本文机器人工作环境位于海水之下，易受到各个方向的乱流干扰，保证行走时的稳定性至关重要，且相较于陆上的行走机器人，水下行走机器人还额外受到水阻力的影响，运动速度越快，受到水阻力越大。因此，选择使用行走步态，四条腿按前左、后右、前右、后左的顺序循环交替进入摆动相，每一条腿在摆动相中时间为1个完整行走周期的1/4，因此占空比为0.75，行走步态相位图如图 2所示。

2 水阻力系数仿真

2.1 物理模型的构建

通过SolidWorks软件建立水下四足机器人原理样机的二自由度简化腿部模型如图 3所示，测得其迎水面截面面积为18 301mm²，大腿长度L₁=0.12m，小腿长度L₂=0.10m，腿部处于伸直状态。

将腿部模型导入Fluent的ICEM CFD中，创建计算域，使用非结构化网格。为了得到更好的模型细节，对腿部模型表面及周围的流体进行网格加密，而远离模型表面的流体划分网格较大。

在Fluent中，设置海水密度ρ=1.05×10³kg/m³，动力黏度1.003×10^－3Ps·s。

2.2 腿部水动力性能分析

物体在流体中所受运动阻力的经验公式为

式中：ρ为流体密度；S为流速垂直方向横截面积；V为流体流速；C_d为水阻力系数。C_d的取值与流体性质、物体性质等一系列因素有关，不同情况下取值不同，因此需要进行仿真计算，为后面粒子群优化设置受力限制提供计算基础。

设定流域进口速度分别为0.25m/s、0.50m/s、0.75m/s、1.00m/s、1.25m/s，对腿部模型所受阻力进行模拟计算，最终Fluent根据海水密度、迎水面截面面积、腿部摆动速度，计算出不同速度下的阻力系数C_d，所得数据结果如表1所示。

将表1中所得的阻力与流速绘制成曲线，使用MATLAB进行多元非线性回归拟合曲线，拟合曲线如图 4所示，可得拟合公式为

F≈6.994 8V²（2）

结合水阻力的经验公式、海水密度和横截面积，可得水下机器人单个腿部模型的水阻力系数约为0.766。

残差是指实际值与估计值之间的差，可以通过残差来验证回归模型的准确性。根据数理统计，计算总平方和T_SS、残差平方和R_SS后，最后求得可决系数R²：

计算得可决系数R²为0.999 8，接近于1，说明此回归方程对实际值的拟合程度好，证明了其拟合的准确性高。

3 时间最优轨迹规划

3.1 机械腿运动学模型

为将机械腿在笛卡儿空间的运动转换为关节空间的运动，方便对机械腿两个关节进行控制，需要对机械腿建立坐标系，构建运动学关系与逆运动学求解。

首先对机械腿建立坐标系，如图 5所示。对于关节i上的坐标系{i，规定z_i轴与关节轴i重合，x_i轴沿公垂线的指向，由关节轴线i指向关节轴线i+1。

将各个相邻关节变换矩阵相乘，可得末端坐标系与基座标系的变换矩阵：

式中θ₁₂为θ₁+θ₂。

3.2 机械腿关节受力分析

所设计的水下机器人整体质量与浮力相近，因此忽略这方面的受力，只考虑腿部运动过程中受到的水阻力。水阻力方向与运动方向相反，因此连杆1、连杆2上受到的力与关节1、关节2引起的旋转有关，如图 6所示。

连杆2所受力矩τ₂可分解为两关节旋转引起的水阻力，而连杆1所受力矩τ₁可分解为关节1旋转引起的水阻力以及关节2对连杆1的作用力。两关节所受力矩计算公式如式（5）、式（6）所示。

最后可化简为只与关节角θ₁、θ₂有关的函数，为后面粒子群优化时提供关节受力的限制条件函数，防止受力超过电机能提供的最大力矩。

3.3 5次样条插值函数构造

机械腿从初始位置运动至某个期望位置，一种方法是在路径描述中给出一系列的期望中间点，机械腿必须经过中间点所描述的一系列过渡位置与姿态。通常期望机械腿的运动是平滑的，包括速度、加速度、脉动等的平滑，因此考虑采用在插值点处具有k－1阶导数连续的k次样条插值多项式。

5次样条插值多项式的通式为

式中：e₀，…，e₅为5次多项式的系数；t为时间；h（t）为在对应时间t时机械腿所处的笛卡儿空间位置。

轨迹规划分为两种：1）在关节空间生成轨迹；2）在笛卡儿空间生成轨迹。本文选择使用笛卡儿空间轨迹规划，使生成的轨迹更加直观。

因为本文机械腿是二自由度，运动轨迹只在x、y平面上，且设定轨迹关于时间中点对称，因此只需在x、y两个方向上分别进行5次样条插值函数构造，且只用规划一半轨迹，另一半进行对称处理，即可获得完整的运动轨迹。

完成5次插值轨迹规划需要一定的约束条件：已知起始时刻t₀的位置x₀、速度v₀、加速度a₀，以及终末时刻t_f的位置x_f、速度v_f、加速度a_f。根据以上条件，可以推导出求解x、y方向的5次多项式系数的矩阵形式式（8）—式（10），式（10）即为5次插值函数的系数的解。

3.4 粒子群算法时间最优求解

在选取粒子群优化目标时，不同的选择会导致计算复杂度和计算时间有很大的不同。时间t和式中的系数e都是待求解的未知数。若是将系数e作为自变量优化，那么粒子群的粒子维数为6维，相比于1维的时间t，其计算量大且复杂。因此直接在时间t的一维搜索空间里优化。

式（8）—式（10）是根据已知的初始末位置、速度、加速度条件，建立起轨迹运动时间T和5次插值多项式函数系数之间的关系映射。求解出优化后的时间T，即可求得系数e。优化目标函数及约束条件为：

式中：T为待优化的这段轨迹的运行时间；θ·_i是第i个关节的角速度；Ӫ_i是第i个关节的角加速度；τ_i是第i个关节的所受力矩。设定最大角速度θ·_max=4rad/s，最大角加速度Ӫ_max=12rad/s²，最大关节受力矩τ_max=1.3N·m。

粒子群优化算法是一种新的基于群体智能的优化算法^[10]。它是一种模拟鸟群飞行觅食行为，并通过个体之间的协作来寻找最优解的进化计算技术^[11]。

首先，粒子群算法在一维解空间中，给粒子总数为n个的粒子群，每个粒子只有一维属性，随机给定空间位置x_i和飞行速度v_i，并规定迭代次数上限M。粒子的适应度值由待优化目标函数决定，该值用来衡量优化的好坏程度。在迭代寻优过程中，每个粒子通过跟踪两个极值来更新自己在解空间中的空间位置和飞行速度：个体极值p_i，即粒子本身在迭代过程中找到的最优解粒子；全局极值p_g，即种群所有粒子在迭代过程中找到的最优解粒子。

标准粒子群算法的迭代公式为

式中：v^k_ij为粒子i第k次迭代时飞行速度的第j维分量；x^k_ij为粒子i第k次迭代时空间位置的第j维分量；p^k_ij为粒子i第k次迭代时个体极值的第j维分量；p^k_gj为第k次迭代时为全局极值的第j维分量；w为惯性权重；c₁、c₂为学习因子；r₁、r₂为[0，1]范围内的均匀随机值。

适应度函数设为时间T，因为优化目标为运动时间最短，所以适应度值越低，代表优化结果越好。

fit（i）=T（15）

除了运动时间要尽可能小，还要考虑到约束条件式（12），将运动控制在合理的范围内。在这方面，通常可采用罚函数调整超出约束条件的粒子适应度值，或者采用两套适应度函数^[12]，在超出约束条件时切换，迫使其收敛到约束条件内。但这些都需要数次迭代，才有可能收敛约束内，效率不高。而分析约束条件和优化目标可得，超出角速度、角加速度或力矩限制，都是由于时间T过小引起，因此可在超出约束时，限定该粒子往时间T增大的方向重新迭代至满足约束条件的粒子位置。具体迭代步骤如下。

1）初始化粒子群，在搜索空间中随机生成n个一维粒子，即n组时间T，并初始化粒子的位置x_i和速度v_i。

2）将n组时间T代入式（8）—式（10），求解出式（10）中5次插值函数的系数矩阵e。计算出各个轨迹的最大角速度、角加速度、力矩是否超出约束条件。若是，则将该粒子的位置x_i朝|v_i|随机增加，增大时间T。

3）对每个粒子，用它的适应度值fit（i）与个体极值p_i和全局极值p_g比较。若是fit（i）更大，则更新p_i或p_g。

4）利用迭代公式（13）—式（14），更新粒子的位置x_i和速度v_i。

5）若到达迭代次数上限，则结束算法，并输出优化结果，否则返回步骤2）。

4 仿真结果分析

在式（13）中，等式右边第1部分表示粒子第k次时刻的速度，保证算法的全局收敛性，右边第2、3部分使算法具有局部收敛性。因此，式中惯性权重w是表示在多大程度上继承原来的速度。

标准粒子群算法采用固定权重，通常根据经验在0.8～1.2之间选取，虽然具有较快的收敛速度，但其后期容易陷入局部最优，求解精度低。因此通常改进粒子群算法采用可变权重。一种采用较多的动态惯性权重值是线性递减权重策略，其表达式如式（16）所示，而改进后的权重策略如式（17）所示。

式中：w_start、w_end分别为设定的惯性权重w的最大、最小值；k表示当前为第k次迭代；M为最大迭代次数。

在算法初期收敛速度稍慢，但在后期会提升局部搜索能力，从而更有可能找到全局最优解。3种权重策略的权重值变化曲线如图 7所示。

分别使用3种权重策略进行时间最优轨迹优化，过程中适应度值随迭代次数变化曲线如图 8所示。可以看出，固定权重策略寻找最优值速率慢，直到第17次迭代才到达最优值，且陷入局部最优，求解精度低；最优适应度值为0.795 2，明显高于另两种动态权重策略。线性权重策略较固定权重寻优速度明显提升，但依旧容易陷入局部最优，在第16次迭代到达最优，最优适应度值为0.788 1。而改进权重策略，在前期维持了算法的全局搜索能力，后期收敛速度较快，提升了算法的局部寻优能力，因此在寻优速度以及最优值上都有明显的提升，在第4次迭代时就到达最优值，且最优适应度值为0.785 9，均优于另两种权重策略。

改进粒子群算法优化过程中，以运动时间t为优化目标，在约束条件范围内，使得运动时间t尽可能小，其等效于使得各关节的平均角速度在不超出电机最大速率的情况下尽可能增大，以提高电机的使用率，各关节平均速度变化如图9所示。从图 9可看出，改进粒子群算法有效地提高了各关节的平均角速度。

图10是经过改进粒子群优化后，各关节在整段运动轨迹过程中的角度、角速度、角加速度以及所受力矩的变化。

从图中可以看出整段轨迹中，运动平滑连续，没有突然的脉动冲击，且结合式设定的边界条件可以看到角速度、关节受力矩均离边界值有一定距离，而角加速度达到了边界值Ӫ_2max=－12rad/s²，证明已达到在边界条件范围内的时间最优解。因此，仿真验证了该改进粒子群算法优化的有效性。

5 结语

本文以所设计的水下四足机器人的二自由度机械腿为例，设定了关节角度、角速度、角加速度以及关节受力矩的约束条件，以时间最优为目标，提出了在笛卡儿空间中的基于改进粒子群优化的5次多项式插值轨迹规划方法。

通过粒子群优化算法，避免了多项式高阶由此引起的优化难度高、计算复杂的问题，并且以优化目标空间为搜索空间，将粒子维度从6维降至1维，大大减少计算量，且采用改进权重策略后，相较于固定权重和线性权重，在优化速率和优化精度上有明显的提升。最后通过仿真，得到平稳无突变的运动轨迹，并且关节角加速度达到了约束条件的临界值，说明已达到了约束条件内的最优，验证了优化方法的可行性。

参考文献：

[1] SAKAKIBARA Y，KAN K，HOSODA Y，et al. Foot trajectory for a quadruped walking machine[C]//IEEE International Workshop on Intelligent Robots and Systems，Towards a New Frontier of Applications. Ibaraki，Japan： IEEE，2002：315-322.

[2] 刘吉成，季洪超. 新型轮腿机器人步态规划策略[J]. 中国机械工程， 2019， 30（18）： 2257-2262.

[3] 李满宏，张建华， 张明路. 新型仿生六足机器人自由步态中足端轨迹规划[J]. 中国机械工程， 2014， 25（6）： 821-825.

[4] 柏龙，龙樟，陈晓红，等. 连续电驱动四足机器人腿部机构设计与分析[J]. 机器人，2018，40（2）：136-145.

[5] 王立鹏，王军政，汪首坤，等. 基于足端轨迹规划算法的液压四足机器人步态控制策略[J]. 机械工程学报，2013，49（1）：39-44.

[6] 韩宝玲，朱琛，罗庆生，等. 液压四足机器人关键步态参数优化[J]. 北京理工大学学报， 2018， 38（10）： 1056-1060.

[7] CHOI Y K，PARK J H，KIM H S，et al. Optimal trajectory planning and sliding mode control for robots using evolution strategy[J]. Robotica，2000，18（4）：423-428.

[8] 董中华，陈鹏飞，李亚龙. 采摘机械臂的PSO-RBF神经网络自适应控制[J]. 机械制造与自动化，2022，51（1）： 181-183，233.

[9] 崔越. 电驱四足机器人步态规划及运动控制系统设计[D]. 马鞍山：安徽工业大学，2019.

[10] SHI Y，EBERHART R. A modified particle swarm optimizer[C]//1998 IEEE International Conference on Evolutionary Computation Proceedings. IEEE World Congress on Computational Intelligence （Cat. No. 98TH8360）. Anchorage，AK，USA： IEEE，2002：69-73.

[11] 张顶学，关治洪，刘新芝. 一种动态改变惯性权重的自适应粒子群算法[J]. 控制与决策，2008，23（11）：1253-1257.

[12] 付荣，居鹤华. 基于粒子群优化的时间最优机械臂轨迹规划算法[J]. 信息与控制，2011，40（6）：802-808.

收稿日期：2022-12-06