程红太, 万登科, 郝丽娜
东北大学 机械工程与自动化学院，辽宁 沈阳 110819
收稿日期：2015-09-16
基金项目：国家自然科学基金资助项目 (61503067)；辽宁省自然科学基金资助项目 (20141016)；中央高校基本科研业务费专项资金资助项目 (N150308001)。
作者简介：程红太 (1985-), 男, 河南漯河人, 东北大学副教授, 博士;
郝丽娜 (1968-), 女, 辽宁庄河人, 东北大学教授，博士生导师。

摘要：运动模式耦合和欠驱动动力学特性是实现灵长类仿生机器人悬臂飞跃的难点.针对运动模式耦合问题，本文通过建立灵长类仿生机器人分段运动模型, 在运动学分析的基础上，结合目标约束条件和切换条件，提出了灵活完整的、适应不同飞跃距离的飞跃轨迹规划方法，以此获得系统飞跃的起始和终止姿态.针对欠驱动问题，采用基于虚约束的轨迹规划和跟踪控制方法来保证系统可以准确达到飞跃起始姿态.最后，搭建了悬臂飞跃仿真模型，仿真结果验证了轨迹规划和控制策略的有效性.
关键词：灵长类仿生机器人轨迹规划悬臂飞跃虚约束
Ricochetal Brachiation Motion Planning and Control for Two-Link Bio-primate Robot
CHENG Hong-tai, WAN Deng-ke, HAO Li-na
School of Mechanical Engineering & Automation, Northeastern University, Shenyang 110819, China
Corresponding author: CHENG Hong-tai, E-mail: chenght@me.neu.edu.cn
Abstract: The tight coupling of locomotion and underactuated characteristics are the main difficulties for realizing ricochetal brachiation of a bio-primate robot. For the locomotion coupling, the segmented dynamic/kinematic models were built and analyzed. Then, a flexible, comprehensive and adaptive ricochetal brachiation motion planning algorithm was proposed in consideration of the constraint condition and switching condition. Therefore, pre-and post-flying postures were obtained. As to the underactuated characteristics, virtual-constraint-based trajectory planning and tracking control methods were adopted to ensure that the system could arrive at the pre-flying posture accurately. Finally, a ricochetal brachiation simulation model was built and the results show the effectiveness of the proposed trajectory planning and control strategy.
Key Words: two-link bio-primate robottrajectory planningricochetal brachiationvirtual constraint
灵长类动物实际的悬臂运动形态大体上可归为两类：悬臂摆动 (荡枝) 和悬臂飞跃.悬臂飞跃是灵长类动物的基本运动模式之一.
为了模拟悬臂运动，学者们研制了各式灵长类仿生机器人.Fukuda课题组最早研制出双臂式悬臂运动仿生机器人 (brachiation mobile robot)^[1].该机器人是一个包含单驱动关节、两自由度的欠驱动机械系统. Fukuda课题组进一步研制了13自由度灵长类悬臂运动仿生机器人^[2]和拥有19根连杆、20个驱动器的Gorilla Robot Ⅲ灵长类仿生机器人^[3].Nishimura等提出了一种具有误差学习算法的最终状态控制策略实现对三连杆悬臂机器人的控制^[4]，并进一步研究了双臂式灵长类仿生机器人，研发出一种控制算法, 使其在仿真过程中能够完成“跳跃动作”(不连续的飞跃运动)^[5-7].Kajima等提出在抓握前对摆动臂进行曲肘动作来减弱碰撞力^[8].
吴伟国及其课题组制作成类人猿机器人物理样机^[9].张晓华课题组研制出两杆式灵长类仿生机器人样机，进而引出了动态伺服控制的概念^[10].赵旖旎等采用基于能量方法结合Lyapunov稳定理论设计的控制器，成功实现了悬臂运动轨迹规划与控制^[11]，并将起摆控制与飞跃轨迹结合，应用滑模控制设计了“跳跃”控制器^[12].
综上所述，尽管国内外学者已经研发出实物样机，并进行了相当数量的实验研究，却鲜见对“悬臂飞跃”这一灵长类动物特有的运动形态进行深入研究与模拟.现有研究成果集中于平面点到点的悬臂摆动控制问题，未针对更一般的姿态控制，即如何调节机器人位置、速度，对悬臂摆动和飞跃运动进行协调控制，使其满足飞跃的初始条件等问题进行深入研究.
本文提出一种新的飞跃轨迹规划方案，比Nakanishi等提出的飞跃模型更加灵活，可以根据需要添加条件，使飞跃姿态更加合理高效；采用基于虚约束的轨迹规划与跟踪控制方法^[13]能够使机器人准确达到起飞所需的姿态，对起飞控制非常有利；利用统一规划出的运动边界条件，设计相应的切换控制策略，将起飞、飞跃和降落三个阶段有机地结合起来，形成完整的悬臂飞跃控制.
1 双臂式灵长类仿生机器人建模在研究长臂猿悬臂运动机理时，Bertram等提出了用单质点摆模型来描述悬臂运动^[14].如图 1所示，悬臂飞跃运动可以分为三个过程：悬臂摆动、自由飞行、悬臂降落.
图 1(Fig. 1)

