蛇形机器人桥梁缆索攀爬步态控制研究

魏 武 孙洪超

华南理工大学，广州，510640

0 引言

由于斜拉桥的缆索长期暴露在外，所以需定期对其进行检测，以保证其安全性。而长期以来缆索的监测工作都由人工完成，检测的准确性和检测人员的安全性不高。现有的国内外缆索检测机器人多采用轮式［1－2］或气动方式［3］，但这两种机器人安装操作不方便，而且对于不同直径的缆索需要不断增加或减少模块。本文将蛇形机器人用于缆索检测中，蛇形机器人不仅可以在缆索上灵活运动，而且对于不同直径和表面状况的缆索可以通过步态的调整来调节适应。

桥梁检测蛇形机器人的运动主要是以螺旋攀爬为主。在设计蛇形机器人时，应注意使机体的机械结构、步态以及控制系统有利于螺旋攀爬运动的进行。螺旋攀爬运动是一种三维运动，本文选择正交关节作为关节链接方式，为了增大蛇体表面的摩擦力以及蛇体与缆索的接触面积，需要对蛇体关节模块的表面进行处理。

蛇形机器人具有较多的关节，这为蛇体运动的灵活性提供了方便，但另一方面必须协调好每个关节来实现蛇体的控制，这样就大大增加了控制的难度。解决这一问题的直接方式就是寻找一种参数化的步态。目前，步态设计方法主要有两种：一种是将蛇体的步态看作是骨干曲线的定向移动［4］；另一种方法则将步态设计成具体关节角转角的控制函数［5］。前者我们称之为骨干曲线法，后者称作控制函数法。骨干曲线法最早是由Hirose［6］提出并将其应用于蛇体的蜿蜒运动中的，Burdick等［4］将骨干曲线法应用于复杂的三维侧移运动中，Goldman等［7］用骨干曲线法实现了万向节关节蛇形机器人的螺旋攀爬运动。控制函数法的应用比较多，最早也是由Hirose使用［6］。近年来，李斌等［8－9］分别给出了实现蛇形机器人蜿蜒运动、伸缩运动和侧向运动的控制函数；Tesch等［10］提出了一系列控制函数实现了正交关节蛇形机器人的侧移、螺旋攀爬等三维运动；另外，Ijspeert［11］利用 CPG（central pattern genera－tor）方法也得到了控制函数。上述两种方法在实现过程中各自存在缺陷：骨干曲线法基本都是应用在万向节关节的蛇形机器人上，在实施过程中要将蛇体的骨干曲线以及其位移转换成低级别驱动器的输入，这一过程比较繁琐，求解存在各关节耦合的问题；控制函数法直接给出了驱动器的输入值，无法直观地得到蛇体整体的运动趋势，缺乏宏观性［12］。为了克服上述两种方法存在的缺陷，本文采用一种新的步态生成方法：迭代骨干曲线拟合以及关键帧提取综合的步态生成方法。该方法将骨干曲线方法和控制函数法结合在一起，在发挥两种方法优点的同时，能够有效地抑制两种方法的缺陷。

1 蛇形机器人建模

蛇体的攀爬运动是一种三维运动，具有三维运动能力的蛇形机器人的关节链接方式主要有三种：万向节、P－R关节［13］、正交关节。万向节结构的关节工作空间为完整的三维空间，关节运动灵活，但其结构复杂。P－R关节过长，蛇体与缆索的接触面积较小，增大了攀爬运动的难度。正交关节与普遍采用的万向节结构相比，完成完整的三维运动所需要的关节更少，控制更简单，且具有较好的鲁棒性。本文中的蛇形机器人采用正交关节链接。

正交关节链接方式如图1所示，第n个关节包含关节点的位姿信息以及其转动轴的方向信息，第n个连杆则是第n个关节与第n＋1个关节之间的链接，它们共同组成了蛇形机器人的第n个模块。

图1 正交关节链接方式示意图

本文设计的蛇形机器人运动学模型如图2所示。其中，kj表示第j个关节转动副轴线的方向向量，lj表示第j个连杆轴线的方向向量。由图2可知，该蛇形机器人存在三个约束：①相邻两个关节转动副轴线相互垂直；②相邻两个关节转动副轴线与二者之间连杆轴线相互垂直；③相邻的水平（或竖直）方向的关节转动副轴线指向相反。

本文中蛇形机器人运动学模型D－H参数如表1所示。其中，θj（j＝1，2，…，19）为从lj－1到lj绕kj旋转的角度；dj为从lj－1到lj沿kj测量的距离；aj为从kj到kj＋1沿lj测量的距离；αj为从kj到kj＋1绕lj旋转的角度。

