第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 卷
第6期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1063· 250 400 200 1500 150 200 1000 100 0 500 0 -500 200 200 38 0 400 y/mm 0 0 y/mm 200 0000 x/mm 40-400200 -200 -200-200 /mm 200-20d0o010o200 x/mm (a)单关节工作空间 (b)两关节工作空间 (c)三关节工作空间 250 500 800 400 200 600 300 400 150 200 200 100 100 0 0 -200 50 -100 -200 -400 -30 020406080100120140160 -50050100150200250300350400 602300 0 300 600 x/mm x/mm x/mm (d)单关节工作空间XoZ视图 (e)两关节工作空间XoZ视图 (①三关节工作空间XoZ视图 图8连续型机器人工作空间视图 Fig.8 Views of the continuous robot workspace 根据仿真结果可知随着关节的增加，其可达 结构可知关节驱动之间存在耦合关系，对连续型 空间越来越大，且在XoZ平面正方向包围空间的 机器人弯曲和旋转对绳长的影响进行仿真。 盲区越来越小，与只有一个自由度的单关节传统 设置初始关节状态弯曲角度为日=0,0,01，旋 连杆机器人对比，连续型机器人单关节具有旋转 转角度为Φ=0,0,01，然后分别将关节1(#1)、关 和弯曲两个自由度，证明连续型机器人优越的空 节2(#2)和关节3(#3)从0°弯曲到180°，进行关节 间可达性。 弯曲和绳长变化的仿真实验：3个关节(#1#2#3) 3.2绳长变化仿真 整体同时从0°弯曲到180°，进行关节弯曲和绳长 连续型机器人在初始状态下，处于直立状态， 变化的仿真实验，仿真结果如图9(a)(d)所示，纵 以首关节底端为坐标系原点，可得到连续型机器 坐标为220、440、660mm附近的红、绿、蓝三线分 人末端点坐标为0,0,660；。根据连续型机器人的 别代表3个关节的3根绳长变化。 800 800 800 800 600 600 600 600 400 400 400 400 200 200 0 0 60 120 180 0 60 120 180 0 60 120 180 0 60 120 180 旋转角度/() 旋转角度/() 旋转角度/() 旋转角度/) (a)#3弯曲绳长变化图 (b)#2弯曲绳长变化图 (c)#1弯曲绳长变化图 (d)#1#2#3弯曲绳长变化图 800 800 800 800 600 600 600 600 400 400 400 400 200 200 200 200 120240 360 120 240 360 120 240 360 0 120240 360 旋转角度() 旋转角度/() 旋转角度/() 旋转角度/() (e)#3旋转绳长变化图 (0#2旋转绳长变化图 (g)#1旋转绳长变化图 (h)#1#2#3旋转绳长变化图 图9关节姿态和绳长的仿真 Fig.9 Simulation diagrams of joint attitude and line length
200 250 150 100 0 0 −200 −200 200 200 50 0 y/mm x/mm z/m m 400 200 0 −200 z/m m 0 0 −400 −400−200 400 200 −200 400 y 200 /mm x/mm (a) 单关节工作空间 (b) 两关节工作空间 (c) 三关节工作空间 250 200 150 100 50 0 20 40 60 80 100 120 140 160 x/mm z/mm 500 400 300 −300 −50 −100 0 100 200 −200 0 50 100150200250 350 300 400 x/mm z/mm 800 600 400 −600 −200 0 200 −400 −300 0 300 600 x/mm z/mm (d) 单关节工作空间 XoZ 视图 (e) 两关节工作空间 XoZ 视图 (f) 三关节工作空间 XoZ 视图 1 500 500 0 1 000 −500 −1 000 1 000 0 −2 000 −1 000 2 000 −2 000 −1 000 0 1 0002 000 y/mm x/mm z/mm 图 8 连续型机器人工作空间视图 Fig. 8 Views of the continuous robot workspace 根据仿真结果可知随着关节的增加，其可达 空间越来越大，且在 XoZ 平面正方向包围空间的 盲区越来越小，与只有一个自由度的单关节传统 连杆机器人对比，连续型机器人单关节具有旋转 和弯曲两个自由度，证明连续型机器人优越的空 间可达性。 3.2 绳长变化仿真 连续型机器人在初始状态下，处于直立状态， 以首关节底端为坐标系原点，可得到连续型机器 人末端点坐标为{0,0,660}。根据连续型机器人的 结构可知关节驱动之间存在耦合关系，对连续型 机器人弯曲和旋转对绳长的影响进行仿真。 Θ = {0,0,0} Φ = {0,0,0} 设置初始关节状态弯曲角度为 ，旋 转角度为 ，然后分别将关节 1(#1)、关 节 2(#2) 和关节 3(#3) 从 0°弯曲到 180°，进行关节 弯曲和绳长变化的仿真实验；3 个关节 (#1#2#3) 整体同时从 0°弯曲到 180°，进行关节弯曲和绳长 变化的仿真实验，仿真结果如图 9(a)~(d) 所示，纵 坐标为 220、440、660 mm附近的红、绿、蓝三线分 别代表 3 个关节的 3 根绳长变化。 800 600 400 200 0 60 120 180 旋转角度/(°) 驱动绳长/mm 旋转角度/(°) 旋转角度/(°) 旋转角度/(°) 120 240 200 400 600 800 360 驱动绳长/mm 旋转角度/(°) (a) #3 弯曲绳长变化图 (e) #3 旋转绳长变化图 (f) #2 旋转绳长变化图 (g) #1 旋转绳长变化图 (b) #2 弯曲绳长变化图 (c) #1 弯曲绳长变化图 (d) #1#2#3 弯曲绳长变化图 (h) #1#2#3 旋转绳长变化图 800 600 400 200 0 60 120 180 驱动绳长/mm 800 600 400 200 0 60 120 180 驱动绳长/mm 800 600 400 200 0 60 120 180 驱动绳长/mm 0 120 240 200 400 600 800 360 驱动绳长/mm 旋转角度/(°) 0 120 240 200 400 600 800 360 驱动绳长/mm 旋转角度/(°) 0 120 240 200 400 600 800 360 驱动绳长/mm 旋转角度/(°) 0 图 9 关节姿态和绳长的仿真 Fig. 9 Simulation diagrams of joint attitude and line length 第 6 期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1063·
·1064· 智能系统学报 第15卷 分别设置初始状态弯曲角度为⊙，={0,0,901， 真图波峰波谷相隔120°，对应于连续型机器人结 ⊙2={0,90,01,93={90,0,01，旋转角度为Φ={0,0,01， 构中单关节驱动绳相隔120°。 然后使相应的弯曲关节从0°旋转到360°，进行关 3.3单关节弯曲实验 节旋转对绳长变化的仿真实验；设置初始状态弯 通过控制手柄，进行连续型机器人单关节弯曲 曲角度为⊙4={90,90,901,3个关节同时从0°旋转到 实验和三关节的末端跟随实验。进行程序 360°，进行关节旋转对绳长变化仿真实验，仿真结 初始化，连续型机器人初始状态为：弯曲角度日= 果如图9(e)~(h)所示，纵坐标为220、440、 {0,0,01,旋转角度为④={0,0,01，设置关节角度每次 660mm附近的红、绿、蓝三线分别代表3个关节 改变为0.05rad,遥控手柄设置连续型机器人第三 的3根绳长变化。 关节的弯曲角度分别为⊙1={0,0,31.52，⊙2={0,0， 根据仿真结果可以验证连续型机器人关节驱 57.301,93={0,0,83.08,则可得到仿真姿态和实际姿 动之间的耦合性，低层关节姿态改变时，高层关 态，如图10所示。并记录连续型机器人第三关节 节穿过该层的驱动线绳长也会改变，由关节旋转 从0°弯曲到90°的过程中理论弯曲角度和姿态传感 仿真图9（©）(h)可知每个关节的每根绳长变化仿 器采集到的角度，进行误差仿真，如图11所示。 (a)初始状态 (b)91平台图 (c)⊙平台图 (d)⊙平台图 700 700 700 700 600 600 600 600 500 500 500 500 400 400 400 号300 200 200 200 200 100 100 100 100 0 0 0 0 -1000100200 -1000100200 -1000100200 -1000100200 x/mm x/mm x/mm x/mm (e)初始状态 ()⊙，仿真图 (g)日2仿真图 h)O仿真图 图10单关节姿态改变实验图 Fig.10 Experimental diagrams of attitude changes of single joint 根据实验结果可知姿态传感器角度变化结 机平台操控的理论弯曲角度存在偏差，且误差 果、连续型的仿真结果以及连续型机器人的实际 积累主要发生在弯曲角度较小时，是由于连续 结果具有一致性，验证了姿态传感器解算连续型 型机器人初始状态下钢丝绳不能处于绝对紧绷 机器人姿态的合理性以及连续型机器人手柄控制 状态所致，在第五个采样点之后，误差趋于稳 方式的可行性。 定，此时连续型机器人样机平台姿态控制趋于 姿态传感器的角度测量与连续型机器人样 正常
Θ1 = {0,0,90} Θ2 = {0,90,0} Θ3 = {90,0,0} Φ = {0,0,0} Θ4 = {90,90,90} 分别设置初始状态弯曲角度为 ， ， ，旋转角度为 ， 然后使相应的弯曲关节从 0°旋转到 360°，进行关 节旋转对绳长变化的仿真实验；设置初始状态弯 曲角度为 ，3 个关节同时从 0°旋转到 360°，进行关节旋转对绳长变化仿真实验，仿真结 果 如 图 9(e) ~ ( h ) 所示，纵坐标 为 220 、 440 、 660 mm 附近的红、绿、蓝三线分别代表 3 个关节 的 3 根绳长变化。 根据仿真结果可以验证连续型机器人关节驱 动之间的耦合性，低层关节姿态改变时，高层关 节穿过该层的驱动线绳长也会改变，由关节旋转 仿真图 9(e)~(h) 可知每个关节的每根绳长变化仿 真图波峰波谷相隔 120°，对应于连续型机器人结 构中单关节驱动绳相隔 120°。 3.3 单关节弯曲实验 Θ = {0,0,0} Φ = {0,0,0} Θ1 = {0,0,31.52} Θ2 = {0,0, 57.30} Θ3 = {0,0,83.08} 通过控制手柄，进行连续型机器人单关节弯曲 实验和三关节的末端跟随实验。进行程序 初始化，连续型机器人初始状态为：弯曲角度 ，旋转角度为 ，设置关节角度每次 改变为 0.05 rad，遥控手柄设置连续型机器人第三 关节的弯曲角度分别为 ， ， ，则可得到仿真姿态和实际姿 态，如图 10 所示。并记录连续型机器人第三关节 从 0°弯曲到 90°的过程中理论弯曲角度和姿态传感 器采集到的角度，进行误差仿真，如图 11 所示。 (a) 初始状态 (b) Θ1 平台图 (c) Θ2 平台图 (d) Θ3 平台图 (e) 初始状态 (f) Θ1 仿真图 (g) Θ2 仿真图 (h) Θ3 仿真图 700 600 500 400 300 200 100 −100 0 100 200 0 z/mm x/mm 700 600 500 400 300 200 100 −100 0 100 200 0 z/mm x/mm 700 600 500 400 300 200 100 −100 0 100 200 0 z/mm x/mm 700 600 500 400 300 200 100 −100 0 100 200 0 z/mm x/mm 图 10 单关节姿态改变实验图 Fig. 10 Experimental diagrams of attitude changes of single joint 根据实验结果可知姿态传感器角度变化结 果、连续型的仿真结果以及连续型机器人的实际 结果具有一致性，验证了姿态传感器解算连续型 机器人姿态的合理性以及连续型机器人手柄控制 方式的可行性。 姿态传感器的角度测量与连续型机器人样 机平台操控的理论弯曲角度存在偏差，且误差 积累主要发生在弯曲角度较小时，是由于连续 型机器人初始状态下钢丝绳不能处于绝对紧绷 状态所致，在第五个采样点之后，误差趋于稳 定，此时连续型机器人样机平台姿态控制趋于 正常。 ·1064· 智 能 系 统 学 报 第 15 卷
第6期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1065· a 理论角度 3)操作第一关节弯曲角度为34.39°，结果如 图12(c)所示： 4)继续点击前进按钮，完成第二关节姿态 。50 跟随，此时连续型机器人姿态为弯曲角度日= 40 0,57.32,34.391,旋转角度中={0,37.83,37.83，结果 30 如图12(d所示； 5)继续点击前进按钮，直到关节姿态参数为 10 日={22.93,34.39,34.39}，Φ={15.13,37.83,37.83)，其 0 5 10 1520253035 姿态传感器的结果为0=22.93,34.39,34.391，Φ= 采样点 {17.19,37.83,36.691,样机平台图如图12(e)所示，仿 图11姿态偏差图 真图如图12（①所示。 Fig.11 Diagram of attitude deviation 根据图12(a)(e)可知，可以通过手柄进行连 3.4三关节操作实验 续型机器人末端姿态的控制，并可以实现在控制 为了验证末端跟随手柄操控方法的正确性， 过程中实现末端关节的操作，验证了连续型机器 设置连续型机器人初始状态弯曲角度为6={0,0,01， 人手柄控制策略的可行性和正确性。根据图12() 旋转角度为Φ={0,0,01。 可知，连续型机器人理论姿态和实际姿态之间存 1)操控手柄使第一关节弯曲角度为57.32°， 在误差，可知在进行多关节弯曲时，随着弯曲角 旋转角度为37.83°，结果如图12(a)所示； 度变大，连续型机器人柔性机械臂易受到重力的 2)点击前进按钮实现第一关节的姿态跟随， 影响，这是由于无法保证各关节初始时刻钢丝绳 图12(b)为姿态跟随的中间过程图； 处于绝对紧绷状态和关节处于绝对直立状态。 (a)操作a姿态 (b)操作b姿态 (c)操作c姿态 400「 300 一理论值 ·实际角度 号200 100 0 -1000 100200300400 x/mm (d操作d姿态 (e)操作e姿态 (①仿真图 图12手柄控制三关节未端跟随实验 Fig.12 Three-joint follow terminal experiment 4结束语 模组作为驱动机构，减小卷轴驱动过程中缠绕不 均带来的线长误差。 本文主要模仿蛇脊椎骨的构造设计了基于球 针对样机平台建立连续型机器人运动学模 铰链连接的连续型机器人柔性机械臂结构，柔性 型，进行空间可达仿真实验，验证连续型机器人 机械臂由各关节段穿过柔性支撑杆构成，相邻导 优越的空间可达性，建立从工作空间到姿态空间 线圆盘的球头球壳同心连接，这种结构较大程度 再到绳长变化空间、驱动空间的映射关系，并进 地保证了柔性机械臂的韧性和等曲性；使用直线 行仿真验证，验证了运动学模型和驱动映射关系
0 10 5 10 15 20 25 30 35 20 30 40 50 60 70 80 90 角度/(°) 采样点 理论角度 实际角度 角度误差 图 11 姿态偏差图 Fig. 11 Diagram of attitude deviation 3.4 三关节操作实验 Θ = {0,0,0} Φ = {0,0,0} 为了验证末端跟随手柄操控方法的正确性， 设置连续型机器人初始状态弯曲角度为 ， 旋转角度为 。 1) 操控手柄使第一关节弯曲角度为 57.32°， 旋转角度为 37.83°，结果如图 12(a) 所示； 2) 点击前进按钮实现第一关节的姿态跟随， 图 12(b) 为姿态跟随的中间过程图； 3) 操作第一关节弯曲角度为 34.39°，结果如 图 12(c) 所示； Θ = {0,57.32,34.39} Φ = {0,37.83,37.83} 4) 继续点击前进按钮，完成第二关节姿态 跟随，此时连续型机器人姿态为弯曲角度 ，旋转角度 ，结果 如图 12(d) 所示； Θ = {22.93,34.39,34.39} Φ = {15.13,37.83,37.83} Θ = {22.93,34.39,34.39} Φ = {17.19,37.83,36.69} 5) 继续点击前进按钮，直到关节姿态参数为 ， ， 其 姿态传感器的结果为 ， ，样机平台图如图 12(e) 所示，仿 真图如图 12(f) 所示。 根据图 12(a)~(e) 可知，可以通过手柄进行连 续型机器人末端姿态的控制，并可以实现在控制 过程中实现末端关节的操作，验证了连续型机器 人手柄控制策略的可行性和正确性。根据图 12(f) 可知，连续型机器人理论姿态和实际姿态之间存 在误差，可知在进行多关节弯曲时，随着弯曲角 度变大，连续型机器人柔性机械臂易受到重力的 影响，这是由于无法保证各关节初始时刻钢丝绳 处于绝对紧绷状态和关节处于绝对直立状态。 (a) 操作 a 姿态 (b) 操作 b 姿态 (c) 操作 c 姿态 (d) 操作 d 姿态 (e) 操作 e 姿态 (f) 仿真图 −100 0 0 100 100 100 200 200 200 300 300 400 400 理论值 实际角度 z/mm y/mm x/mm 图 12 手柄控制三关节末端跟随实验 Fig. 12 Three-joint follow terminal experiment 4 结束语 本文主要模仿蛇脊椎骨的构造设计了基于球 铰链连接的连续型机器人柔性机械臂结构，柔性 机械臂由各关节段穿过柔性支撑杆构成，相邻导 线圆盘的球头球壳同心连接，这种结构较大程度 地保证了柔性机械臂的韧性和等曲性；使用直线 模组作为驱动机构，减小卷轴驱动过程中缠绕不 均带来的线长误差。 针对样机平台建立连续型机器人运动学模 型，进行空间可达仿真实验，验证连续型机器人 优越的空间可达性，建立从工作空间到姿态空间 再到绳长变化空间、驱动空间的映射关系，并进 行仿真验证，验证了运动学模型和驱动映射关系 第 6 期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1065·
·1066· 智能系统学报 第15卷 的正确性和合理性。 actions on mechatronics,2017,22(1):29-40. 搭建了三关节连续型机器人样机平台整体系 [10]DONG X,AXINTE D,PALMER D,et al.Development 统，分析了姿态传感器测量姿态到连续型机器人 of a slender continuum robotic system for on-wing in- 姿态的转换关系，并根据连续型机器人多自由度 spection/repair of gas turbine engines[J].Robotics and 的特点，选择多自由度手柄，设计手柄末端跟随 computer-integrated manufacturing,2017,44:218-229. 控制模式，通过实际平台实验，验证了姿态转换 [11]BUCKINGHAM R.GRAHAM A.Nuclear snake-arm ro- bots[J].Industrial robot,2012,39(1):6-11. 关系的正确性以及手柄控制连续型机器人多关节 [12]孙立宁，胡海燕，李满天.连续型机器人研究综述.机 实现设置姿态方法的正确性和可行性。 器人，2010,32(5)：688694. 参考文献： SUN Lining,HU Haiyan,LI Mantian.A review on con- tinuum robot[J].Robot,2010,32(5):688-694. [1]王丹，宁宁，石磊，等.飞机结构强度试验中的油箱泄漏 [13]胡海燕，王鹏飞，孙立宁，等.线驱动连续型机器人的运 检测).无损检测，2019,41(9：56-59. 动学分析与仿真[.机械工程学报，2010,46(19)：1-8. WANG Dan,NING Ning,SHI Lei,et al.The leak detec- HU Haiyan,WANG Pengfei,SUN Lining,et al.Kinemat- tion for fuel tank in the aircraft structure strength test[]. ic analysis and simulation for cable-driven continuum ro- Nondestructive testing,2019,41(9):56-59. bot[J].Journal of mechanical engineering,2010,46(19): [2]冯振宇，陈磊，周惠文.飞机整体油箱的微生物腐蚀及维 1-8 护[.航空维修与工程，2009(3)：54-56. [14]钱文欢.仿象鼻混合关节连续型机器人的设计与分 FENG Zhenyu,CHEN Lei,ZHOU Huiwen.Microbial cor- 析D].合肥：中国科学技术大学，2014。 rosion of aircraft integral fuel tanks and related mainten- QIAN Wenhuan.Design and analysis of a trunk-like con- ance[J].Aviation maintenance engineering,2009(3): tinuum robot with hybrid joints[D].Hefei:University of 54-56. Science and Technology of China,2014. [3]仇方迹.A320飞机油箱渗漏修理的新方法[J).科技传 [15]ZHAO Jianghai,YE Xiaodong,QIAN Wenhuan.Re- 播.2012(21)：30.29 search on kinematic modeling of octopus-like arm manip- CHOU Fangji.A new method for repairing fuel tank leaks ulator composed with mixed joints[J].Applied mechanics on A320 aircraft[J].Public communication of science and materials,2014,461:278-283. technology,2012(21):30,29. [16]LI Minhan,KANG Rongjie,GENG Shineng,et al. [4]王田苗，郝雨飞，杨兴帮，等.软体机器人：结构、驱动、传 Design and control of a tendon-driven continuum 感与控制.机械工程学报，2017,53(13)：1-13. robot[J].Transactions of the institute of measurement and WANG Tianmiao,HAO Yufei,YANG Xingbang,et al control,2018,40113263-3272. Soft robotics:structure,actuation,sensing and control[J]. [17]DONG Xin,RAFFLES M.COBOS-GUZMAN S,et al.A Journal of mechanical engineering,2017,53(13):1-13. novel continuum robot using twin-pivot compliant joints: [5]ROBINSON G.DAVIES J B C.Continuum robots-a state design,modeling,and validation[J].Journal of mechan- of the art[C]//Proceedings of 1999 IEEE International Con- isms and robotics,2016,8(2):021010 ference on Robotics and Automation.Detroit,USA,1999: [18]曹燕飞.变刚度微创介入手术连续体机器人研究[D], 2849-2854 南京：南京航空航天大学，2019 [6]BUCKINGHAM R.Snake arm robots[J].Industrial robot: CAO Yanfei.Research on a variable-stiffness continuum an international journal,2002,29(3):242-245. manipulator for minimally invasive surgery[D].Nanjing: [7]NIU Guochen.ZHENG Zunchao.GAO Qingji.Collision Nanjing University of Aeronautics and Astronautics, free path planning based on region clipping for aircraft fuel 2019. tank inspection robot[C]//Proceedings of 2014 IEEE Inter- [19]CAO Yanfei,JU Feng,ZHANG Lei,et al.A novel vari- national Conference on Robotics and Automation.Hong able-stiffness flexible manipulator actuated by shape Kong.China,2014:3227-3233. memory alloy for minimally invasive surgery[J].Proceed- [8]GILBERT H B,RUCKER D C,WEBSTER III R J.Con- ings of the institution of mechanical engineers,part H: centric tube robots:the state of the art and future direc- journal of engineering in medicine,2018,232(11): tions[J].Robotics research,2016:253-269. 1098-1110. [9]CONRAD B L.ZINN M R.Interleaved continuum-rigid [20]赵智远.线驱动连续型机械臂运动学与运动规划研 manipulation:an approach to increase the capability of 究D].长春：中国科学院大学（中国科学院长春光学精 minimally invasive surgical systems[J].IEEE/ASME trans- 密机械与物理研究所)，2019
的正确性和合理性。 搭建了三关节连续型机器人样机平台整体系 统，分析了姿态传感器测量姿态到连续型机器人 姿态的转换关系，并根据连续型机器人多自由度 的特点，选择多自由度手柄，设计手柄末端跟随 控制模式，通过实际平台实验，验证了姿态转换 关系的正确性以及手柄控制连续型机器人多关节 实现设置姿态方法的正确性和可行性。 参考文献： 王丹, 宁宁, 石磊, 等. 飞机结构强度试验中的油箱泄漏 检测 [J]. 无损检测, 2019, 41(9): 56–59. WANG Dan, NING Ning, SHI Lei, et al. The leak detection for fuel tank in the aircraft structure strength test[J]. Nondestructive testing, 2019, 41(9): 56–59. [1] 冯振宇, 陈磊, 周惠文. 飞机整体油箱的微生物腐蚀及维 护 [J]. 航空维修与工程, 2009(3): 54–56. FENG Zhenyu, CHEN Lei, ZHOU Huiwen. Microbial corrosion of aircraft integral fuel tanks and related maintenance[J]. Aviation maintenance & engineering, 2009(3): 54–56. [2] 仇方迹. A320 飞机油箱渗漏修理的新方法 [J]. 科技传 播, 2012(21): 30, 29. CHOU Fangji. A new method for repairing fuel tank leaks on A320 aircraft[J]. Public communication of science & technology, 2012(21): 30, 29. [3] 王田苗, 郝雨飞, 杨兴帮, 等. 软体机器人: 结构、驱动、传 感与控制 [J]. 机械工程学报, 2017, 53(13): 1–13. WANG Tianmiao, HAO Yufei, YANG Xingbang, et al. Soft robotics: structure, actuation, sensing and control[J]. Journal of mechanical engineering, 2017, 53(13): 1–13. [4] ROBINSON G, DAVIES J B C. Continuum robots - a state of the art[C]//Proceedings of 1999 IEEE International Conference on Robotics and Automation. Detroit, USA, 1999: 2849-2854. [5] BUCKINGHAM R. Snake arm robots[J]. Industrial robot: an international journal, 2002, 29(3): 242–245. [6] NIU Guochen, ZHENG Zunchao, GAO Qingji. Collision free path planning based on region clipping for aircraft fuel tank inspection robot[C]//Proceedings of 2014 IEEE International Conference on Robotics and Automation. Hong Kong, China, 2014: 3227−3233. [7] GILBERT H B, RUCKER D C, WEBSTER III R J. Concentric tube robots: the state of the art and future directions[J]. Robotics research, 2016: 253–269. [8] CONRAD B L, ZINN M R. Interleaved continuum-rigid manipulation: an approach to increase the capability of minimally invasive surgical systems[J]. IEEE/ASME trans- [9] actions on mechatronics, 2017, 22(1): 29–40. DONG X, AXINTE D, PALMER D, et al. Development of a slender continuum robotic system for on-wing inspection/repair of gas turbine engines[J]. Robotics and computer-integrated manufacturing, 2017, 44: 218–229. [10] BUCKINGHAM R, GRAHAM A. Nuclear snake-arm robots[J]. Industrial robot, 2012, 39(1): 6–11. [11] 孙立宁, 胡海燕, 李满天. 连续型机器人研究综述 [J]. 机 器人, 2010, 32(5): 688–694. SUN Lining, HU Haiyan, LI Mantian. A review on continuum robot[J]. Robot, 2010, 32(5): 688–694. [12] 胡海燕, 王鹏飞, 孙立宁, 等. 线驱动连续型机器人的运 动学分析与仿真 [J]. 机械工程学报, 2010, 46(19): 1–8. HU Haiyan, WANG Pengfei, SUN Lining, et al. Kinematic analysis and simulation for cable-driven continuum robot[J]. Journal of mechanical engineering, 2010, 46(19): 1–8. [13] 钱文欢. 仿象鼻混合关节连续型机器人的设计与分 析 [D]. 合肥: 中国科学技术大学, 2014. QIAN Wenhuan. Design and analysis of a trunk-like continuum robot with hybrid joints[D]. Hefei: University of Science and Technology of China, 2014. [14] ZHAO Jianghai, YE Xiaodong, QIAN Wenhuan. Research on kinematic modeling of octopus-like arm manipulator composed with mixed joints[J]. Applied mechanics and materials, 2014, 461: 278–283. [15] LI Minhan, KANG Rongjie, GENG Shineng, et al. Design and control of a tendon-driven continuum robot[J]. Transactions of the institute of measurement and control, 2018, 40(11): 3263–3272. [16] DONG Xin, RAFFLES M, COBOS-GUZMAN S, et al. A novel continuum robot using twin-pivot compliant joints: design, modeling, and validation[J]. Journal of mechanisms and robotics, 2016, 8(2): 021010. [17] 曹燕飞. 变刚度微创介入手术连续体机器人研究 [D]. 南京: 南京航空航天大学, 2019. CAO Yanfei. Research on a variable-stiffness continuum manipulator for minimally invasive surgery[D]. Nanjing: Nanjing University of Aeronautics and Astronautics, 2019. [18] CAO Yanfei, JU Feng, ZHANG Lei, et al. A novel variable-stiffness flexible manipulator actuated by shape memory alloy for minimally invasive surgery[J]. Proceedings of the institution of mechanical engineers, part H: journal of engineering in medicine, 2018, 232(11): 1098–1110. [19] 赵智远. 线驱动连续型机械臂运动学与运动规划研 究 [D]. 长春: 中国科学院大学 (中国科学院长春光学精 密机械与物理研究所), 2019. [20] ·1066· 智 能 系 统 学 报 第 15 卷
第6期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1067· ZHAO Zhiyuan.Research on kinematics and motion GAO Qingji,WANG Weijuan,NIU Guochen,et al.Study planning of cable driven continuum manipulator[D] of bionic structure and kinematics of robot for aircraft fuel Changchun:Changchun Institute of Optics,Fine Mechan- tank inspection.Acta aeronautica et astronautica sinica, ics and Physics Chinese Academy of Sciences,2019. 2013,34(7):1748-1756. [21]韦贵炜.空间连续型机械臂设计与运动学仿真D].长 [25]NIU Guochen,WANG Li,GAO Qingji,et al.Path-track- 春：中国科学院大学（中国科学院长春光学精密机械与 ing algorithm for aircraft fuel tank inspection robots[J]. 物理研究所)，2019. International journal of advanced robotic systems,2014, WEI Guiwei.Design and kinematics simulation of a space 11(5):82 continuous manipulator[D].Changchun:Changchun Insti- 作者简介： tute of Optics,Fine Mechanics and Physics,Chinese Academy of Sciences,2019. 牛国臣，副教授，博士，中国人工 智能学会智能产品与产业工作委员会 [22]韦贵炜，徐振邦，赵智远，等.线驱动连续型机械臂设计 委员，主要研究方向为智能机器人环 与运动学仿真).机械传动，2019,43(11)32-38,53. 境感知、导航控制规划。参与完成国 WEI Guiwei,XU Zhenbang,ZHAO Zhiyuan,et al. 家自然科学基金及民航局科技项目 Design and kinematics simulation of a wire-driven multi- 3项、主持完成横向及校级项目共 section continuous manipulator[J].Journal of mechanical 4项。发表学术论文几十余篇。 transmission,.2019.43(1132-38.53. [23]NIU Guochen,ZHENG Zunchao,GAO Qingji,et al.A 张云霄，硕士研究生，主要研究方 向为连续型机器人运动规划。 novel design of aircraft fuel tank inspection robot. Telkomnika,2013,11(7):36843692 [24]高庆吉，王维娟，牛国臣，等.飞机油箱检查机器人的仿 生结构及运动学研究[J].航空学报，2013,34(7)： 1748-1756
ZHAO Zhiyuan. Research on kinematics and motion planning of cable driven continuum manipulator[D]. Changchun: Changchun Institute of Optics, Fine Mechanics and Physics Chinese Academy of Sciences, 2019. 韦贵炜. 空间连续型机械臂设计与运动学仿真 [D]. 长 春: 中国科学院大学 (中国科学院长春光学精密机械与 物理研究所), 2019. WEI Guiwei. Design and kinematics simulation of a space continuous manipulator[D]. Changchun: Changchun Institute of Optics, Fine Mechanics and Physics, Chinese Academy of Sciences, 2019. [21] 韦贵炜, 徐振邦, 赵智远, 等. 线驱动连续型机械臂设计 与运动学仿真 [J]. 机械传动, 2019, 43(11): 32–38, 53. WEI Guiwei, XU Zhenbang, ZHAO Zhiyuan, et al. Design and kinematics simulation of a wire-driven multisection continuous manipulator[J]. Journal of mechanical transmission, 2019, 43(11): 32–38, 53. [22] NIU Guochen, ZHENG Zunchao, GAO Qingji, et al. A novel design of aircraft fuel tank inspection robot[J]. Telkomnika, 2013, 11(7): 3684–3692. [23] 高庆吉, 王维娟, 牛国臣, 等. 飞机油箱检查机器人的仿 生结构及运动学研究 [J]. 航空学报, 2013, 34(7): 1748–1756. [24] GAO Qingji, WANG Weijuan, NIU Guochen, et al. Study of bionic structure and kinematics of robot for aircraft fuel tank inspection[J]. Acta aeronautica et astronautica sinica, 2013, 34(7): 1748–1756. NIU Guochen, WANG Li, GAO Qingji, et al. Path-tracking algorithm for aircraft fuel tank inspection robots[J]. International journal of advanced robotic systems, 2014, 11(5): 82. [25] 作者简介： 牛国臣，副教授，博士，中国人工 智能学会智能产品与产业工作委员会 委员，主要研究方向为智能机器人环 境感知、导航控制规划。参与完成国 家自然科学基金及民航局科技项目 3 项、主持完成横向及校级项目共 4 项。发表学术论文几十余篇。 张云霄，硕士研究生，主要研究方 向为连续型机器人运动规划。 第 6 期 牛国臣，等：连续型机器人运动学仿真和操控系统设计 ·1067·
