房立金¹, 贺长林², 祝帅², 陶广宏²
1. 东北大学 机器人科学与工程学院, 辽宁 沈阳 110819;
2. 东北大学 机械工程与自动化学院, 辽宁 沈阳 110819
收稿日期：2018-04-16
基金项目：国家自然科学基金资助项目(51575092)。
作者简介：房立金(1965-)，男，辽宁沈阳人，东北大学教授，博士生导师。

摘要：以一款三臂式巡检机器人为例, 对其进行分层规划控制策略分析, 将其分为线路信息库层、动作规划层和执行规划层.线路信息库层根据高压输电线路上的已知信息所得, 其中包括障碍物种类、位置、数量等信息.规划层和执行规划层根据有限状态机理论总结为当输入事件为a(第二层动作规划层)时, 在f(第三层执行层)的作用下, 将状态q(机器人起始状态)映射到状态p(机器人终止状态).最后对该款机器人的轨迹规划进行了Matlab仿真和样机实验, 结果验证了该控制策略及轨迹规划方法有效性.
关键词：巡检机器人越障分层规划有限状态机轨迹规划
Control Strategy and Trajectory Planning of Serial Multi-arm Inspection Robots
FANG Li-jin¹, HE Chang-lin², ZHU Shuai², TAO Guang-hong²
1. Faculty of Robot Science & Engineering, Northeastern University, Shenyang 110819, China;
2. School of Mechanical Engineering & Automation, Northeastern University, Shenyang 110819, China
Corresponding author: FANG Li-jin, E-mail: ljfang@mail.neu.edu.cn
Abstract: With a three-arm inspection robot taken as an example, a hierarchical planning control strategy was analyzed and divided into a line information library layer, an action planning layer and an execution planning layer. The line information library layer is based on the known information of the high-voltage transmission line to be detected, including information on the type, location, and quantity of obstacles. The overall action planning layer and the execution planning layer are summarized according to the finite-state machine theory that when the input event is a (the second layer of the action planning layer), under the action of f (the third layer of the execution layer), the state q (the robot's initial state) is mapped to the state p (the robot's termination state). In the end, the trajectory planning of this robot was simulated by Matlab and the experiment of the prototype laboratory spanning the anti-vibration hammer and overhanging metal fittings, whose results verified the effectiveness and feasibility of the control strategy and the trajectory planning method.
Key words: inspection robotobstacle-crossinghierarchical planningfinite-state machinetrajectory planning
近年来, 国内外学者对电网巡检机器人进行了大量研究.2006年, 加拿大魁北克水电研究中心研制出了一款LineScout机器人^[1-2], 通过模式操作策略配合关节上的传感器传回的信息对机器人进行控制策略规划.2008年, Debenest等^[3-4]研制出了一台远程高压线缆巡检机器人Expliner, 通过控制机器人下方操作器的伸缩来调节机器人的重心完成对障碍物的跨越.中国科学院沈阳自动化研究所设计了一种适用于多种线路障碍的巡检机器人^[5-8], 它既可远程遥控越障也可以自主进行越障, 并提出了一种基于有限状态机理论的越障流程规划方法.冯祖仁等^[9-11]设计了一种新型的三臂巡检机器人, 它利用平行四边形结构来实现对三个轮臂的位置姿态控制, 利用机器人下方的吊舱前后移动来保持机器人重心平衡.任志斌等^[12]对巡检机器人的控制策略进行分层规划, 结合每一层的行为知识库完成机器人自主越障.综合来看目前在巡检机器人领域的控制策略各有优缺点, 都要针对自身研究的机器人来制定.
本文以一款三臂式巡检机器人为例, 对其进行分层规划分析, 结合有限状态机理论和机器人轨迹规划将机器人的各个动作细化为简单的步骤.最后对实验室的机器人样机进行实验来验证本文的控制策略和轨迹规划方法的有效性.
1 机器人系统构成该机器人具有一定的侧向重力平衡能力和转向越障能力, 越障时至少有两只手臂抓线.另外还有对应的五臂式巡检机器人^[13], 与此款三臂式巡检机器人相比具有更好的重力平衡能力和适应更多障碍种类, 但其所需电机更多, 质量更大, 在不同需求情况下有不同应用, 本文方法对五臂式巡检机器人同样适用.在结构上, 机器人由带有行走轮的前中后三臂和四个平行四边形单元组成, 如图 1所示.各个平行四边形单元是由各组的伺服电机带动滚珠丝杠, 通过柔索改变平行四边形机构对角线长度来实现各个臂的抬起与落下.回转结构是通过舵机齿轮传动来控制回转角度.
图 1(Fig. 1)

