一种多关节履带式机器人自主越障运动规划

朱 磊，樊继壮，赵 杰，吴晓光，韩震峰

(哈尔滨工业大学机器人技术与系统国家重点实验室，150001哈尔滨，rayjew@sohu.com)

机器人的运动规划就是在一定的工作空间中，求解使得机器人能够从起始位姿达到终止目标位姿的中间运动序列的问题[1].因此，运动规划问题就可以看作是满足一定优化指标的，针对一系列空间位姿的机器人逆运动学的求解问题.目前，较为广泛采用的求解机器人逆运动学的方法有解析法、代数法、几何法、数值解法等.前三种方法由于涉及到矩阵与反三角函数运算，不适合处理多关节的机器人逆运动学问题;数值方法模型简单，但是不能求得所有解，而且计算量随着关节数目的增加呈指数增长[2－3];传统的方法已经无法满足需要.随着遗传算法[4]、神经网络算法[5]、模拟退火算法[6]、蚁群算法[7]、粒子群算法[8]等现代启发式数值优化方法的出现，为机器人的逆运动学求解提供了便捷的方式.以上算法都有各自的优点，但是当自变量数较多而且之间有耦合作用时，存在着收敛速度慢，结果不精确等一些缺点.人工鱼群算法作为一种新的优化算法已经得到了越来越多的认可，在很多领域中得到了应用[9－10].

本文提出了一种改进的人工鱼群算法，用于求解多关节履带式机器人的越障运动学逆解.首先将要求解的问题转化为几个嵌套的子问题分别应用人工鱼群算法，然后通过一种随机搜索的方法缩小自变量的搜索范围，并且自适应地调整视野和步长，从而可以离线精确地获得机器人越障的各个关键步骤的关节角度，满足目标函数的要求.仿真结果证明了方法的正确性与可靠性.利用得到的关节角度值对机器人进行越障规划实验，结果显示机器人能够顺利地越过高度为500 mm的垂直障碍.

1 机器人系统本体结构设计

由于履带式移动机器人可以较好地适应复杂地形，而多关节的机器人具有较大的灵活性，本文根据机器人的应用环境和实际功能，设计了一种用于矿难后井下环境探测的多关节履带式矿难搜索机器人.该机器人采用4个履带单元串联的多关节铰接的蛇形结构(如图1所示)，可以在崎岖地形中自由行走，并且可翻越一定高度的障碍.机器人的运动主要由4个行走单元模块以及3个十字轴俯仰偏航关节模块来完成.依据功能可以分为传感器单元、控制与其他单元、中间俯仰单元以及节点单元.俯仰偏航关节模块则通过两对锥齿轮分别驱动一个偏心的十字轴的两个垂直轴，可以轻松地实现机器人各个关节之间的俯仰与偏航运动.

1 机器人总体结构示意图

2 机器人越障运动性能分析

由机器人的自由度分析可知，对于高度比履带轮半径还小的障碍，机器人可以轻松越过，而不需要对姿态做任何改变;对于不方便直接越过而横截面积很小且可以通过转弯绕行避开的障碍，机器人可以通过对关节偏航运动实现各个单元之间的运动，控制也相对简便;但是对于无法绕过或者很难绕过的较大障碍，机器人只能通过抬起模块单元的方式进行越障，此方法是最为复杂的，也是能量消耗最大的方式.因此，本文将重点针对这种方式的运动性能进行分析.

2.1 越障模型分析

根据机器人的本体结构可以建立如图2所示的机器人越障模型，关节角度的变化范围及杆件的长度如表1所示，并且均假定每一单元的质心位置为其中心位置.

图2 机器人翻越垂直障碍模型

表1 机器人参数

由文献[11]可知，只有抬起的最前端的单元的质心超过障碍物的高度，机器人才能平稳越过障碍，因此，对于单单元越障模式机器人所能越过的高度应该满足(如图3(1)).

当θ4=45°时，通过式(1)可以得到机器人所能越过的最大高度H1max=166.59 mm.图3(1)中最右边的为传感器单元(图3与图4中的各单元放置顺序均与此相同).

图3 单单元模式越障动作规划

对于双单元越障模式机器人能够翻越的高度应该满足(如图4(2))所示)

当 θ3=45°，θ4=45°时，通过式(2)可以得到机器人所能翻过的最高障碍为H2max=620.05 mm.