图2 蛇体的运动学模型

表1 蛇体D－H参数表

关节j到关节j＋1的变换矩阵为

将表1中的值代入式（1）得到：

实际应用中，L为已知量，则jTj＋1为θj的函数。如果在已知第一个关节与原点之间的变换矩阵RT1（A0）情况下，则第j个关节的位姿RTj＝A0A1A2…Aj－1。

2 蛇形机器人螺旋攀爬步态生成方法

传统的两种步态生成方法均存在一定的缺陷，骨干曲线法注重骨干曲线的整体性，但在转化为驱动器输入时，往往通过逆运动学求解，在求解过程中存在关节耦合的问题［14］，控制函数法则以驱动器的具体输入为出发点，无法直接得到骨干曲线的情况。本文使用的骨干曲线链拟合与关键帧骨干提取相结合的方法可以将传统的两种步态生成方法的优点有效地结合，使形成的步态既可以直观地得到骨干曲线的形状及运动趋势，又可以得到以关节号和时间为变量的控制函数，方便底层驱动器控制。

该算法首先进行骨干曲线拟合。采用一种新的拟合算法——迭代链拟合算法，该算法不要求关节具有两自由度（以往的骨干曲线拟合算法往往针对万向节关节［4，7］），具有更加广泛的应用范围。然后，将该拟合算法应用到一系列固定的骨干曲线中，这些骨干曲线表征蛇体在特定运动中的形状和运动趋势，将这一系列的骨干曲线称为关键帧骨干曲线，它们是蛇体在固定时间间隔的离散时间点上的骨干曲线。可以将这一过程看作是将蛇体运动过程的离散化。在完成一系列关键帧骨干曲线的拟合后，得到各个关节在整个步态中各个离散时刻的角度。对这些离散数据进行整理分析便可得到与关节号和时间相关的一系列的控制函数。整个算法从蛇体骨干曲线为出发点，最终得到每个关节具体的控制函数。

2.1 骨干曲线的链拟合

以往基于骨干曲线的步态形成方法都是针对万向节关节的蛇形机器人，每个关节有两个自由度，骨干曲线拟合只需调节关节转角顺次将每个关节放在骨干曲线上即可［15］。本研究中的蛇形机器人采用的是正交关节，每个关节只有一个自由度，在对三维曲线的拟合过程中，某给定关节与曲线曲率的拟合程度与上一关节的转角有关。在设计针对正交关节的曲线拟合算法时，考虑某关节角时还必须对这一关节前面的关节转角进行优化。

2.1.1 初始模块位置的确定

蛇体与骨干曲线间的拟合从首关节与骨干曲线初始端的拟合开始，蛇体首个模块的位置由它的一个指定法向量与骨干曲线某一指定法向量的夹角φ唯一确定。将φ称作蛇体相对于骨干曲线的扭转角。

蛇体的第一个模块作为蛇体骨干曲线的一条割线，模块始端与骨干曲线初始点重合。图3中，k0为初始模块的法向量；l0为初始模块的中轴线向量；v为骨干曲线的起始点切线的法向量，该向量同时垂直于l0；u为骨干曲线初始点的切向量；φ为顺着l0正方向看去，v到k0的逆时针转角。由图3中各向量的关系可以看出，骨干曲线一旦确定，初始关节的位姿A0可由φ唯一确定。

图3 首关节位姿确定示意图

2.1.2 迭代链拟合

蛇体由n个模块组成，若同时对每个关节转角进行优化则优化函数中含有n－1个变量，计算量将会很大。因此本文采用迭代法通过多次的少量参数优化来达到优化目的。

在每次的迭代计算中将所有关节划分为三种类型：①已完成优化的关节，已经完成与曲线拟合的关节在后续关节的拟合中转角不会再发生变化；②正在优化的关节；③自由关节，这一部分关节转角将在后续迭代中进行优化。

图4 迭代优化窗口

由图4可知，不同的模块由一个优化窗口分割开，优化窗口的大小表示当前优化关节的个数，迭代步长则表示每次迭代后当前优化关节转化成已优化关节的个数（或是待优化的自由关节转化成当前优化关节的个数）。对当前优化关节的转角进行优化，使当前优化模块能够最大程度地拟合骨干曲线，本文迭代窗口大小为3，迭代步长为1。本文使用如下函数作为第i次迭代的优化目标函数：