图 1 三臂式巡检机器人结构简图Fig.1 Structure diagram of the three-arm Inspection robot

机器人控制系统由STM32开发板与伺服驱动器通过CAN总线相连进行通讯, CAN总线通过CAN收发器接口芯片82C250的两个输出端CANH和CANL与物理总线相连, 伺服电机驱动器再通过与伺服电机上的编码器连接来控制直流电机运转.而舵机控制信号由接收机的通道进入信号调制芯片, 获得直流偏置电压, 通过STM32给舵机的信号线传递PWM波来控制舵机的旋转角度.
2 机器人越障模式分层规划2.1 机器人分层控制规划将机器人动作分层规划有利于对机器人的动作规划进行分析和控制.根据机器人的作业环境和工作特点, 可以将机器人的控制规划分为三层:线路信息库层、动作规划层和执行规划层.
线路信息库层是根据需要机器人检测的高压输电线上已知信息所得, 其中包括障碍物的种类、位置、数量等信息; 动作规划层是当机器人根据第一层线路信息库层所得到的信息知道遇到的障碍物种类和数量时进行机器人运动的进一步决策, 表 1列出了当机器人遇到防振锤与悬垂金具时的动作规划层的工作内容; 执行规划层将上一层动作规划层的动作进一步细化到各个关节对应电机的运动, 进一步对机器人进行轨迹规划完成各个电机的工作状态.
表 1(Table 1)

表 1 防振锤与悬垂金具动作规划Table 1 Anti-vibration hammer and overhanging metal action planning 障碍种类 动作规划流程 防振锤 停止前进 前臂抬起 整机前进 停止前进 前臂落线 整机前进 停止前进 中臂抬起 整机前进 停止前进 中臂落线 整机前进 停止前进 后臂抬起 整机前进 停止前进 后臂落线 悬垂金具 停止前进 前臂抬起 前臂侧移 整机前进 停止前进 前臂偏回 前臂落线 整机前进 停止前进 中臂抬起 中臂侧移 整机前进 停止前进 中臂偏回 中臂落线 整机前进 停止前进 后臂抬起 后臂侧移 整机前进 停止前进 后臂偏回 后臂落线

表 1 防振锤与悬垂金具动作规划 Table 1 Anti-vibration hammer and overhanging metal action planning

2.2 机器人越障流程分析机器人在动作规划层中的动作状态为已知且有限, 故可以用有限状态机理论对机器人越障流程进行分析和规划, 这可以方便地调整机器人的动作序列并且能形象地描述机器人动作行为设计.有限状态机的基本表示方法为

(1)