图4 双单元越障动作规划

2.2 单单元越障模式

由上面分析可知，对于高度小于166 mm的垂直障碍可以采用单单元越障模式，如图3所示.设机器人初始状态各个关节角度均为0.每一步骤中由于只有一个角度是自变量，因此很容易得到各个角度在不同阶段与高度之间的关系.

当抬起第1单元越障时(如图3(1)所示)，

当抬起第2单元时(如图3(2)所示)，

当抬起第3单元时(如图3(3)所示)，

当抬起第4单元时(如图3(4)所示)，

2.3 双单元越障模式

当要翻越的垂直障碍的高度大于166 mm、小于620 mm时，机器人只能采用双单元越障模式，如图4所示.设机器人初始状态各个关节角度均为0.

对于步骤1与步骤2，机器人的角度参数与高度的关系应该满足

对于步骤3与步骤4，机器人的关节角度参数与高度的关系应该满足

对于步骤5，机器人的关节角度参数与高度的关系应该满足

对于步骤 6 与步骤7，θ3‴= θ4‴=0，θ2‴=θ2″，θ1‴= － θ2″.对于步骤 8，机器人的关节角度全部都为0.

因此，可以依据要翻越障碍的高度与优化指标决定各个状态变化时刻的关节角度值，完成机器人基于逆运动学的自主越障规划.

3 基于NAFSA的自主越障运动规划

3.1 机器人越障运动规划优化问题

由上面的分析可知，单单元越障模式每步骤中的关节角度可以容易地得到解析解.而双单元越障模式中每一个步骤则与多个关节角度有关，存在多组解，并且关节角度相互之间有制约关系，难于求解，需要针对双单元越障模式进行合理的运动学优化.由于机器人是通过电池提供能量，每一次运动如果能量消耗最低将会增加机器人的运动时间，从而扩大搜索范围，对救援工作提供更多的支持.而翻越垂直障碍时，机器人的能量耗费与关节角度的变化大小有关系，因此本文将机器人在越障中各个步骤间的关节角度的变化值作为目标函数进行优化，即

其中，θi、θi'、θi″分别对应了步骤1 与步骤2 中、步骤3与步骤4中以及步骤5中的机器人所对应的第i个关节角度.因此可以将越障运动规划理解成为满足式(3)～(5)并且使得式(6)取得最小值的最优关节角度值的求解问题.由于式(6)具有多变量、非线性的特点，传统的解析方法很难获得最优解，提出了一种基于改进人工鱼群算法的机器人越障的逆运动学求解方法.为了使问题简化，由式(3)～ (5)可知，θ4、θ4'，θ2″可以由 θ3、θ2'、θ3'、θ3″来表示，从而使得式(6)优化自变量数由7个减少至4个.

3.2 人工鱼群算法简介

人工鱼群算法是2002年李晓磊等[9]提出的一种基于动物行为的自治体寻优模式.是模拟自然界中鱼的觅食、聚群和追尾行为以及鱼群之间的相互协助从而达到全局寻优的目的.但是，人工鱼群算法在求解优化问题中依然存在着一些问题:1)当搜素域较大或处于变化平坦的区域时，搜索性能变差，收敛速度变慢;2)算法一般在初期收敛较快，后期却往往收敛较慢;3)AFSA具有把握搜索方向和在一定程度上逃出局部最优的特性，但当很多人工鱼处于漫无目的地随机移动或人工鱼群在非全局极值点出现较严重聚集情况时，大大降低了收敛速度，使得精度也大大降低[10].

3.3 基于改进的人工鱼群算法的越障逆运动学求解

3.3.1 多层分组优化思想

考虑到本文目标函数的自变量有4个，如果同时对其进行优化，运算的复杂程度将会很高，而且自变量之间互相影响很难收敛到理想的结果.由于θ3与θ3″对于求解目标函数的最优值是互不相关的，所以利用分组的思想将目标函数分解为两个子函数，即

这样可以将问题改进成为了一个三层四组单自变量的嵌套人工鱼群算法，从而将各个参数之间的耦合作用降到最低，提高了收敛速度.具体的分层原则如下.

第1层人工鱼群(最外层):求出使得目标函数F最小的自变量θ2'.

第2层人工鱼群(中间层):求出在θ2'一定的情况下使得目标函数F取得最小值的自变量θ3'.