其中，Di（αopt）为优化目标函数，αopt为迭代窗口中待优化的关节角度，dj为当前优化模块远端（第j＋1个关节点）到骨干曲线的距离，当前优化模块远端的位置利用标准D－H法求取。设正在进行第i次迭代优化，需要通过优化θi、θi＋1以及θi＋2使第i＋1、i＋2及i＋3关节点最大程度地靠近螺旋曲线。优化窗口中第一个需优化关节为第i个关节，由于第i个关节之前的各关节已经完成优化，其关节角都已知，则第i个关节点的位姿RTi可以确定，第i次迭代优化中由D－H方法可得各关节点的位姿：

关节的位姿矩阵的最后一列表征了相应关节点在标准坐标系中的坐标。该坐标的点与指定骨干曲线的距离便是需要优化的距离值。式（3）中还包含一个惩罚函数κ（αj）。在蛇形机器人实际运动中由于自身机械设计以及驱动器的约束，关节转角受一定的限制，可以利用惩罚函数限制关节转角的范围。另一方面，惩罚函数亦可防止在拟合过程中出现极端角度，而导致多个模块“摞”在骨干曲线的某个位置。针对不同的蛇体惩罚函数，β的选择也不同，β表征了对驱动器转角的限制，理论上每一个关节驱动器均可绕关节轴转动任意角度，但是由于所选择驱动器的差异以及蛇体关节机械设计的限制，驱动器的转角仅能在某一范围内变化，在惩罚函数中便用β表征这种限制。β在的实际选择中应有一定的余量，应小于最大转角弧度制的绝对值。譬如，某蛇体关节转角范围的弧度制表示为［－1.2，1.2］rad，则β的取值可选1，这样可以尽可能地使拟合过程中的各关节转角不超出实际蛇体关节转角的限制值。

每一次迭代优化的结果都会直接影响后面的优化结果，第一个关节的位置对整个曲线拟合有很明显的影响，其他模块通过调节关节角才能确定蛇体的形态。因此有必要对第一个模块进行二次优化。为了改善初始边界条件，在一定程度上维持第一模块的位置，将第一模块的二次优化放在第一次迭代和第二次迭代之间进行。具体方法是在第一次迭代结束后，以第四模块尾端为初始端对前面的关节进行曲线拟合来确定第一模块的位置，当第一模块位置确定后，进行第二次迭代。其余关节的拟合按照上述迭代链拟合方法进行。

2.1.3 迭代链拟合算法

将迭代链拟合算法的步骤总结如下：

（1）将蛇体第一模块作为骨干曲线的一条割线，模块始端与骨干曲线的初始点重合，使φ为某一值。

（2）固定第一模块作第一次迭代，对前3个关节转角进行优化。

（3）以第4模块为端点对前3个关节进行优化，在此过程中第一模块为自由模块，确定出第一模块的最优位置。

（4）按照前述迭代链拟合方法对蛇体的每个关节逐个优化。

2.2 关键时刻骨干曲线提取

蛇形机器人的一个重要的特点是能够缠绕在圆柱体上（图5）并向上攀爬。在这种步态下，如图5所示，身体的各个模块沿各自中轴向某方向翻滚。

图5 攀爬运动骨干曲线移动示意图

将蛇体攀爬过程看作是其螺旋状的骨干曲线随时间变化的过程，我们将这个连续的过程离散化，以若干等距时间点tj的骨干曲线作样本，用迭代链拟合方法对样本骨干曲线进行拟合，得到蛇体各个关节在样本时间点的转角。具体做法是：在每个时间点tj，初始模块摆放时的扭转角φj＝tj（例如若选中tj＝1s为一个时间点，此时φj＝1rad。），然后进行曲线拟合。φ的变化率表征了骨干曲线移动的速度。在完成关键帧的骨干曲线拟合后，需对生成的步态信息进行进一步的分析。蛇型机器人关节控制函数是关节号n与时间t的函数。

3 蛇形机器人桥梁缆索攀爬步态控制仿真与实验

3.1 迭代链拟合与关键帧提取综合方法仿真

本文为了验证上述算法，利用MATLAB机器人工具箱针对一系列φ对蛇体进行迭代链拟合，得到了不同关键时间点不同关节的转角，并作出了蛇体螺旋攀爬运动步态波形和不同关节转角随时间变化曲线，为形成控制函数提供了支持；然后用1stOpt对控制函数的参数进行估计，得到了与期望骨干曲线及其运动趋势对应的控制函数；最后通过Webots机器人仿真软件对本算法生成的控制函数控制效果与传统控制函数法的控制效果进行了比较。

蛇体在攀爬运动中的骨干曲线为螺旋线，为了最大程度地接近现实实验中的各种条件，设骨干曲线螺旋半径R＝70mm，螺距为314mm，蛇体连杆长度为55mm，本文中β取1。链拟合过程中为了防止关节角出现局部最小值，采用遗传算法进行迭代优化。链拟合结果如图6所示。