图 1 单质点摆飞跃运动模型[14]Fig.1 Flight process of a point-mass model

单质点摆模型能够简明地展示长臂猿悬臂飞跃运动机理, 但缺乏对长臂猿身体细节的刻画, 无法应用于机器人运动仿生与控制器设计.本文采用更切实际的双臂式灵长类仿生机器人结构如图 2所示.
图 2(Fig. 2)

图 2 双臂式灵长类仿生机器人模型Fig.2 Model of the two-link brachiation robot

1.1 悬臂摆动动力学建模在悬臂摆动和降落阶段，机器人一端手爪与支撑物接触，是一个二自由度欠驱动机械系统.m₁, m₂为悬臂及摆臂质量；l₁, l₂为悬臂及摆臂长度；l_c1, l_c2为悬臂及摆臂质心距；θ₁为悬臂与竖直方向的夹角；θ₂为摆臂与悬臂延长线的夹角；τ为两臂间输入力矩；I₁, I₂为悬臂及摆臂绕质心转动惯量；ρ_c1, ρ_c2分别为从系统质心到臂1和臂2质心的空间位置矢量.
利用Lagrange方程建立系统动力学模型：

(1)

其中：
1.2 自由飞行过程动力学建模当双臂系统处于飞行状态时, 两个手爪均脱离外界支撑物，系统处于自由飞行状态，仅在重力作用下沿抛物线轨迹运动，如图 3所示.
图 3(Fig. 3)

图 3 双臂式灵长类仿生机器人飞跃过程Fig.3 Flight process of two-link brachiating robot

其中θ_c0为起飞前瞬时质心与杆1末端连线与竖直方向的夹角，l_c0为质心到杆1末端的距离，β为起飞前瞬时质心飞跃速度v与水平方向的夹角，d为飞跃距离，R为起飞瞬时到降落瞬时质心水平方向的位移.
由于所受的唯一外力为系统的重力，符合角动量守恒定律条件，因此有

(2)

其中：
由式 (2) 可知，在自由飞行阶段，机器人双臂夹角的角速度与θ₂和有如下关系：

(3)

因此，可以利用力矩τ来调节θ₂和，实现对自由飞行过程中系统姿态的调控.
由于悬臂飞跃运动包含多个不同运动阶段，在已知飞跃距离d时，为了实现控制，需要计算出各运动阶段的边界条件、控制目标，因此需要对其进行轨迹规划.
2 悬臂飞跃轨迹规划灵长类仿生机器人飞跃过程如图 3所示，飞跃前瞬时系统满足：设初始位置为：.
飞跃距离为d，当飞跃结束时，双臂式仿生机器人抓握点坐标为 (d，0).降落瞬时系统满足以下条件：.考虑到飞跃轨迹的对称性，质心与杆2末端连线与竖直方向上的夹角为-θ_c0，杆2与竖直方向上的夹角为θ′₁.
2.1 飞跃轨迹规划与边界条件确定整个飞跃过程分为起飞、自由飞行、降落三个阶段，其中包含两个切换过程.
2.1.1 飞跃前瞬时所需姿态参数分析双臂式灵长类仿生机器人在飞跃前瞬时质心坐标满足以下条件：

(4a)

(4b)

质心飞跃速度在水平和竖直方向分速度等于质心坐标的导数：

(5a)

(5b)

如图 3所示，假设飞跃轨迹是对称的，