其中:S是一个有限集, 其中的每一个元素称为一个状态, s_i∈S为某一个状态; Σ是一个有穷字母表, 它的每一个元素称为一个输入字符; f是转换函数，定义为f:S×Σ→S, 表示某状态接受某个输入字符所到达的状态, 例如:f(p, a)=q; p, q∈S; a∈Σ; 它表示当输入事件为a时, 在f的作用下, 将状态q映射到状态p; s₀是S中的元素, 是唯一初始状态; Z?S, 且Z≠?, Z是S的一个子集, 是一个终态集.
当机器人收缩状态遇到防振锤等上侧可通过障碍时机器人状态集合为S={s₀, s₁, s₂, s₃, s₄, s₅, s₆, s₇}, Z={s₀}, 其中s₀为图 2a所示的机器人原始状态, s₄为机器人前进状态, s₁为图 2b所示的机器人前臂抬起时静止状态, s₅为机器人前臂抬起时前进状态, s₂为图 2c所示的机器人中臂抬起时静止状态, s₆为机器人中臂抬起时前进状态, s₃为图 2d所示的机器人后臂抬起时静止状态, s₇为机器人后臂抬起时前进状态.
图 2(Fig. 2)

图 2 跨越上侧可通过障碍时的机器人状态集合Fig.2 Robot's state set of spanning obstacles from upside (a)—原始状态；(b)—前臂抬起；(c)—中臂抬起；(d)—后臂抬起.

根据前文叙述的机器人已知状态, 在高压线上以s₀为机器人的初始状态, 当遇到输入事件a(启动)时，机器人开始启动在线路上进行巡检进入s₄; 根据控制规划第一层的线路信息库层已知遇到高压线上的防振锤, 系统输入事件b(停止前进)时，机器人重新回到s₀; 接着系统输入事件c(前臂抬起)时, 机器人会进入s₁; 而机器人跨越防振锤的整体动作流程根据动作规划层中的步骤对应有限状态机中的事件集Σ，建立对应的有限状态机模型见图 3.
图 3(Fig. 3)

图 3 机器人跨越防振锤时的有限状态机模型Fig.3 Finite state machine model for the robot crossing over anti-vibration hammers

当机器人收缩状态遇到悬垂金具等上侧不可通过障碍，机器人状态集合为S={s₀, s₁, s₂, s₃, s₄, s₈, s₉, s₁₀, s₁₁, s₁₂, s₁₃}, Z={s₀}, 其中s₈为图 4a所示的机器人前臂侧移时静止状态, s₁₁为机器人前臂侧移时前进状态, s₉为图 4b所示的机器人中臂侧移时静止状态, s₁₂为机器人中臂侧移时前进状态, s₁₀为图 4c所示的机器人后臂侧移时静止状态, s₁₃为机器人后臂侧移时前进状态.
图 4(Fig. 4)

图 4 跨越上侧不可通过障碍时的机器人状态集合Fig.4 Robot's state set of spanning obstacles impassable from upside (a)—前臂侧移；(b)—中臂侧移；(c)—后臂侧移.

机器人跨越悬垂金具的整体动作流程根据动作规划层中的步骤对应有限状态机中的事件集Σ，建立对应的有限状机模型见图 5.
图 5(Fig. 5)

图 5 机器人跨越悬垂金具时的有限状态机模型Fig.5 Finite state machine model for the robot crossing over overhanging metal action planning

3 机器人轨迹规划机器人在执行规划层中要对动作规划层所要完成的各个动作作进一步细化, 所以需要对机器人进行轨迹规划完成有限状态机中所对应的转换函数f来保证机器人动作实现.
3.1 机器人运动学D-H模型建立对三臂式巡检机器人以前臂为基准建立D-H模型, 如图 6所示.其中各个几何参数已在图 1中标出, 机器人手臂长度为d, 手臂到平行四边形结构距离为a, 平行四边形长度为l, 平行四边形单元到回转单元距离为b.
图 6(Fig. 6)

图 6 三臂式巡检机器人D-H模型Fig.6 D-H model of the three-arm inspection robot