第3层人工鱼群(最内层):求出在θ2'、θ3'一定的情况下使得目标函数f1与f2分别取得最小值的自变量 θ3与 θ3″.

通过这三层人工鱼群使得对于自变量θ2'的任意值，可以找到唯一的一组 θ3、θ3'与 θ3″使得F最小，因此只需要对自变量θ2'进行鱼群行为来寻优即可，使得鱼群行为方向明确，算法效率提高，有利于得到最优解.

3.3.2 自适应地调节视野与步长

视野的大小决定了收敛的速度，而步长的大小决定了求解的精度.可以动态调整人工鱼群的视野和步长.在算法运行前期，采用较大的视野和步长，可以增强算法的全局搜索能力和收敛速度，使人工鱼在更大的范围内进行粗搜索;随着搜索的进行，视野V和步长S逐步减小;在算法运行后期，算法逐步演化为局部搜索，定位在最优解附近区域并进行精细搜索，从而提高了算法的局部搜索能力和寻优结果的精度.视野V和步长S可按式(7)动态调整[12]:

其中，s为大于1 的整数，一般取s=5、20、30，Vmin为最小视野，Smin为最小步长，t为迭代次数，Tmax为最大迭代次数.

3.3.3 缩小搜索域

对于优化算法来说，搜索域范围越大其搜索的速度就会越慢，从而导致收敛速度变慢.而且对于很多有约束条件的优化问题，其初始的搜索域往往存在很多不满足条件的解，从而浪费了大量的运行时间.本文采用一种变步长快速随机搜索的方法来缩小自变量的搜索域，进而提高收敛速度.由于搜索自变量上边界与下边界方法一样，所以以θ3下边界的搜索为例说明.

Step 1 设 θ3的下边界为 xl，初始步长为S=2，精度为θjd=0.01°，迭代次数为K=4 000.首先令 θ3=xl，求出 θ4，判断 θ3与 θ4是否满足约束条件，如果满足则直接输出xl，结束搜索;令k=1开始迭代.

Step 2 判断θ3=x1(k)时，θ3与θ4是否满足约束条件:

如果不满足约束条件，

如果满足约束条件，则令

其中fix为取整函数.

Step 3 对于k＞2判断

是否成立，如果成立结束迭代，取max((x1(k)，x1(k－1))作为结果输出，跳出迭代.

Step 4 依据公式(7)动态调整S，取S=2，Smin=0.01°.

Step 5 如果k＞K成立，结束迭代，说明变量的下界不存在;如果不成立k=k+1转Step 2.

求上界与求下界类似，但要把Step 2中的加号换成减号，把减号换成加号，在Step 3中取最小值即可.

3.3.4 公告板的使用

公告板是用来保存人工鱼群中的最优人工鱼群个体的.各人工鱼个体在寻优过程中，每次行动完毕就检验自身状态与公告板的状态，如果自身的状态优于公告板状态，就将公告板的状态改写为自身状态，这样就使公告板记录下了历史最优的状态.采用末位淘汰的方法，用公告板中的最优的状态替换掉每次迭代后的最差的状态.

综上所述，每层的人工鱼群算法的目标函数都是相似的，只是自变量有所不同，因此具体的步骤都是相同的.现在以基于自变量θ3的人工鱼群算法的步骤为例来说明.

Step 1 根据变步长快速随机搜索算法确定θ3的取值范围，并且对人工鱼群的参数初始化，生成鱼群 θ31，θ32，…，θ3N1，令 i=1.

Step 2 对鱼群中个体θ3i分别进行觅食、聚群与追尾操作.

Step 3 判断i＞N1+1是否成立，如果不成立，i=i+1转Step 2;如果成立，判断θ3i是否满足迭代收敛条件，不满足，令i=1，并且记录最优个体更新公告板，转Step 2;如果成立输出θ3，对θ3寻优结束.

设带*的为每一层人工鱼群求出的最优解，则整个算法的主要流程如图5所示.

图5 改进的嵌套鱼群算法求解过程

4 仿真结果及分析

为了验证本文所用方法的正确性以及优良的收敛性能，针对相同的高度值H=500 mm，在matlab中分别对传统人工鱼群方法与改进的嵌套鱼群方法进行了仿真验证.设置传统人工鱼群算法的群体规模N=50，迭代次数K=500.对改进的人工鱼群算法的各个参数设置如下，第1层人工鱼的群体规模N=20，迭代次数K=100，第2层人工鱼的群体规模N=10，迭代次数K=100，第3层两组人工鱼的群体规模N=10，迭代次数K=50.两种人工鱼群的公共相同参数分别为拥挤度因子δ=0.618，人工鱼群的最大试探次数ntry=3，初始视野V0=5°，Vmin=0.04°，初始步长S0=2°，Smin=0.01°，s=30.两种算法的机器人的优化的目标函数值与进化代数的关系分别如图6(1)和6(2)所示.可见传统人工鱼群算法需要280代才能收敛到最优解，并且不能保证收敛到全局最优解，而本算法可以在31代内很快地收敛到全局最优解.算法的效率有了显著的提高.

图6 目标函数值变化过程比较

为了验证算法的稳定性，利用本文所提的算法对机器人要越过的3种高度(500、525、550 mm)分别进行了10次优化计算，所得的结果如表2所示.由表2可以看出，算法每次都能收敛到对应高度的全局最优解，从而证明了算法的鲁棒性.

表2 针对3种高度算法连续运行10次的结果比较

5 实验验证

为了证明本文方法的正确性，在高度为500 mm的垂直障碍上依据算法得到的关节角度进行了越障实验，如图7所示.结果表明本文的规划方法能够实现机器人的越障，计算得到的结果是合理的.

图7 双单元模式越障实验

6 结束语

1)依据多关节履带式矿难搜索移动机器人的本体结构，分别建立了机器人的单单元与双单元的越障模型，并对其越障能力进行了分析.