图6 蛇形机器人骨干曲线对螺旋线拟合

在关键帧提取时，φ的取值为iπ/10，i＝1，2，…，20，得到多个时间点的各关节的转角后，可得到拟合图（图7）。图中的实线为本方法生成的步态信息。

图7 螺旋攀爬运动步态波形

图8为关节3、关节4、关节5转角随时间变化曲线，从图8可以看出，每一个关节转角随时间呈周期性变化，且不同关节的控制函数存在一定的相位差。因此可以将蛇体关节的控制函数表示为θ＝asin（ωt＋nb），其中，θ表示关节转角，ω表示每个关节输出角度变化周期，a表示关节的最大转角，b表示关节之间的相位差。利用1stOpt软件使用遗传算法对a、b的参数进行估计，得到a＝0.985，b＝1.82。此时便得到螺旋半径R＝70mm，螺距为314mm，蛇体连杆长度为55mm，φj＝tj时蛇形机器人攀爬运动的控制函数为θ＝0.985sin（t＋1.82n）。根据此函数，以t和n为变量作图得到图7点状线所表示的图形。

图8 不同关节转角随时间变化曲线

将此控制函数在Webots仿真环境下进行可行性验证，如图9a所示，以θ＝0.985sin（t＋1.82n）为控制函数，蛇体可在半径为70mm的竖直杆上完成攀爬运动。至此，本文完成了由骨干曲线及其运动趋势到蛇形机器人底层控制函数的对应。

传统的骨干曲线法应用在万向节关节的蛇形机器人上，与本文方法不便比较。使用传统的控制函数法，有多个参数化的控制函数可实现同一半径竖直杆的攀爬。魏武等［16］运用粒子群优化算法（PSO）对控制参数进行了优化，给出了正交关节蛇形机器人控制参数和螺旋半径之间的拟合函数：

式中，变量x表示控制参数a的值；变量y表示控制参数b的值。

以不同半径竖直杆对应的蛇体步态参数作为样本，使用遗传算法对pi（i＝1，2，…，8）进行拟合得到对应于本蛇体模型的参数值如表2所示。

表2 pi的优化参数值

由式（5）中螺旋半径与步态参数的关系可看出同一螺旋半径有多组步态参数与之对应。图9中三幅图对应的竖杆的直径均为70mm，图9b对应的控制函数为θ＝0.8sin（t＋1.88n），图9c对应的控制函数为θ＝1.21sin（t＋1.7n），由于传统的控制函数法无法由控制函数直接得到骨干曲线，进而无法对蛇体攀爬运动进行宏观的考量，而在分析蛇体安全性以及蛇体能量最优时这种宏观的考量是必须的。

图9 不同控制函数在Webots环境下螺旋攀爬运动仿真

从上面的仿真可知，迭代链拟合与关键帧提取综合算法以蛇形机器人的骨干曲线及其运动趋势为出发点，得到了蛇形机器人底层驱动器的控制函数。在蛇形机器人攀爬运动中，缆索的表面状况复杂，通过视觉设备可以对缆索的表面状况进行实时检测，进而可以分析出对蛇形机器人安全性最有利的骨干曲线及合理的运动速度。由这些信息便可使用本文提出的算法得到底层驱动器的输入。正交关节单个驱动器的控制稳定性和鲁棒性较好，可以加强攀爬运动中的安全性，由于整个过程具有一定的实时性，蛇形机器人攀爬运动的步态更加灵活。

3.2 蛇体攀爬运动实验

上文的仿真对迭代链拟合与关键帧提取综合方法与传统的步态形成方法的比较结果说明了本文方法的优越性。现将生成的控制函数应用在实际中以验证其可行性，并进一步观察其在实际应用中要注意的问题。

蛇形机器人以舵机作为关节驱动器，单个舵机输出扭矩为1.04N·m，蛇体包含19个关节，总质量为2.3kg。根据蛇形机器人的特点使用基于双冗余CAN总线和无线局域网实现蛇形机器人的多级分布式控制。上层监控系统可通过无线局域网向蛇体发送各种控制命令，蛇体通过CAN总线将命令发送到各个从控制器，从控制器根据接收到的命令计算出每个舵机所要完成的角度变化，然后输出PWM信号控制舵机的运动 。

以硬质PVC管模拟桥梁缆索进行攀爬运动实验，如图10所示。PVC管外侧包裹一层防滑材料，以增大摩擦力，PVC管直径为140mm。利用前面步态生成方法得到的控制函数θ＝0.985sin（t＋1.82n）进行实验。