3.2 机器人运动学逆解该机器人为了方便跨越障碍设计为冗余机器人, 这样就会对机器人的运动逆解造成许多不便.首先在对机器人进行运动学逆解之前对14个关节角关系进行整合.其中θ₂与θ₃, θ₅与θ₆, θ₉与θ₁₀, θ₁₂与θ₁₃由于平行四边形结构的缘故互为相反数.本文列出机器人前臂抬起的逆解方法, 其余机器人动作逆解解法类似不赘述.
首先在前臂抬起的条件下对关节角进行整合, 由图 1可以看出当机器人在s₀到s₁过程中只有前臂抬起时满足θ₂=θ₅=0, 同时也可添加约束θ₉=θ₁₂, 2θ₁=-θ₄, 2θ₈=-θ₁₁，而不影响机器人正常运动同时方便后面机器人重力平衡方程建立与机器人运动学逆解运算, 另外要确保机器人行走轮的姿态则要满足θ₇=θ₁, θ₁₄=θ₈.这样给定的关节角就整合到了3个, 只需要三组约束方程即可求到唯一合理解.以机器人后臂为基准建立直角坐标系, 通过对关节角的约束可知机器人前臂y坐标为0, 而机器人前臂的另外两坐标方程可以通过D-H模型进行化简得到：

(2)

另外，本机器人结构上具有一定的侧向重力平衡调节能力^[14], 当机器人在s₀到s₁过程中部件1, 4, 7都应保持在重心平面中不影响侧向重力平衡, 只需对部件2, 3, 5, 6的侧向重力平衡进行分析运算.各个部件相对于重心平面的重力矩为

(3)

其中:i为图 1a中部件1, 2, 3, 5, 6, 7分别对应的序号; M_i为其对应部件相对于重心所在平面的重力矩, 其中M₂=M₃, M₅=M₆; m_i为其对应部件的质量, 其中已知m₂=m₃=m₅=m₆=2.35 kg, 另外已知m₁=m₄=m₇=0.97kg; g为重力加速度; l_i为其对应部件重心到达机器人整体重心所在平面的距离, 其中l₂=l₃, l₅=l₆, 而l₂与l₅利用D-H模型进行化简得

(4)

重力平衡条件为

(5)

根据机器人的结构尺寸, 跨越的防振锤的结构尺寸和规定s₀时各个关节角度, 以机器人后臂为基准建立直角坐标系，前臂抬起时前臂行走轮中心所需经过的三点坐标, 取后两点分别代入式(2), 再联立式(3)~式(5)分别解出在机器人前臂抬起中与结束时的各个关节角度.
3.3 机器人运动轨迹三次多项式插值求得各个关节对应角度值后, 对各组关节角度值进行曲线拟合使其通过各个路径点，为了使机器人的运动能够平稳，应满足各个关节位移、速度、加速度在机器人运动过程中连续平稳变化.三次样条曲线满足这一系列要求, 本文在不影响跨越障碍的前提下取三次样条中三点的情况进行三次曲线插值.
假设各组关节角值的起始角为θ_{(j, 0)}, 中间点为θ_{(j, v)}, 目标点为θ_{(j, g)}.第一个三次曲线设为

(6)

第二个三次曲线设为

(7)

每个三次曲线的起始时刻t=0, 终止时刻t=t_fi, 其中i=1或2.为保证上述条件, 添加三次样条所需约束.
1) 确定三点的角度值:

(8)

2) 保证两段三次曲线中间点角度值、角速度值、角加速度值相等.

(9)

3) 确定整段轨迹的初末位置速度为0.

(10)

同时使t_f1=t_f2=1s满足机器人的各个关节角的最大速度、最大加速度以及对动作的时间要求等综合因素, 通过上述方程组可解得各个系数, 求解出机器人各个关节的运动函数θ_j(t), 再对其求导和二次导即可求得机器人各个关节的角速度和角加速度函数.
4 机器人的仿真与实验4.1 机器人轨迹规划与运动过程仿真根据机器人运动轨迹三次多项式插值方法，应用Matlab绘制出机器人两个关键动作中各个关节角的角度、角速度、角加速度曲线(图 7).
图 7(Fig. 7)