(6)

飞跃前瞬时质心的飞跃速度v满足：

(7)

自由飞行时间满足：

(8)

2.1.2 飞跃结束抓握瞬间姿态参数分析飞跃结束姿态分析与起飞姿态分析方法相似，以飞跃结束抓握点为坐标原点.飞跃结束，抓握到目标位置时质心坐标满足：

(9)

考虑到飞跃过程的对称性，即满足以下条件：

(10)

将式 (9) 左右求导并联立方程 (10) 可得

(11)

杆1与竖直方向上的夹角为θ_1f：

(12)

2.1.3 自由飞行过程质心轨迹与姿态分析在自由飞行阶段，系统只受到重力的作用，使θ₂以固定变化率 () 变化，则有.在自由飞行过程中满足角动量守恒定律，根据式 (3) 可得

(13)

θ₂的变化率满足：

(14)

已知飞跃两抓握点的距离d和质心到坐标原点的距离l_c0，即

(15)

联立式 (4)~式 (9) 和式 (11)~式 (15)，有19个方程，20个未知数，添加一个条件：

(16)

则有20个方程，20个未知数，即已知飞跃两抓握点的距离d，起飞瞬时质心到坐标原点的距离l_c0和起飞瞬时质心速度与水平面的夹角β，即可求出起飞前变量v, 的瞬时值，降落时变量的瞬时值，以及变量的值；而且β可以灵活地调节，为能量的主动调节创造了条件.
2.2 起飞悬臂运动轨迹规划求出飞跃所需的初始姿态条件后，仍需要解决如何达到飞跃姿态的问题.这里采用基于虚约束的方法来规划起飞悬臂运动轨迹.
2.2.1 虚约束选择与计算对于灵长类仿生机器人，要想飞跃到指定目标位置，需要达到的起飞姿态参数为.虚约束的形式采用线性虚约束:φ(θ₂)=aθ₂+b，来规划起飞轨迹.
2.2.2 虚约束分析将虚约束θ₁=φ(θ₂) 及其一阶和二阶导数方程代入动力学方程 (1) 中，得到系统零动态方程：

(17)

其中:
虚约束作用下的零动态方程为一元二阶微分方程的形式, 引入中间变量，可得

(18)

其中.考虑到I>0, 于是有

(19)

式 (19) 即为目标轨迹方程.
3 悬臂飞跃控制本文采用文献[10]所提出的基于Lyapunov的轨迹跟踪控制策略.
3.1 基于虚约束的起飞轨迹控制为保证系统准确收敛到目标轨道，需要对以下两个函数进行镇定，它们分别是虚约束函数和轨道函数:

(20)

(21)

根据文献[10]，上述两个输出函数可以采用下面的控制器实现镇定控制.其中控制力矩为

(22)

其中虚拟输入χ为

(23)

控制器参数需满足如下条件：
其中：
合理地选择系数K₁, K₂, K₃即可实现虚约束函数 (20) 和轨道函数 (21) 的同时镇定.
3.2 飞跃姿态控制在进行飞跃姿态控制时，使θ₂以固定变化率进行调节即可保证质心到达目标位置时两杆也达到目标姿态参数.
3.3 切换控制在悬臂飞跃过程中，为将起飞、自由飞行、降落三个阶段衔接在一起，设计了如图 4所示的切换控制器.
图 4(Fig. 4)

图 4 切换控制原理图Fig.4 Principle diagram of the switch control

4 仿真实验与分析机器人的物理参数取为：m₁=m₂=1 kg, l₁=l₂=1 m, l_c1=l_c2=0.5 m, I₁=I₂=1/12 kg/m², g=9.81 m/s²，质心到抓握点的距离l_c0=0.982 m.表 1给出了不同飞跃距离和不同起飞角度的起飞条件值，可以看出对于相同飞跃距离d、不同起飞角度β，所需的能量和起飞速度是不同的，可以通过调节起飞角度来主动调节飞跃所需的能量E.
表 1(Table 1)