图10 蛇形机器人攀爬实验

如图10a所示，利用本文方法所生成的控制函数，蛇体可实现对与地面夹角60°的斜杆的攀爬，沿斜杆径向攀爬速度为8cm/s；如图10b所示，蛇体以同样的控制函数沿竖直杆攀爬，攀爬速度为3cm/s。由于重力的作用，竖直情况下蛇体攀爬的速度不理想，通过对步态参数的微调得到新的控制函数θ＝sin（t＋1.83n），使蛇体“抱紧”PVC管，蛇体可达到6cm/s的攀爬速度。实验表明，在实际应用中，该步态生成方法所生成的控制函数并不能完全适应攀爬环境的变化，还需通过其他传感器（如压力传感器）的配合才能达到最佳的控制效果，这也是下一步研究的重点。

蛇形机器人对桥梁缆索的检测任务，主要是利用这种螺旋攀爬运动使蛇形机器人沿缆索上下运动，通过安装在蛇形机器人体内的检测装置对缆索健康状况进行检测。放置在蛇形机器人体内的检测装置可以包括电磁传感器、摄像头等。随着在螺旋攀爬运动过程中蛇身的转动，检测装置可以对缆索进行360°全方位的信号采集，完成对缆索断丝、腐蚀、护套损伤等情况的检测任务。

4 结语

本文根据桥梁缆索攀爬蛇形机器人在攀爬运动中所面临的问题，对蛇形机器人的结构、步态生成方法进行了优化，将传统的骨干曲线法与控制函数法有效结合，得到了迭代链拟合与关键帧提取综合方法，建立了蛇体攀爬运动过程中宏观骨干曲线与底层驱动器控制函数之间的关系，进而为蛇体运动的灵活性和安全性提供了保障。通过仿真实验说明了步态生成方法的可行性与优越性。下一步的研究将把本方法与蛇体的压力、力矩等传感器进行结合，进一步地对步态进行优化。

［1］Aracil R，Saltaren R J，Reinoso O.A Climbing Parallel Robot：A Robot to Climb Along Tubular and Metallic Structures［J］.IEEE Robotics and Automation Magazine，2006，13（1）：16－22.

［2］Xu F Y，Wang X S.Design and Analysis of a Bisected Wheel－based Cable Climbing Robot［J］.Journal of Southeast University（English Edition），2009，25（1）：41－46.

［3］罗均.缆索维护机器人及其关键技术的研究［D］.上海：上海交通大学，2000.

［4］Burdick J W，Radford J，Chirikjian G S.A Sidewinding Locomotion Gait for Hyper－redundant Robots［J］.Robotics and Automation，1993，3（3）：101－106.

［5］Limpkin K，Brown I，Peck A，et al.Differentiable and Piecewise Differentiable Gaits for Snake Robots［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.San Diego，CA，USA，2007：1864－1869.

［6］Hirose S.Biologically Inspired Robots（Snake－like Locomotor and Manipulator）［M］.Oxford：Oxford University Press，1987.

［7］Goldman G，Hong D W.Considerations for Finding the Optimal Design Parameters for a Novel Pole Climbing Robot［C］//ASME Mechanisms and Robotics Conference.New York，2008：859－866.

［8］李斌，叶长龙.蛇形机器人的扭转运动研究［J］.中国机械工程，2005，16（11）：941－945.

［9］李斌，马书根，王越超，等.一种具有三维运动能力的蛇形机器人的研究［J］.机器人，2004，26（6）：506－509.

［10］Tesch M，Lipkin K，Brown I.Parameterized and Scripted Gaits for Modular Snake Robots［J］.Advanced Robotics，2009，23（9）：1131－1158.

［11］Ijspeert A J.Central Pattern Generators for Loco－motion Control in Animals and Robotics［J］.Neural Networks，2008，21：642－653.

［12］Hatton R L，Choset H.Generating Gaits for Snake Robots：Annealed Chain Fitting and Keyframe Wave Extraction［J］.Autonomous Robots，2010，28（3）：271－281.

［13］孙洪，刘立祥，马培荪.攀爬蛇形机器人爬树的静态机理研究［J］.机器人，2008，30（2）：112－122.

［14］Goldman G J.Design Space and Motion Development for a Pole Climbing Serpentine Robot Featuring Actuated Universal Joints［D］.Virginia：Virginia Polytech－nic Institute and State University，2009.

［15］Anderson S B.Discretization of a Continuous Curve［J］.IEEE Transactions on Robotics，2008，24（2）：456－461.

［16］魏武，张占.基于控制函数的蛇形机器人攀爬运动分析［J］.控制工程，2011，18（2）：322－326.