李凤刚，赵磊，何广平，狄杰建
（北方工业大学机械与材料工程学院，北京 100144）
地震、采矿事故等灾害的发生导致了大量的人员伤亡，据统计，灾害后的72h是搜寻和救助幸存者的关键时间[1]，然而，灾后通道受阻会导致大多数救援设备难以进入废墟，这对救援工作的开展提出了严峻挑战。
研究人员正尝试开发搜索设备进入灾难现场执行救援任务。轮式搜索机器人[2]结构简单，成本低，平面移动速度快；
但在面对复杂的废墟环境是穿越能力略显不足。履带式搜索机器人[3]具有较强的通过能力，不仅适用于平坦的地面的运动，而且适用于非结构性环境；
但它们的体积相对较大，不能在灾后环境中进入狭窄的空间[4]。仿生搜索机器人起步晚，但发展迅速；
文献[5]是典型的超冗余度串联仿生机器人[6]，模块化的设计使其在沙漠、废墟等环境下可快速前行，然而该类机器人在转角过大处容易卡在废墟中并且难以摆脱困境。连续体[7]与软体[8]机器人由于固有的柔顺性和连续变形的能力，使其在复杂的灾后现场的中有着良好的应用前景。通过推进装置工业内窥镜探入废墟缝内部以获取狭小内部的实时状态是消防人员搜寻幸存者有效途径之一，但在复杂多变的废墟间隙中，工业内窥镜存在严重的转向问题，很难将其头部准确的推入预期的间隙或通道[9]。
以增加内窥式搜索机器人的导向性与灵活性为目的，设计了一款由并联拉线驱动的内窥式搜索机器人，其具备连续体机器人的灵活变形能力和工业内窥镜成本低廉与方便携带的特点，可协助消防员完成灾难现场复杂狭小空间的搜索工作。
接着，研究机器人导向末端的运动学特点，给出了导向末端完整的运动学建模过程。然后，基于获得的运动学模型给出了导向末端工作空间的分析结果和正、逆运动学仿真算例。最后，通过实验验证了内窥式搜索机器人末端的弯转导向能力与狭小通道的穿越能力。
该内窥式搜索机器人主要由导向末端、柔性臂、摄像头模组以及驱动装置构成，如图1所示。导向末端由柔性骨架、导线盘和驱动线三部分组成。其柔性骨架保证运动过程中导向末端轴向不可拉伸或压缩；
周向均布三个穿线孔的导线盘可限定导驱动线的位置与形状；
三根驱动线与端部导线盘刚性连接。柔性臂是基部导线盘以下的部分，包含多媒体数据线、驱动线、解耦套管以及外部套管；
解耦套管的解耦原理类似与自行车的刹车系统，柔性臂运动过程任意改变套管区的形状，并不会引起无套管区拉线长度的变化。摄像头模组集成了视频、音频系统和照明系统。
图1 内窥式搜索机器人机械结构Fig.1 Mechanical Structure of Endoscopic Search Robot
该机器人导向末端采用冗余的驱动方式，通过协调改变3根驱动线的长度使机器人完成2自由度(DOF)的弯转运动，以满足搜索现场探寻非结构性环境中狭小减小或通道的需求。此外，机器人采用线驱动的方式有效的减轻了自重，缩小了体积；
柔性骨架固有的连续性与柔顺性能够降低人机接触过程的冲击力，提高搜索过程的安全性。
由于导向末端采用电机后置的拉线驱动方式，使得其关节空间和驱动空间不相同。以获取拉线长度变化量与机器人末端位姿变化的对应关系为目的，设定三个空间，如图2所示。
图2 运动学分析流程Fig.2 Kinematics Analysis Process
进而分析获得机器人导向末端的全局正向运动学与逆运动学。图2中S1S2S3表示三根驱动线的长度，ϕ∈[0，2π]为导向末端弯曲后所在平面与x轴正方向的夹角，θ∈[0，π]表征导向末端弯曲后的曲率角，(x y z)表示导向末端顶部在工作空间中的坐标值；
基于导向末端轴向不可伸缩与延伸的特点，设中心柔性骨架长度为定值S。
3.1 关节空间到工作空间的映射
对机器人导向末端建模如下：基坐标系(o0−x0y0z0)固定在基部导线盘的几何中心O0，末端标系(o1−x1y1z1)固定在顶端导线盘的几何中心O1；
其x指向基础导线盘的第一个孔眼z轴竖直朝向上，y轴方向可由右手定则确定。基于恒定曲率假设建立导向末端的运动学模型，如图3所示。
图3 工作空间位姿模型Fig.3 Workspace Pose Model
即假设导向末端运动过程中，柔性骨架弯曲形状近似为圆弧且曲率处处相等［10］。
由图3 机器人工作时(θ≠0)的位姿可得，基坐标到末端坐标的变换可以通过四个基本变换实现：首先将基准坐标原点O0平移至末端坐标系原点O1；
然后将平移得到的坐标绕其z轴旋转角度ϕ；
接着将旋转得到的坐标系绕其y轴旋转角度θ；
最后坐标系绕新z轴旋转−ϕ。按上述过程我们将相应的旋量坐标ξ∈ℜ6写成：
其中，R=S/θ。通过定义算子“∧”将矢量从ℜ6映射到SE(3)的李代数se(3)，即将六维向量ξ映射为4 × 4 阶矩阵。由此旋量坐标（1）对应的李代数可以写成：
根据乘积指数公式：
获得该机器人从关节空间到工作空间段的齐次变换矩阵如下：
3.2 关节空间到驱动空间的映射
为推导机器人关节空间Θ=[ϕ θ]T到驱动空间q=[S1S2S3]T的映射关系，在机器人工作过程中我们可以建立几何模型，如图4所示。并做出机器人在x1o1y1面横切图，如图5所示。当导向末端弯曲方向与x0轴夹角为ϕ，弯曲角度为θ时，可得：
图4 驱动空间与关节空间之间的关系Fig.4 Relationship Between Driving Space and Joint Space
图5 x1o1y1面横切图Fig.5 x1o1y1 Sectional View
驱动线对应的曲率半径Ri可被记为：
式中：S—弧oo1的长度；
r—穿线孔到导线盘中心的距离。
驱动线长度Si可表示为：
综合式（6）～式（8）可得关节空间到驱动空间的映射为：
3.3 驱动空间到关节空间的映射
本小节中，将分析机器人的驱动空间q=[S1S2S3]T到位姿空间Θ=[ϕ θ]T的映射。
由式（9）可得：
进而确定驱动空间到关节空间的映射：
3.4 工作空间到关节空间的映射
连续体搜索机器人在运动过程中，其中基部导线盘原点O0固定在右旋欧几里德空间的原点，其末端在基坐标下的位置可被参数化为Ω=[x y z]T。
本节在工作空间x、y、z已知的条件下确定关节空间参数Θ=[ϕ θ]T。
机器人弯转运动过程，如图6所示。ϕ可表示为：
在图6中，A为O’1在x0o0y0面上的投影，B为弧OO’1的圆心，进而可得出：
图6 工作空间与关节空间之间的关系Fig.6 Relationship Between Workspace and Joint Space
在Rt△ABO"1中，根据Pythagoras定理可得：
进而得到：
式（12）与式（18）为工作空间到关节空间的映射。
为了验证所得运动学模型的有效性，基于MATLAB 分析了导向末端的工作范围，并给出了搜索机器人末端的运动学的仿真算例，算例中用到的基本参数如下：柔性骨架长度为150mm，驱动线轴心所在的分度圆半径为10.5mm；
运动过程中的弯转参数范围为θ∈[−π，π]、ϕ∈[0，2π]。
4.1 工作空间分析
为了标定该机器人导向末端的工作空间，以基盘中心为坐标原点，绘制机器人末端点空间取值范围，如图7所示。
图7 搜索机器人的工作空间Fig.7 Work Space of Massage Robot
仿真结果表明，该机器人导向末端工作空间形状近似一个半球面，其工作空间坐标最大值为：Xmax=Ymax=108.7mm，Zmax=150mm。
4.2 运动学仿真算例
在导向末端顶点可达的工作空间范围内规划一条如式（19）所示的空间螺旋轨迹，基于前文获得的逆运动学算法对导向末端进行运动控制。
所给算例中，起始状态机器人末端坐标为( 0，0，150)，导向末端驱动线的长度为150mm，弯曲角度ϕ与旋转角度θ均为0°；
当采样间隔为0.12s时，机器人导向末端在（0～6）内的空间位姿，如图8中的彩色弧线所示。连接末端点的洋红色曲线表示机器人末端的空间轨迹。
图8 运动轨迹与空间位姿Fig.8 Motion Trajectory and Spatial Pose
由图9中关节空间参数随时间的变化可知，当运动停止时，弯、转角度值θ=3.142、φ=6.283；
且该过程中关节空间参数曲线斜率均随时间的延长而增大，表征着运动过程中弯转速度都在增长，与图8中空间姿态间隙逐渐增大相吻合。
图9 关节空间参数随时间变化曲线Fig.9 Curves of Time Parameters of Virtual Joint Space
此外，算例中三根驱动线长度随时间的变化曲线，如图10所示。结果显示，该过程中拉线长度变化量最大的为L1，其最大拉动长度为32.99mm；
在运动停止时L2、L3长度变化相同，均为16.49mm。所得仿真数据对样机的加工制作具有指导意义。
图10 驱动空间参数随时间变化曲线Fig.10 Curve of Driving Space Parameters Over Time
5.1 弯转控制实验
为进一步验证导向末端的空间弯转能力，在无约束环境下对导向末端运动性能进行弯转验证实验。令导向末端首先在10s内沿φ=0°平面内完成60°弯曲；
然后，在接下来的30s内完成顺时针（俯视）周向转动。为保证运动的平稳性，弯转动作的运动规划通过关节空间中的五次多项式插值完成，两个动作的起止时刻导向末端顶点的速度、加速度均设置为零，得到的关节空间运动参数，如图11所示。
图11 关节空间运动规划参数Fig.11 Joint Space Motion Planning Parameters
图像为平滑的曲线，可有效避免驱动冲击，对电机具有保护作用。弯、转过程的空间位姿仿真结果与实验结果对比，如图12、图13所示。
图12 弯曲运动空间位姿对比Fig.12 Comparison of Postures in Bending Motion Space
图13 周转运动空间位姿对比Fig.13 Spatial Posture Comparison of Turnover Movement
实验中，导向末端弯转过程柔顺、灵活，能够在以基部导向盘为中心的任意平面内完成弯曲运动，与仿真预期的运动过程具有一致性，证明了所提出运动学模型的有效性。
实验过程中，通过位移传感器对驱动线的拉动长度进行采集，采样间隔为1s，将得到的结果与驱动空间参数仿真结果对比，如图14所示。
图14 驱动空间仿真与实验参数对比Fig.14 Comparison of Driving Space Simulation and Experimental Earameters
实验结果表明，最大驱动误差不超2%，造成误差的主要原因是导向末端柔性骨架支撑刚度不够，导致其弯转过程回拉弹力不足造成的。
5.2 搜索性能实验
为验证搜索机器人在复杂狭小空间内的搜索能力，搭建废墟三维模拟通道，如图15所示。
图15 搜索性能实验Fig.15 Search and Rescue Performance Experiment
所给示例中，机器人在穿越模拟的三维废墟狭小通道时，需依次完成择向、转向和内窥等动作，并通过调整导向末端位姿将目标点的实时信息传给使用者。
实验结果表明原理样机能够在转角小于90°、直径大于50mm的空间不规则通道内顺利完成择向、转向和内窥操作；
在没有辅助推送设备的前提下，原理样机的柔性臂能够顺利通过三次弯折为90°的空间转向通道。
线驱动的方式满足搜索机器人进一步小型化制作的要求，减小的导线盘尺寸与增加柔性臂表面光滑度，将能够进一步提升搜索机器人对小尺寸通道与多转向通道的适应能力。
（1）设计了一款具有灵活导向末端的内窥式柔性搜索机器人。该机器人纤细、灵活的柔性臂对灾难现场的狭小通道有良好的适应能力；
面对多变的前行通道，导向末端能有效的择向能力，并将前端空间状态实时传给操作者。
（2）提出了一种用于灵活导向末端的完整运动学分析方法。正向运动学中关节空间到工作空间的映射基于旋量理论获得，驱动空间到关节空间的映射以及全局的逆运动学映射通过几何分析的方法得到。给出的运动学解析映射简单、直观，可用于机器人导向末端的实时控制。
（3）通过对搜索机器人导向末端进行仿真分析和原理样机实验，验证了运动学算法的正确性和机器人末端轨迹规划与控制的可行性。
性能实验表明，所设计的搜索机器人对空间复杂的狭小通道有良好的适应能力强，能够为搜索救援任务提供有效帮助。
猜你喜欢位姿运动学柔性一种柔性抛光打磨头设计机械工业标准化与质量(2022年9期)2022-09-30灌注式半柔性路面研究进展(1)——半柔性混合料组成设计石油沥青(2021年5期)2021-12-02高校学生管理工作中柔性管理模式应用探索文化创新比较研究(2020年7期)2021-01-13基于MATLAB的6R机器人逆运动学求解分析河北省科学院学报(2020年1期)2020-05-25工业机器人在MATLAB-Robotics中的运动学分析重型机械(2019年3期)2019-08-27基于D-H法的5-DOF串并联机床运动学分析制造技术与机床(2018年11期)2018-11-23基于共面直线迭代加权最小二乘的相机位姿估计光学精密工程(2016年5期)2016-11-07基于CAD模型的单目六自由度位姿测量光学精密工程(2016年4期)2016-11-07小型四旋翼飞行器位姿建模及其仿真湖北工业大学学报(2016年5期)2016-02-27基于运动学原理的LBI解模糊算法海军航空大学学报(2015年1期)2015-11-11