表 1 系列起飞条件规划值Table 1 Series planning values of the take-off condition d/m β/rad v/(m·s-1) E/J tf/s 2.4 0.5 4.334 8 11.731 7 0.423 7 0.6 3.856 5 5.970 8 0.443 9 0.7 3.482 4 2.388 7 0.457 4 2.5 0.5 4.414 0 11.648 3 0.431 4 0.6 3.934 8 6.036 9 0.452 9 0.7 3.656 3 2.628 5 0.467 9 2.6 0.5 4.495 9 11.730 9 0.439 4 0.6 4.016 5 6.261 5 0.462 4 0.7 3.647 8 3.025 7 0.479 1 2.7 0.5 4.580 3 11.964 7 0.447 7 0.6 4.101 1 6.628 4 0.472 1 0.7 3.736 4 3.561 3 0.490 7

表 1 系列起飞条件规划值 Table 1 Series planning values of the take-off condition

飞跃距离d=2.5 m，取起飞瞬时质心速度与水平面夹角β值为0.7.飞跃前瞬时所需姿态参数为=[0.564 8, 0.439 7, 3.346 1, 1.166 6].抓握点机器人姿态参数为=[2.799 3，-0.439 7, 2.741 2, 1.166 6].虚约束函数的形式为θ₁=aθ₂+b，求出a=2.868 2，b=-0.696 5.
控制器参数取K₁=20, K₂=400, K₃=1 000，机器人的初始位置选为=[－π/2, －0.3, 0, 0]，从图 5可以看出在基于虚约束的动态伺服控制策略下，机器人从初始位置开始，在t=0.9 s时就可以首次达到目标姿态=[0.564 8, 0.439 7, 3.346 1, 1.166 6], 在t=3.7 s时第二次达到目标姿态, 即在首次达到目标姿态后机器人在控制策略下能够周期地达到目标姿态, 这样非常有利于机器人起飞时刻的选择.
图 5(Fig. 5)

图 5 基于虚约束的动态伺服控制下的姿态响应 (d=2.5 m)Fig.5 Posture response under dynamic servo control based on virtual constraint (d=2.5 m)

在t=0.9 s时起飞，应用Matlab虚拟现实工具箱 (vitual reality) 将仿真得到的状态变量通过三维实体显示出整个飞跃过程，如图 6所示.
图 6(Fig. 6)

图 6 飞跃过程图Fig.6 Ricochetal brachiation snapshots (a)—d=2.5 m; (b)—d=2.7 m.

图 7为飞跃距离d=2.5 m和d=2.7 m时飞跃过程质心轨迹图，仿真实验得到的质心轨迹图和文献[14]单质点摆模型的质心轨迹图是一致的，这也验证了仿真实验的准确性.
图 7(Fig. 7)

图 7 飞跃过程质心轨迹图Fig.7 Mass center trajectory in the process of ricochetal brachiation

飞跃距离d=2.7 m，取起飞瞬时质心速度与水平面夹角β值为0.5.飞跃前瞬时所需姿态参数为θ==[0.367 7, 0.439 7, 4.341 4, 1.335 53].抓握点机器人姿态参数为=[2.996 44, －0.439 7, 2.810 8, 1.204 8].虚约束函数的形式为θ₁=aθ₂+b，求出a=3.250 7, b=-1.061 7.控制器参数取K₁=20, K₂=400, K₃=1 000, 机器人的初始位置选为=[－2, －0.5, 0, 0].图 8所示为飞跃距离d=2.7 m基于虚约束的动态伺服控制下的姿态响应.
图 8(Fig. 8)

图 8 基于虚约束的动态伺服控制下的姿态响应 (d=2.7 m)Fig.8 Posture response under dynamic servo control based on virtual constraint (d=2.7 m)

从仿真实验中可以看出，按照轨迹规划出的姿态参数, 系统能够准确地完成飞跃过程，并且选用的控制策略能够有效地使系统达到目标姿态参数.
5 结语本文针对灵长类仿生机器人悬臂飞跃仿生中存在的运动模式耦合和欠驱动动力学特性等难点问题，提出了系统化的飞跃轨迹规划与控制方法，并结合基于虚约束的欠驱动机械系统轨迹规划与跟踪控制策略，成功实现了灵长类仿生机器人飞跃全过程运动.给出了两臂式灵长类仿生机器人飞跃过程三个阶段姿态参数的分析求解过程.相比原有的轨迹规划方法，本文方法更加灵活，可以人为引入约束条件.为解决不同间隔连续飞跃时能量的主动调节问题创造了条件.仿真实验验证了飞跃轨迹规划方案和控制策略的合理有效性.
未来将在设计的实物平台上开展实物实验，验证提出的飞跃轨迹规划方案与选择的控制策略，并进一步研究连续飞跃的控制问题.
参考文献