图 7 机器人轨迹规划曲线Fig.7 Robot's trajectory planning curve (a)—前臂抬起；(b)—前臂侧移.

图 7a为机器人前臂抬起动作轨迹规划曲线, 其中θ₁, θ₈, θ₉为该动作控制的关键关节角.图 7b为机器人前臂侧移动作轨迹规划曲线, 其中θ₈, θ₁₁为该动作控制的关键关节角.
如图 8所示，根据轨迹规划中得出了一系列关节角度、角速度、角加速度的变化.利用Matlab对运动规化层的每一步动作进行仿真, 分别得到机器人各个关键动作的运动过程.
图 8(Fig. 8)

图 8 机器人运动过程仿真Fig.8 Robot's motion process simulation (a)—机器人前臂抬起；(b)—机器人中臂抬起；(c)—机器人后臂抬起；(d)—机器人前臂侧移；(e)—机器人中臂侧移；(f)—机器人后臂侧移.

4.2 机器人样机实验在实验室对三臂式巡检机器人样机进行了实验, 验证其控制策略及轨迹规划方法的有效性.三臂式巡检机器人进行了跨越FD-2型防振锤与XUG-2型悬垂金具实验.机器人通过线路信息库层得知需要跨越的障碍物的种类与位置, 当到达指定位置, 开始根据动作规划层的动作顺序信息调取对应执行规划层对每个关节进行控制(图 9).
图 9(Fig. 9)

图 9 机器人样机越障关键状态Fig.9 Key state of the robot prototype for overcoming obstacles (a)—状态s1；(b)—状态s2；(c)—状态s3；(d)—状态s8；(e)—状态s9；(f)—状态s10.

在机器人距防振锤100 mm时，机器人停止前进将前臂抬起如图 9a后继续前进470 mm.前臂跨越障碍后落线继续前进210 mm时, 停止前进将中臂抬起如图 9b后继续前进200 mm.中臂跨越障碍后落线继续前进110 mm时, 停止前进将后臂抬起如图10c后继续前进470 mm.后臂跨越障碍后落线完成样机跨越防振锤实验.同理, 跨越悬垂金具机器人样机关键状态如图 9d~图 9f所示.实验结果表明该控制策略与轨迹规划方法可以满足机器人跨越线路障碍的作业任务.
5 结论1) 以一款三臂式巡检机器人为例, 本文提出一种分层规划结合有限状态机理论对各个控制规划层进行整合的控制策略方法, 可以方便地调整机器人的动作序列并形象地描述机器人动作行为设计的过程.当使用五臂或更多手臂机器人或跨越更多种类障碍时, 只需在原有分析基础上进行添加或调动对应动作即可.
2) 轨迹规划后机器人各个关节角度、角速度曲线光滑连续, 且角加速度曲线连续.进行了机器人样机跨越防振锤和悬垂金具实验, 验证了该控制策略及轨迹规划方法有效可行.
参考文献