2)设计了一种基于三层四组嵌套人工鱼群算法的机器人越障逆运动学优化算法.该算法只需要完成多次单变量寻优，避免了多变量之间的相互干扰，并且在自变量进行初始化前通过一种变步长的随机搜索方法将变量的搜素域缩小，提高了计算效率.

3)仿真分析表明，这种方法的收敛代数很少，精度也很高，并且可以保证每次都能收敛到最优解.

4)机器人真实越障实验证明，算法规划结果正确，可以用于解决这一类机器人基于逆运动学的越障规划.

[1]宣俊.高维空间机器人运动规划算法研究[D].杭州:浙江大学，2005:1－4.

[2]荣盘祥，杨晶，胡林果，等.基于RBF网络的SCARA机器人的运动学逆解[J].电机与控制学报，2007，11(3):303－305.

[3]周友行，何清华，邓伯禄.一种改进的爬山法优化求解冗余机械手运动学逆解[J].机器人，2003，25(1):35－38.

[4]ALBERT F Y C，KOH S P，TIONG S K，et al.Inverse kinematic solution in handling 3R manipulator via realtime genetic algorithm [C]//Proceedings of the 2008 IEEE International Symposium on Information Technology.Kuala Lumpur:IEEE，2008:1－6.

[5]ZHOU You Hang，TANG Wen Zhuang，ZHANG Jian Xun.Arithmetic for multi-joint redundant robot inverse kinematics based on the bayesian-BP neural network[C]//Proceedings of the 2008 IEEE International Conference on Intelligent Computation Technology and Automation Mechatronics and Automation.Changsha:IEEE，2008:173－178.

[6]何广忠，高洪明，张广军，等.弧焊机器人放置规划问题的研究[J].机器人，2006，28(3):249－254.

[7]梅红，王勇，赵荣齐.基于蚁群神经网络算法的机器人逆解[J].山东大学学报:工学版，2008，38(5):72－76.

[8]芮挺，朱经纬，周游，等.多自由度机器人逆运动粒子群优化求解方法[J].系统仿真学报，2009，21(10):2930－2932.

[9]李晓磊，邵之江，钱积新.一种基于动物自治体的寻优模式:鱼群算法[J].系统工程理论与实践，2002，22(11):32－38.

[10]张梅凤，邵诚，甘勇，等.基于变异算子与模拟退火混合的人工鱼群优化算法[J].电子学报，2006，30(8):1381－1385.

[11]TAKAYAMA T，HIROSE S.Development of"Souryu I＆ II"[J].Joumal of Robotics and Mcchahonics，2003，15(1):61－69.

[12]王联国，洪毅，赵付青，等.一种简化的人工鱼群算法[J].小型微型计算机系统，2009，30(8):1663－1667.