第15卷第6期 智能系统学报 Vol.15 No.6 2020年11月 CAAI Transactions on Intelligent Systems Nov.2020 D0L:10.11992/tis.202005031 连续型机器人运动学仿真和操控系统设计 牛国臣，张云霄 (中国民航大学机器人研究所，天津300300) 摘要：为了适应越来越复杂的非结构化环境，设计了一种基于球铰链连接和柔性支撑杆结合的线驱动连续型 机械臂，并基于常曲率模型的假设建立连续型机器人的运动学模型，研究连续型机器人驱动映射关系，利用 MATLAB进行运动学和驱动映射的仿真，仿真结果表明连续型机器人的空间优越性。搭建三关节连续型机器 人样机平台，基于连续型机器人的特点设计末端关节跟随手柄操作模式，并在样机平台上实验验证，实验结果 表明了运动学模型和驱动映射关系的合理性和正确性以及操控方式的可行性。 关键词：连续型机器人；末端跟随：操控方式：运动学模型：柔性机械臂：空间转换：手柄控制：三关节 中图分类号：TP242文献标志码：A文章编号：1673-4785(2020)06-1058-10 中文引用格式：牛国臣，张云雪.连续型机器人运动学仿真和操控系统设计.智能系统学报，2020,15(6)：1058-1067. 英文引用格式：NIU Guochen,ZHANG Yunxiao..Kinematics simulation and control system design of continuous robot J.CAAI transactions on intelligent systems,2020,15(6):1058-1067. Kinematics simulation and control system design of continuous robot NIU Guochen,ZHANG Yunxiao (Robotics Institute,Civil Aviation University of China,Tianjin 300300,China) Abstract:To enable robotic adaptation to increasingly complex unstructured environments,we designed a wire-driven continuous manipulator that combines a spherical joint and flexible support rod.To study the drive-mapping relation of the continuous robot,we established a kinematics model based on the assumptions of the constant curvature model,and we used MATLAB to simulate the kinematics and drive mapping.The spatial superiority of the continuous robot is demonstrated by the simulation results.We built a prototype platform for the three-joint continuous robot,and designed the handle operation mode for the end joint based on the characteristics of the robot.Experimental verification was per- formed on the prototype platform.Our experimental results verify both the rationality and correctness of the kinematic model and drive mapping relationship and the feasibility of the manipulation method. Keywords:continuous robot;follow terminal;control mode;kinematic model;flexible manipulator,space transforma- tion:handle control:three joints 民航飞机油箱中存在大量的诸如油管、隔 前，很多研究者对连续型机器人在人体手术8列 板、桁条等障碍物，并且有大量易燃易爆气体，是 燃气涡轮发动机维修、核电厂检修等领域的 一个结构复杂、空间狭窄的环境，在其中进行人 应用进行了探索。 工作业难度高、劳动强度大，并且存在安全隐患， 由于连续型机器人各个关节之间具有耦合 因此需要一种能够替代人工检查的装置。相 性，不能将其每一个关节看作一个操作关节，其 比于传统的刚性连杆机器人，连续型机器人具有 操作方法也是一项具有挑战的任务。钱文欢 多冗余自由度、驱动器和控制器分离的特点，理 等41设计了一种仿象鼻混合关节的连续型机器 论上可以达到空间上的任一点“：1，因此更加灵 人，对机器人的机械结构、运动学等进行了分析， 活、安全，更加适用于飞机油箱检查任务m。目 并搭建了两关节的混合关节机器人。该机器人由 收稿日期：2020-05-25 离散关节和连续关节串联而成，其各个关节之间 基金项目：天津科技支撑计划项目(17 ZXHLGX00120):中央高 校基本科研业务费项目(3122019043). 并非为连续的，所以其本质仍然是多关节串联机 通信作者：牛国臣.E-mail:niu_guochen@139.com 械臂。Li等提出了一种新型的连续体机器人
DOI: 10.11992/tis.202005031 连续型机器人运动学仿真和操控系统设计 牛国臣，张云霄 （中国民航大学 机器人研究所，天津 300300） 摘 要：为了适应越来越复杂的非结构化环境，设计了一种基于球铰链连接和柔性支撑杆结合的线驱动连续型 机械臂，并基于常曲率模型的假设建立连续型机器人的运动学模型，研究连续型机器人驱动映射关系，利用 MATLAB 进行运动学和驱动映射的仿真，仿真结果表明连续型机器人的空间优越性。搭建三关节连续型机器 人样机平台，基于连续型机器人的特点设计末端关节跟随手柄操作模式，并在样机平台上实验验证，实验结果 表明了运动学模型和驱动映射关系的合理性和正确性以及操控方式的可行性。 关键词：连续型机器人；末端跟随；操控方式；运动学模型；柔性机械臂；空间转换；手柄控制；三关节 中图分类号：TP242 文献标志码：A 文章编号：1673−4785(2020)06−1058−10 中文引用格式：牛国臣, 张云霄. 连续型机器人运动学仿真和操控系统设计 [J]. 智能系统学报, 2020, 15(6): 1058–1067. 英文引用格式：NIU Guochen, ZHANG Yunxiao. Kinematics simulation and control system design of continuous robot[J]. CAAI transactions on intelligent systems, 2020, 15(6): 1058–1067. Kinematics simulation and control system design of continuous robot NIU Guochen，ZHANG Yunxiao (Robotics Institute, Civil Aviation University of China, Tianjin 300300, China) Abstract: To enable robotic adaptation to increasingly complex unstructured environments, we designed a wire-driven continuous manipulator that combines a spherical joint and flexible support rod. To study the drive-mapping relation of the continuous robot, we established a kinematics model based on the assumptions of the constant curvature model, and we used MATLAB to simulate the kinematics and drive mapping. The spatial superiority of the continuous robot is demonstrated by the simulation results. We built a prototype platform for the three-joint continuous robot, and designed the handle operation mode for the end joint based on the characteristics of the robot. Experimental verification was performed on the prototype platform. Our experimental results verify both the rationality and correctness of the kinematic model and drive mapping relationship and the feasibility of the manipulation method. Keywords: continuous robot; follow terminal; control mode; kinematic model; flexible manipulator; space transformation; handle control; three joints 民航飞机油箱中存在大量的诸如油管、隔 板、桁条等障碍物，并且有大量易燃易爆气体，是 一个结构复杂、空间狭窄的环境，在其中进行人 工作业难度高、劳动强度大，并且存在安全隐患， 因此需要一种能够替代人工检查的装置[1-3]。相 比于传统的刚性连杆机器人，连续型机器人具有 多冗余自由度、驱动器和控制器分离的特点，理 论上可以达到空间上的任一点[4-6] ，因此更加灵 活、安全，更加适用于飞机油箱检查任务[7]。目 前，很多研究者对连续型机器人在人体手术[8-9] 、 燃气涡轮发动机维修[10] 、核电厂检修[11] 等领域的 应用进行了探索。 由于连续型机器人各个关节之间具有耦合 性，不能将其每一个关节看作一个操作关节，其 操作方法也是一项具有挑战的任务[12-13]。钱文欢 等 [14-15] 设计了一种仿象鼻混合关节的连续型机器 人，对机器人的机械结构、运动学等进行了分析， 并搭建了两关节的混合关节机器人。该机器人由 离散关节和连续关节串联而成，其各个关节之间 并非为连续的，所以其本质仍然是多关节串联机 械臂。Li 等 [16] 提出了一种新型的连续体机器人， 收稿日期：2020−05−25. 基金项目：天津科技支撑计划项目 (17ZXHLGX00120)；中央高 校基本科研业务费项目 (3122019043). 通信作者：牛国臣. E-mail：niu_guochen@139.com. 第 15 卷第 6 期 智 能 系 统 学 报 Vol.15 No.6 2020 年 11 月 CAAI Transactions on Intelligent Systems Nov. 2020
第6期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1059· 它由两个模块和一个由螺旋弹簧构成的柔性骨架 支撑杆等组成，机器人以镍钛合金杆作为支撑 构成。每个模块由4个平行排列的肌腱驱动，实 轴，各导向圆盘球铰连接，用以保证关节的灵活 现冗余驱动系统，保证机器人的灵活运动。在此 性和关节的等曲率弯曲，以钢丝绳作为驱动线穿 基础上，设计了连续体机器人的位置反馈控制 过导向圆盘驱动关节运动，保证关节韧性，以直 器，并在控制器中加入二次规划算法，实现了机 线模组组成驱动机构，降低绳长变化误差的影 器人的平滑配置。Dong等叨设计了一款双枢轴 响；并且分析连续型机器人的关节映射模型，设 关节结构的连续型机器人，可最大程度地减少绕 计手柄控制方式，实现样机平台手柄操作控制， 其轴的扭曲，该连续型机器人由导向盘、驱动绳 搭建三关节样机平台，在样机平台进行关节实验。 索以及连接导向盘的双枢轴组成，驱动绳索穿过 导向盘实现关节姿态控制，相邻两个双枢轴之间 1连续型机器人的结构设计和运动 相互交错90°，这样的结构能够得到较好的刚度 学分析 又能保证连续型机器人一定的弯曲能力。曹燕飞 等&设计了变刚度微创介入手术的连续体机器 1.1结构设计 人，该机器人由导向盘、芯柱、绳索、连接盘、硅 课题组模仿蛇脊椎骨的构造设计了基于球铰 胶壁组成，每个关节由3根绳索驱动关节运动，并 链连接的连续型机器人结构，连续型机器人由驱 深入研究了SMA的相变机理，并且设计其控制 动机构和柔性机械臂组成，如图1所示。柔性机 模式为点位控制模式和关节控制模式。赵智远 械臂由多个关节段和柔性支撑杆组成，各个关节 等202四仿照章鱼触手的结构和运动机理，设计了 段穿过中间的柔性支撑杆组成整个柔性机械臂： ~款线驱动的连续型机器人，对连续型机器人的 其中每个关节段由多个导线圆盘连接而成，导线 运动学进行了分析，并在三关节样机平台上进行 圆盘分为底端圆盘、受力圆盘、中间引线圆盘和 了关节弯曲和旋转实验；该机器人由远程驱动机 顶端圆盘，各圆盘都是以中间引线圆盘为基础改 构和机械臂部分组成，驱动部分采用步进电机带 型的，中间导线圆盘含有球头、球壳、导线孔、支 动卷轴进而改变驱动绳长的方式，各个关节之间 撑杆孔以及预留孔，如图2所示，图中D表示圆 采用球铰链连接，增加其灵活性。 盘直径，d表示导线孔所在圆直径，α表示相邻导 目前，课题组针对飞机油箱检查的问题，提出 线孔角度，h表示导线圆盘厚度，I表示相邻圆盘 了使用连续型机器人辅助进行飞机油箱泄漏点检 距离。各个导线圆盘穿过中间的弹性支撑杆，相 查的构想2)，并在连续型机器人运动学、路径规 邻两个导线圆盘的球头和球壳相连，球头球壳相 划2、路径跟随2等方面都进行了研究。本文设 接于同心圆，这种结构既能方便各个导线圆盘的 计了一种仿蛇脊椎骨的线驱动连续型机器人，机 连接又能给柔性机械臂提供轴向支撑力，使关节 器人由直线模组、导向圆盘，钢丝绳、导向滑轮、 弯曲更趋于等曲率圆弧。 关 顶端圆盘 节 柔性机械臂 第 中间引线圆盘 关节受力圆盘 驱动机构 第 关 节 底端圆盘 (a)Solidworks装配图 (b)实物装配图 (C)柔性机械臂装配图 图1三关节连续型机器人 Fig.1 Three-joint continuous robot
它由两个模块和一个由螺旋弹簧构成的柔性骨架 构成。每个模块由 4 个平行排列的肌腱驱动，实 现冗余驱动系统，保证机器人的灵活运动。在此 基础上，设计了连续体机器人的位置反馈控制 器，并在控制器中加入二次规划算法，实现了机 器人的平滑配置。Dong 等 [17] 设计了一款双枢轴 关节结构的连续型机器人，可最大程度地减少绕 其轴的扭曲，该连续型机器人由导向盘、驱动绳 索以及连接导向盘的双枢轴组成，驱动绳索穿过 导向盘实现关节姿态控制，相邻两个双枢轴之间 相互交错 90°，这样的结构能够得到较好的刚度 又能保证连续型机器人一定的弯曲能力。曹燕飞 等 [18-19] 设计了变刚度微创介入手术的连续体机器 人，该机器人由导向盘、芯柱、绳索、连接盘、硅 胶壁组成，每个关节由 3 根绳索驱动关节运动，并 深入研究了 SMA 的相变机理，并且设计其控制 模式为点位控制模式和关节控制模式。赵智远 等 [20-22] 仿照章鱼触手的结构和运动机理，设计了 一款线驱动的连续型机器人，对连续型机器人的 运动学进行了分析，并在三关节样机平台上进行 了关节弯曲和旋转实验；该机器人由远程驱动机 构和机械臂部分组成，驱动部分采用步进电机带 动卷轴进而改变驱动绳长的方式，各个关节之间 采用球铰链连接，增加其灵活性。 目前，课题组针对飞机油箱检查的问题，提出 了使用连续型机器人辅助进行飞机油箱泄漏点检 查的构想[23] ，并在连续型机器人运动学、路径规 划 [24] 、路径跟随[25] 等方面都进行了研究。本文设 计了一种仿蛇脊椎骨的线驱动连续型机器人，机 器人由直线模组、导向圆盘，钢丝绳、导向滑轮、 支撑杆等组成，机器人以镍钛合金杆作为支撑 轴，各导向圆盘球铰连接，用以保证关节的灵活 性和关节的等曲率弯曲，以钢丝绳作为驱动线穿 过导向圆盘驱动关节运动，保证关节韧性，以直 线模组组成驱动机构，降低绳长变化误差的影 响；并且分析连续型机器人的关节映射模型，设 计手柄控制方式，实现样机平台手柄操作控制， 搭建三关节样机平台，在样机平台进行关节实验。 1 连续型机器人的结构设计和运动 学分析 1.1 结构设计 课题组模仿蛇脊椎骨的构造设计了基于球铰 链连接的连续型机器人结构，连续型机器人由驱 动机构和柔性机械臂组成，如图 1 所示。柔性机 械臂由多个关节段和柔性支撑杆组成，各个关节 段穿过中间的柔性支撑杆组成整个柔性机械臂； 其中每个关节段由多个导线圆盘连接而成，导线 圆盘分为底端圆盘、受力圆盘、中间引线圆盘和 顶端圆盘，各圆盘都是以中间引线圆盘为基础改 型的，中间导线圆盘含有球头、球壳、导线孔、支 撑杆孔以及预留孔，如图 2 所示，图中 D 表示圆 盘直径，d 表示导线孔所在圆直径，α 表示相邻导 线孔角度，h 表示导线圆盘厚度，l 表示相邻圆盘 距离。各个导线圆盘穿过中间的弹性支撑杆，相 邻两个导线圆盘的球头和球壳相连，球头球壳相 接于同心圆，这种结构既能方便各个导线圆盘的 连接又能给柔性机械臂提供轴向支撑力，使关节 弯曲更趋于等曲率圆弧。 柔性机械臂 驱动机构 (a) Solidworks 装配图 (b) 实物装配图 底端圆盘 中间引线圆盘 关节受力圆盘 顶端圆盘 第 二 关 节 第 三 关 节 第 一 关 节 (c) 柔性机械臂装配图 图 1 三关节连续型机器人 Fig. 1 Three-joint continuous robot 第 6 期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1059·
·1060· 智能系统学报 第15卷 型机器人的三个变量，单关节长度L,弯曲角度日 导线孔 和旋转角度9，弯曲角度0为圆弧的圆心角，旋转 角度p为平面ooo”和平面x0z的夹角。 预留孔 支撑杆孔 图4单关节运动学模型 Fig.4 Single-joint kinematics model 由几何关系可以知道，连续型机器人单关节 图2导线圆盘结构 首端坐标系到末端坐标系的齐次变换矩阵可用 Fig.2 Structure of wire disk 式(1)表示，它是一个4×4的矩阵。 连续型机器人的驱动机构由直线模组、上下 R 连接板、导线滑轮以及钢丝绳等组成。以直线模 T=P(L.0.p)Rz(p)Rr(0)Rz(-p)= 组作为连续型机器人的驱动器，直线模组将步进 cOc-+s 电机的旋转运动转换为直线模组中滑台的直线运 cosuc-csp sBcw g(1-c0) 动，这样可以有效减少转轴驱动带来的误差，导 L cOcs-csp c2+s"vce seso s(1-c0) 向滑轮改变钢丝绳的受力方向，避免钢丝绳的摩 擦，每个关节由三根钢丝绳驱动，钢丝绳连接直 -s0cip -s0s9 co L 线模组的滑台和连续型机器人柔性机械臂的各个 0 0 0 1 关节末端，以驱动各关节的运动。 (1) 1.2运动学分析 为了方便，记c0=cos0,s0=sin0,s20=sin20 在进行连续型机器人的操作控制之前需建立 c20=cos20,其中0∈(0，，9∈[0,2，P(L,0,p)表示 连续型机器人从操作空间到驱动信号空间的映射 平移矩阵，Rz(p)、Rv(O和Rz(-P)分别表示绕+Z、 关系，其空间转换图如图3所示。这里假设连续 +Y和+Z的坐标转换矩阵，R表示旋转矩阵，它是 型机器人柔性机械臂为等曲率圆弧模型，且各个 一个3×3的矩阵，P表示位置向量，它是一个 关节长度恒定。 3×1的矩阵。 各个关节都为等曲率圆弧模型，则根据坐标 操作空间 关节空间 绳长空间 驱动信号 转换公式可得第n关节段坐标系{m到第1关节 (化，y,) {⊙， ALy 空间{FM 段坐标系1)之间的齐次变换矩阵如式(2)所示： 图3空间转换图 +T=T×T×L×+T (2) Fig.3 Spatial transformation diagram 1.2.2关节空间到绳长空间的转换 图3中操作空间(x,yz)表示连续型机器人操 由连续型机器人的结构分析可知，机器人的 作空间的坐标，关节空间{⊙，表示连续型机器 每个关节的姿态变化依赖于穿过该关节绳长的变 人的姿态参数，即弯曲角度和旋转角度，绳长空 化，为了实现连续型机器人的操作运动，建立了 间△L表示连续型机器人第i关节第j根钢丝绳 从关节空间到绳长空间的映射关系。 的绳长变化量，驱动信号空间{F,M表示步进电 根据关节等曲率模型，可知每个关节的相邻 机的所需的驱动信号的频率和脉冲个数。 导线圆盘之间的夹角相同，为了便于分析建立连 1.2.1运动学建模 续型机器人的关节空间到绳长空间的映射关系， 连续型机器人单关节运动学模型如图4所 对其进行简化（如图5所示），上下两个矩形表示 示。将其简化为等曲率连续光滑曲线，定义连续 导线圆盘截面，圆点表示关节的旋转中心。把用
D 导线孔 预留孔 支撑杆孔 α d/2 l h 球壳 球头 图 2 导线圆盘结构 Fig. 2 Structure of wire disk 连续型机器人的驱动机构由直线模组、上下 连接板、导线滑轮以及钢丝绳等组成。以直线模 组作为连续型机器人的驱动器，直线模组将步进 电机的旋转运动转换为直线模组中滑台的直线运 动，这样可以有效减少转轴驱动带来的误差，导 向滑轮改变钢丝绳的受力方向，避免钢丝绳的摩 擦，每个关节由三根钢丝绳驱动，钢丝绳连接直 线模组的滑台和连续型机器人柔性机械臂的各个 关节末端，以驱动各关节的运动。 1.2 运动学分析 在进行连续型机器人的操作控制之前需建立 连续型机器人从操作空间到驱动信号空间的映射 关系，其空间转换图如图 3 所示。这里假设连续 型机器人柔性机械臂为等曲率圆弧模型，且各个 关节长度恒定。 操作空间 (x, y, z) 关节空间 {Θ, Φ} 绳长空间 ∆Lij 驱动信号 空间{F, M} 图 3 空间转换图 Fig. 3 Spatial transformation diagram (x, y,z) {Θ,Φ} ∆Li j {F, M} 图 3 中操作空间 表示连续型机器人操 作空间的坐标，关节空间 表示连续型机器 人的姿态参数，即弯曲角度和旋转角度，绳长空 间 表示连续型机器人第 i 关节第 j 根钢丝绳 的绳长变化量，驱动信号空间 表示步进电 机的所需的驱动信号的频率和脉冲个数。 1.2.1 运动学建模 连续型机器人单关节运动学模型如图 4 所 示。将其简化为等曲率连续光滑曲线，定义连续 L θ φ θ φ oo′o ′′ xoz 型机器人的三个变量，单关节长度 ，弯曲角度 和旋转角度 ，弯曲角度 为圆弧的圆心角，旋转 角度 为平面 和平面 的夹角。 z o o″ o′ x x′ z′ y′ y φ θ 图 4 单关节运动学模型 Fig. 4 Single-joint kinematics model 由几何关系可以知道，连续型机器人单关节 首端坐标系到末端坐标系的齐次变换矩阵可用 式 (1) 表示，它是一个 4×4 的矩阵。 T = P(L,θ,φ)RZ(φ)RY(θ)RZ(−φ) = [ R P 0 1 ] = cθc 2φ+ s 2φ cθsφcφ−cφsφ sθcφ L θ cφ(1−cθ) cθcφsφ−cφsφ c 2φ+ s 2φcθ sθsφ L θ sφ(1−cθ) −sθcφ −sθsφ cθ L θ sθ 0 0 0 1 (1) cθ = cos θ sθ = sinθ s 2 θ = sin2 θ c 2 θ = cos2 θ θ ∈ (0,π] φ ∈ [0,2π] P(L,θ,φ) RZ(φ) RY(θ) RZ(−φ) R P 为了方便，记 ， ， ， ，其中 ， ， 表示 平移矩阵， 、 和 分别表示绕+Z、 +Y 和+Z 的坐标转换矩阵， 表示旋转矩阵，它是 一个 3×3 的矩阵， 表示位置向量，它是一个 3×1 的矩阵。 n {n} {1} 各个关节都为等曲率圆弧模型，则根据坐标 转换公式可得第 关节段坐标系 到第 1 关节 段坐标系 之间的齐次变换矩阵如式 (2) 所示： 1 n+1T = 1 2T × 2 3T × L× n n+1T (2) 1.2.2 关节空间到绳长空间的转换 由连续型机器人的结构分析可知，机器人的 每个关节的姿态变化依赖于穿过该关节绳长的变 化，为了实现连续型机器人的操作运动，建立了 从关节空间到绳长空间的映射关系。 根据关节等曲率模型，可知每个关节的相邻 导线圆盘之间的夹角相同，为了便于分析建立连 续型机器人的关节空间到绳长空间的映射关系， 对其进行简化（如图 5 所示），上下两个矩形表示 导线圆盘截面，圆点表示关节的旋转中心。把用 ·1060· 智 能 系 统 学 报 第 15 卷
第6期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1061· 来完成驱动功能的钢丝绳分成两部分，在驱动线 行角度转换，建立从(a,By)到(9,0，-p)的转换关系。 孔中的部分，长度为h,处于两个导线圆盘之间的 根据旋转公式可求得旋转顺序为ZYX的旋 部分，长度分别为1和2。假设连续型机器人有 转变换矩阵为 n个关节，则其直立状态时驱动线长度为L=心 a11 a12 a13 (h+lo)。 R(@.B.y)=Rz(@)Rx(B)Rz(Y)= a21 22 a23 a31a32 a33 (6) cBca sBsyca-cysa sBcyca+sysa CBsa sBsysa +cyca sBcysa-syca -sB sycB cycB 由于姿态传感器测量Z轴旋转角度易受环境 磁场等因素的影响，所以这里避免使用角度α，联 合式(6)和式(1)中的旋转变换矩阵R,则可以得 到从姿态传感器角度到关节姿态的转换式(7)、 式(8)： Atan2(va+a,a3s),s0 0= 0 (7) 180 图5关节弯曲引起绳长变化示意图 Fig.5 Schematic of rope length changes caused by joint -Atan2(a32,-a31),s0≠0 bending 9= 0°，0=0° (8) 0°，0=180° 假设导线圆盘导线孔按逆时针进行编号，由 于每个关节由3根钢丝绳驱动，则相邻两孔夹角 1.24绳长空间到驱动信号空间的映射 为。=会在连续型机器人机械臂弯曲时，存在 连续型机器人绳长的变化控制最终将反映到 电机的驱动信号控制上，通过式(5)可以计算出 导线孔无线的情况，在该情况下，线长变化量为 连续型机器人从一个姿态转换到另一个姿态绳长 零，设第i关节段的关节变量为(0，)，由几何分 的变化量，这里选择直线模组驱动连续型机器人 析即可得到第关节，第j个孔的驱动线在该关节 的线长变化，直线模组导程为c,单位mm,设置 范围内的线长为 直线模组驱动器电机每转的脉冲个数为5，根据 L与=nH+cos2 -dsin ncos(+(j-D)a) 6 (3) 式(5)可计算出各个驱动绳长的变化值△1，设直 则线长变化量为 线模组速度为v,单位为mm/s,则可以得到绳长 A,=nk-beo号-dsin号casg,+U-ladj 变化量和步进电机驱动信号频率和脉冲的映射关 2n 系，如式(9)所示： 由于连续型机器人在进行关节运动时，越靠 m= .△ 近连续型机器人基座，穿过该关节的驱动线数量 (9) 越多，所有穿过该关节的驱动线线长都将随之改 f =-.v 变。在进行实际计算时，需要的是整体机械臂一 式中：m表示脉冲数目；f表示在设定速度v下的 个驱动线的变化量，则第导线孔驱动线总线长 频率，单位为Hz。 变化量△为 2操作方法研究 (5) 2.1系统设计 1.2.3ZYZ欧拉角求解 为了验证连续型机器人的运动学，搭建了三 为了测量连续型机器人的姿态，这里采用姿 关节的连续型机器人控制系统，如图6所示，连续 态传感器采集连续型机器人的姿态信息。由连续 型机器人平台由PC、MCU、电源、步进电机驱动 型机器人单关节段建模可知，其坐标旋转变换按 器、姿态传感器、直线模组、连续型机器人机械臂 照Z-Y-Z次序，分别为(，6，-p),而姿态传感器测 等组成。连续型机器人柔性机械臂由3个关节组 量的姿态角的旋转变换按照ZY-X次序，假设分 成，单关节长度220mm,驱动部分由9个直线模 别为（α，B,y),因此无法直接将姿态传感器的输出 组构成。 角度应用到连续型机器人的姿态测量上，需要进 由式(7)和式(8)可知，能够由姿态传感器得
h l1 l2 n L = n· (h+l0) 来完成驱动功能的钢丝绳分成两部分，在驱动线 孔中的部分，长度为 ，处于两个导线圆盘之间的 部分，长度分别为 和 。假设连续型机器人有 个关节，则其直立状态时驱动线长度为 。 x h y z θ l2 l1 l0 /2 d/2 d/2 图 5 关节弯曲引起绳长变化示意图 Fig. 5 Schematic of rope length changes caused by joint bending α = 2π 3n (θi ,φi) 假设导线圆盘导线孔按逆时针进行编号，由 于每个关节由 3 根钢丝绳驱动，则相邻两孔夹角 为 。在连续型机器人机械臂弯曲时，存在 导线孔无线的情况，在该情况下，线长变化量为 零，设第 i 关节段的关节变量为 ，由几何分 析即可得到第 i 关节，第 j 个孔的驱动线在该关节 范围内的线长为 Li j =n [ H+l0 cos θi 2n −d sin θi 2n cos(φi +(j−1)α) ] (3) 则线长变化量为 ∆Li j = n { l0 − [ l0 cos θi 2n −d sin θi 2n cos(φi +(j−1)α) ]} (4) ∆lj 由于连续型机器人在进行关节运动时，越靠 近连续型机器人基座，穿过该关节的驱动线数量 越多，所有穿过该关节的驱动线线长都将随之改 变。在进行实际计算时，需要的是整体机械臂一 个驱动线的变化量，则第 j 导线孔驱动线总线长 变化量 为 ∆Lj = ∑n i=1 ∆Li j (5) 1.2.3 ZYZ 欧拉角求解 (φ, θ,−φ) (α, β, γ) 为了测量连续型机器人的姿态，这里采用姿 态传感器采集连续型机器人的姿态信息。由连续 型机器人单关节段建模可知，其坐标旋转变换按 照 Z-Y-Z 次序，分别为 ，而姿态传感器测 量的姿态角的旋转变换按照 Z-Y-X 次序，假设分 别为 ，因此无法直接将姿态传感器的输出 角度应用到连续型机器人的姿态测量上，需要进 行角度转换，建立从 (α, β, γ) 到 (φ, θ,−φ) 的转换关系。 根据旋转公式可求得旋转顺序为 Z-Y-X 的旋 转变换矩阵为 R(α,β,γ) = RZ(α)RY(β)RZ(γ) = a11 a12 a13 a21 a22 a23 a31 a32 a33 = cβcα sβsγcα−cγsα sβcγcα+ sγsα cβsα sβsγsα+cγcα sβcγsα− sγcα −sβ sγcβ cγcβ (6) α R 由于姿态传感器测量 Z 轴旋转角度易受环境 磁场等因素的影响，所以这里避免使用角度 ，联 合式 (6) 和式 (1) 中的旋转变换矩阵 ，则可以得 到从姿态传感器角度到关节姿态的转换式 (7)、 式 (8)： θ = Atan 2( √ a 2 31 +a 2 32,a33), sθ , 0 0 ◦ 180◦ (7) φ = −Atan 2(a32,−a31), sθ , 0 0 ◦ , θ = 0 ◦ 0 ◦ , θ = 180◦ (8) 1.2.4 绳长空间到驱动信号空间的映射 c s ∆l v 连续型机器人绳长的变化控制最终将反映到 电机的驱动信号控制上，通过式 (5) 可以计算出 连续型机器人从一个姿态转换到另一个姿态绳长 的变化量，这里选择直线模组驱动连续型机器人 的线长变化，直线模组导程为 ，单位 mm，设置 直线模组驱动器电机每转的脉冲个数为 ，根据 式 (5) 可计算出各个驱动绳长的变化值 ，设直 线模组速度为 ，单位为 mm/s，则可以得到绳长 变化量和步进电机驱动信号频率和脉冲的映射关 系，如式 (9) 所示： m = s c ·∆l f = s c · v (9) 式中：m 表示脉冲数目； f 表示在设定速度 v 下的 频率，单位为 Hz。 2 操作方法研究 2.1 系统设计 为了验证连续型机器人的运动学，搭建了三 关节的连续型机器人控制系统，如图 6 所示，连续 型机器人平台由 PC、MCU、电源、步进电机驱动 器、姿态传感器、直线模组、连续型机器人机械臂 等组成。连续型机器人柔性机械臂由 3 个关节组 成，单关节长度 220 mm，驱动部分由 9 个直线模 组构成。 由式 (7) 和式 (8) 可知，能够由姿态传感器得 第 6 期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1061·
·1062· 智能系统学报 第15卷 到连续型机器人姿态，且连续型机器人的姿态计 行信息交互，发送控制命令实现连续型机器人的 算与绕Z轴的旋转角度无关，因此只需要保证 手柄控制；实现连续型机器人各个关节姿态的实 X轴和Y轴与关节末端坐标系的对应轴的平行关 时显示。姿态传感器负责采集连续型机器人各个 系即可，如图6所示，在每一关节末端安装一个姿 关节的姿态信息，并将姿态信息传输到PC。操控 态传感器。 手柄负责手动控制的人机交互，将手柄信息传输 到PC。MCU为驱动程序的载体，与PC实现信息 交互，接受PC的控制命令，并进行控制命令的解 析，根据解析结构产生驱动信号直接驱动连续型 机器人。连续型机器人平台搭载姿态传感器，受 姿态传感器 MCU的直接驱动，根据直线模组的运动，实现不 同的关节姿态。 电机驱动和电源 2.2末端跟随控制 手柄 连续型机器人具有多个关节，对每个关节都 连续型机器人 进行直接控制必然会加大控制难度，这里设计末 端跟随控制方法实现手柄控制连续型机器人多关 节达到设定姿态，即只控制连续型机器人的末端 MCU 关节姿态，其余关节都跟随末端关节的姿态进行 逐步改变。连续型机器人的关节为等曲率模型， 各关节之间具有连续性，不能突然弯折，且各关 节姿态改变需要依靠多根钢丝绳的协同运动，如 果钢丝绳的运动不同步，将会造成连续型机器人 偏离预期姿态，甚至造成机械结构的损坏。 图6连续型机器人系统 为了防止连续型机器人单次姿态变化过大， Fig.6 Continuous robot system 造成结构破坏，将连续型机器人的姿态变化离散 连续型机器人操控系统主要由PC、MCU、连 化小角度变化，步长为0.05rad;同时也为了使所 续型机器人、操控手柄和姿态传感器5个部分组 有的直线模组在相同的时间内完成线长变化，这 成。图7为各个模块之间的控制框图。 里选取单次线长变化最大的直线模组作为基准， 通过改变其他直线模组的驱动信号频率，从而达 暴 到直线模组的同步。这样既便于实现连续型机器 人的姿态控制，又能够有效避免驱动绳长变化不 、键盘 示器 均匀造成的潜在危险。 3实验验证 USB USB PC 串口 MCU 3.1工作空间仿真 拓展 (主控程序) 口 (驱动板) 由连续型机器人的结构设计可知，其单关节 脉冲 长度为220mm,根据连续型机器人的运动学模 电源 USB 信号 模块 型，可以得到连续型机器人关节末端点位置，遍 USBHID 驱动器 历连续型机器人的末端点进行连续型机器人的工 钢丝绳 姿态 操控 作空间仿真。设置连续型机器人各个关节弯曲角 采集 模块 手柄 连续型机器人 度范围[0,180]，关节旋转角度范围为[0,360]，分 别进行连续型机器人单关节、两关节和三关节的 图7连续型机器人系统框图 工作空间仿真，仿真结果分别如图8(a)、(c)、(e)所 Fig.7 Block diagram of continuous robot system 示；由于连续型机器人工作空间的对称性，对其 其中，PC为主控程序的载体，负责姿态传感 工作空间在XoZ平面的剖视图进行仿真，仿真结 器、操控手柄的信息采集、解析；与MCU模块进 果分别如图8b)、(d)、()所示
到连续型机器人姿态，且连续型机器人的姿态计 算与绕 Z 轴的旋转角度无关，因此只需要保证 X 轴和 Y 轴与关节末端坐标系的对应轴的平行关 系即可，如图 6 所示，在每一关节末端安装一个姿 态传感器。 连续型机器人 电机驱动和电源 MCU 手柄 PC 姿态传感器 图 6 连续型机器人系统 Fig. 6 Continuous robot system 连续型机器人操控系统主要由 PC、MCU、连 续型机器人、操控手柄和姿态传感器 5 个部分组 成。图 7 为各个模块之间的控制框图。 PC (主控程序) 操控 手柄 姿态 采集 模块 MCU (驱动板) 驱动器 USB USB 串口 拓展 口 USBHID 连续型机器人 电源 模块 USB 转 TTL 脉冲 信号 钢丝绳 鼠 标 、 键 盘 显 示 器 图 7 连续型机器人系统框图 Fig. 7 Block diagram of continuous robot system 其中，PC 为主控程序的载体，负责姿态传感 器、操控手柄的信息采集、解析；与 MCU 模块进 行信息交互，发送控制命令实现连续型机器人的 手柄控制；实现连续型机器人各个关节姿态的实 时显示。姿态传感器负责采集连续型机器人各个 关节的姿态信息，并将姿态信息传输到 PC。操控 手柄负责手动控制的人机交互，将手柄信息传输 到 PC。MCU 为驱动程序的载体，与 PC 实现信息 交互，接受 PC 的控制命令，并进行控制命令的解 析，根据解析结构产生驱动信号直接驱动连续型 机器人。连续型机器人平台搭载姿态传感器，受 MCU 的直接驱动，根据直线模组的运动，实现不 同的关节姿态。 2.2 末端跟随控制 连续型机器人具有多个关节，对每个关节都 进行直接控制必然会加大控制难度，这里设计末 端跟随控制方法实现手柄控制连续型机器人多关 节达到设定姿态，即只控制连续型机器人的末端 关节姿态，其余关节都跟随末端关节的姿态进行 逐步改变。连续型机器人的关节为等曲率模型， 各关节之间具有连续性，不能突然弯折，且各关 节姿态改变需要依靠多根钢丝绳的协同运动，如 果钢丝绳的运动不同步，将会造成连续型机器人 偏离预期姿态，甚至造成机械结构的损坏。 为了防止连续型机器人单次姿态变化过大， 造成结构破坏，将连续型机器人的姿态变化离散 化小角度变化，步长为 0.05 rad；同时也为了使所 有的直线模组在相同的时间内完成线长变化，这 里选取单次线长变化最大的直线模组作为基准， 通过改变其他直线模组的驱动信号频率，从而达 到直线模组的同步。这样既便于实现连续型机器 人的姿态控制，又能够有效避免驱动绳长变化不 均匀造成的潜在危险。 3 实验验证 3.1 工作空间仿真 由连续型机器人的结构设计可知，其单关节 长度为 220 mm，根据连续型机器人的运动学模 型，可以得到连续型机器人关节末端点位置，遍 历连续型机器人的末端点进行连续型机器人的工 作空间仿真。设置连续型机器人各个关节弯曲角 度范围 [0,180°]，关节旋转角度范围为 [0,360°]，分 别进行连续型机器人单关节、两关节和三关节的 工作空间仿真，仿真结果分别如图 8(a)、(c)、(e) 所 示；由于连续型机器人工作空间的对称性，对其 工作空间在 XoZ 平面的剖视图进行仿真，仿真结 果分别如图 8(b)、(d)、(f) 所示。 ·1062· 智 能 系 统 学 报 第 15 卷