[1]	Ramirez D, Ma Y B, Bedir S, et al. Introduction of a LIDAR-based obstacle detection system on the LineScout power line robot[J].Journal of Endourology, 2014, 28(3): 330–334.DOI:10.1089/end.2013.0542
[2]	Pouliot N, Richard P L, Montambault S. LineScout technology opens the way to robotic inspection and maintenance of high-voltage power lines[J].IEEE Power & Energy Technology Systems Journal, 2015, 2(1): 1–11.
[3]	Debenest P, Guarnieri M.Expliner-from prototype towards a practical robot for inspection of high-voltage lines[C]// International Conference on Applied Robotics for the Power Industry.New York: IEEE, 2010: 1-6.https://www.researchgate.net/publication/224190298_Expliner_-_From_prototype_towards_a_practical_robot_for_inspection_of_high-voltage_lines
[4]	Debenest P, Guarnieri M, Takita K, et al.Expliner-toward a practical robot for inspection of high-voltage lines[C]// Field and Service Robotics, Results of the International Conference.Cambridge, 2010: 45-55.https://link.springer.com/chapter/10.1007/978-3-642-13408-1_5
[5]	Li T, Fang L, Wang H.Development of an inspection robot control system for 500kV extra-high voltage power transmission lines[C]// Sice 2004 Conference.New York: IEEE, 2004: 1819-1824.https://www.researchgate.net/publication/303564297_Development_of_an_inspection_robot_control_system_for_500KV_extra-high_voltage_power_transmission_lines
[6]	Zhu X, Zhou J, Wang H, et al.An autonomous obstacles negotiating inspection robot for extra-high voltage power transmission lines[C]// International Conference on Control, Automation, Robotics and Vision.New York: IEEE, 2007: 1-6.https://www.researchgate.net/publication/221145062_An_Autonomous_Obstacles_Negotiating_Inspection_Robot_for_Extra-High_Voltage_Power_Transmission_Lines
[7]	王鲁单, 王洪光, 房立金, 等. 一种输电线路巡检机器人控制系统的设计与实现[J].机器人, 2007, 29(1): 7–11. ( Wang Lu-dan, Wang Hong-guang, Fang Li-jin, et al. Design and implementation of the control system of a power transmission line inspection robot[J].Robot, 2007, 29(1): 7–11.DOI:10.3321/j.issn:1002-0446.2007.01.002)
[8]	郭伟斌, 王洪光, 姜勇, 等. 一种输电线路巡检机器人越障规划方法[J].机器人, 2012, 34(4): 505–512. ( Guo Wei-bin, Wang Hong-guang, Jiang Yong, et al. Obstacle navigation planning for a power transmission line inspection robot[J].Robot, 2012, 34(4): 505–512.)
[9]	杨德伟, 冯祖仁, 张翔. 新型三臂巡线机器人机构设计及运动分析[J].西安交通大学学报, 2012, 46(9): 43–48. ( Yang De-wei, Feng Zu-ren, Zhang Xiang. Mechanism design and kinematic analysis of a novel tribrachiation robot for transmission line inspection[J].Academic Journal of Xi'an Jiaotong University, 2012, 46(9): 43–48.)
[10]	Yang D, Feng Z, Ren X, et al. A novel power line inspection robot with dual-parallelogram architecture and its vibration suppression control[J].Advanced Robotics, 2014, 28(12): 807–819.DOI:10.1080/01691864.2014.884936
[11]	Liang Q, Wang Y, Zhang D, et al. Multifunctional robotic system for live power transmission lines[J].International Journal of Robotics & Automation, 2014, 29(2): 175–183.
[12]	任志斌, 阮毅. 基于知识库的输电线路巡检机器人的越障控制[J].计算机工程与应用, 2008, 44(3): 236–239. ( Ren Zhi-bin, Ruan Yi. Obstacle-navigation control of inspection robot for power transmission lines based on knowledge base[J].Computer Engineering and Application, 2008, 44(3): 236–239.DOI:10.3778/j.issn.1002-8331.2008.03.074)
[13]	Tao G, Fang L, Lin X.Optimization design of the multi-unit serial inspection robot for power transmission line[C]// International Conference on Applied Robotics for the Power Industry.New York: IEEE, 2016: 1-6.https://www.researchgate.net/publication/310498036_Optimization_design_of_the_multi-unit_serial_inspection_robot_for_power_transmission_line
[14]	陶广宏, 房立金. 三臂式巡检机器人重力平衡及其转向越障方法[J].中国机械工程, 2016, 27(9): 1150–1157. ( Tao Guang-hong, Fang Li-jin. Gravity balance and steering and obstacle-crossing of three-arm inspection robots[J].Chinese Journal of Mechanical Engineering, 2016, 27(9): 1150–1157.DOI:10.3969/j.issn.1004-132X.2016.09.003)