[1]	Fukuda T, Saito F, Arai F.A study on the brachiation type of mobile robot (heuristic creation of driving input and control using CMAC)[C]// Proceedings of IEEE International Workshop on Intelligent Robots and Systems.New York:IEEE, 1991:478-483.
[2]	Saito B F, Fukuda T.A first result of the brachiator Ⅲ:a new brachiation robot modeled on a Siamang[C]//Proceedings of the Fifth International Workshop on Artificial Life.Cambridge:MIT Press, 1997:354-361.
[3]	Lu Z G, Aoyama T, Sekiyama K, et al. Motion transfer control from walking to brachiation through vertical ladder climbing for a multi-locomotion robot[J].IEEE/ASME Transactions on Mechatronics, 2014, 19(5): 1681–1693.DOI:10.1109/TMECH.2013.2292327
[4]	Nishimura H, Funaki K. Motion control of three-link brachiation robot by using final-state control with error learning[J].IEEE/ASME Transactions on Mechatronics, 1998, 3(2): 120–128.DOI:10.1109/3516.686680
[5]	Nakanishi J, Fukuda T, Koditschek D E.Experimental implementation of a target dynamics controller on a two-link brachiating robot[C]//IEEE International Conference on Robotics and Automation.New York:IEEE, 1998:787-792.
[6]	Nakanishi J, Fukuda T, Koditschek D E. A brachiating robot controller[J].IEEE Transactions on Robotics and Automation, 2000, 16(2): 109–123.DOI:10.1109/70.843166
[7]	Nakanishi J, Fukuda T.A leaping maneuver for a brachiating robot[C]//IEEE International Conference on Robotics and Automation.New York:IEEE, 2000:2822-2827.
[8]	Kajima H, Hasegawa Y, Fukuda T. Energy-based swing back control for continuous brachiation of a multi-locomotion robot[J].International Journal of Intelligent Systems, 2006, 21(9): 1025–1043.DOI:10.1002/(ISSN)1098-111X
[9]	Wu W G, Wang Y, Liang F, et al.Development of modular combinational gorilla robot system[C]//Proceedings of the 2004 IEEE International Conference on Robotics and Biomimetics.New York:IEEE, 2004:437-440.
[10]	程红太, 张晓华. 一类欠驱动机械系统的虚约束动态伺服控制[J].浙江大学学报 (工学版), 2011, 45(5): 818–824. ( Cheng Hong-tai, Zhang Xiao-hua. Virtual constraints based dynamical servo control for a class of underactuated mechanical systems[J].Journal of Zhejiang University (Engineering Science), 2011, 45(5): 818–824.)
[11]	赵旖旎, 程红太, 张晓华. 基于动态伺服的欠驱动双摆机器人仿生悬摆控制[J].机器人, 2009, 31(4): 289–295. ( Zhao Yi-ni, Cheng Hong-tai, Zhang Xiao-hua. Life-like brachiation control of underactuated two-link robot based on dynamical servo[J].Robot, 2009, 31(4): 289–295.)
[12]	张晓华, 赵旖旎, 程红太. 灵长类仿生机器人悬臂飞跃运动控制研究[J].信息与控制, 2010, 39(6): 794–800. ( Zhang Xiao-hua, Zhao Yi-ni, Cheng Hong-tai. Ricochetal brachiate control of two-link primate-like robot[J].Information and Control, 2010, 39(6): 794–800.)
[13]	程红太, 赵旖旎, 张晓华. Acrobot动态伺服控制及其对称虚约束方法研究[J].自动化学报, 2010, 36(11): 1594–1600. ( Cheng Hong-tai, Zhao Yi-ni, Zhang Xiao-hua. Dynamical servo control and symmetrical virtual constraints method for the acrobot[J].Acta Automatica Sinica, 2010, 36(11): 1594–1600.DOI:10.3724/SP.J.1004.2010.1594)
[14]	Bertram J E, Ruina A, Cannon C E, et al. A point-mall model of gibbon locomotion[J].The Journal of Experimental Biology, 1999, 202: 2609–2617.