基于ROS 的液压重载机械臂避障仿真

霍寒旭，袁昌耀，曹秀芳，杨秀萍，乔志峰，王收军

（1.天津理工大学机械工程学院，天津市先进机电系统设计与智能控制重点实验室，机电工程国家级实验教学示范中心，天津300384；2.南京晨光集团有限公司，南京210006）

开源机器人操作系统ROS 可以有效地处理这些问题，成为当下热门的机器人研究平台.

本文基于ROS 平台，以某七自由度液压重载机械臂为研究对象进行避障仿真.

1 机械臂运动学分析

图1 为机械臂的运动示意图.

机械臂的模型可看作由多个彼此关联的运动关节组成.正运动学问题的计算即为通过各关节的位置变换得到末端执行器的位置与姿态的过程[1].建立该机械臂的D-H 坐标系，如图2 所示，用D-H 参数来描述各关节位置变换的关系，由此计算机械臂的正运动学问题[2-3].

图2 中X0Y0Z0为基坐标系，X7Y7Z7为末端执行器的坐标系.机械臂的D-H 参数表如表1 所示.

表1 中，di为相邻关节间的距离，ai为连杆的长度，αi为连杆相对上个连杆的扭角，θi为关节转角范围.

表1 中的参数由机械臂相邻关节的相对位置得到，用于计算机械臂的正逆解.实现正逆解求解可以利用ROS 平台中MoveIt! 工具中的RRT 算法得到.

图1 机械臂的运动示意图Fig.1 The motion diagram of manipulator

图2 机械臂坐标系Fig.2 The coordinate of manipulator

表1 机械臂的D-H 参数表Tab.1 The D-H parameters of manipulator

式中，cθi表示cosθi，sθi表示sinθi.

式（2）中，n，o，a表示末端执行器相对于基坐标的姿态，即n=[nx，ny，nz]T，o=[ox，oy，oz]T，a=[ax，ay，az]T分别为末端执行器在基坐标中的X轴方向，Y轴方向和Z轴方向，p=[px，py，pz]T示末端执行器相对于基坐标的位置关系.矩阵由各关节积累获得，表示从基坐标变换为末端经过的位置变换与姿态改变.将表1 中的各关节变量代入式（2），即可得机械臂的从基坐标到末端执行器的位姿改变.

2 仿真功能包配置

2.1 ROS 机器人操作系统

ROS 以Linux 系统的发行版Ubuntu 为基础，充分利用了该系统运行和管理的特点.为了提升机器人领域的研发效率，ROS 对机器人许多现已存在项目的代码进行了存储.在ROS 的库中能找到大部分复杂的代码，只有一小部分的代码用于展示ROS 库的各种功能并且创建了一些小的应用.ROS 提供许多公开的包，如创建地图包、定位算法包等，适用于各种软件及硬件[4].

ROS 集成了众多简单实用的工具以及先进的机器人算法，相较于其它系统有着许多鲜明的特点.它的中立性框架使得C++、Python、Octave 和Lisp 等编程语言都可以在ROS 系统中使用.再借由接口定义语言，使得ROS 能够利用交叉语言实现模块之间的消息传送.ROS 操作系统提供许多开源工具来编译和运行不同的ROS 组件，管理复杂的软件架构. 如机器人三维物理仿真工具Gazebo、机械臂运动规划工具MoveIt、可视化工具Rviz 等. ROS 系统是免费且开源的，这有利于ROS 软件层面的校正并可被用作多种用途.

2.2 URDF 文件

URDF（Unified Robot Description Format），是一种在ROS 中大量使用的特殊的xml 文件[5].它可以描述机器人各个关节的基本信息、连接关系等，确定机械臂模型的基坐标及各关节的坐标系，同时支持模型的干涉碰撞检查.本文使用SolidWorks 建立液压重载机械臂的模型，在不改变模型结构情况下对模型中螺栓等紧固零件进行简化以减少计算量，这对仿真精度的影响可以忽略.利用SW2urdf 插件，设置各个关节的运动类型和参数，然后将模型导入ROS 环境中生成可用的URDF 文件.

2.3 配置功能包

MoveIt! 是ROS 中进行机械臂避障规划的重要工具，由一系列移动操作的功能包组成了的运动规划库，可提供运动规划、操作控制、环境感知、正逆运动学求解等功能[6].使用MoveIt! 配置助手配置仿真所需的功能包流程如图3 所示.

图3 MoveIt! 配置流程图Fig.3 The flow chart of configuration process for MoveIt!

在配置添加运动链时，选用KDL（The Kinematics and Dynamics Library）即运动学与动力学的库来求解正逆运动学解.其优势在于解决串联机械臂运动学数值解的问题，可应用于机械臂正逆运动学解的情况[7].按照配置流程就可以生成机械臂避障仿真所需要的功能包.

3 机械臂避障仿真

3.1 避障规划算法

对于求解过程，有两种不同解决方案，一是寻找最优解，使用最优规划；二是快速寻找有效解，使用采样规划.机械臂的避障规划旨在解决存有障碍物情况下机械臂的路径选择问题.在满足要求标准的情况下，寻找从起始点至目标点的最优解. ROS 的OMPL 是一个用于运动规划的C++采样规划库，包含众多运动规划可用的C++算法，并能提供模块化的设计及稳定更新.本文使用采样规划库中中常见的RRT（Rapidly-exploring Random Trees）算法.

RRT 算法通过随机采样的方式，寻找从初始点到目标点的路径.该算法类似树状规划方式，以初始点为根节点，生成随机拓展树向外随机延伸[8]，当目标节点出现在随机树节点搜索区域时，可以沿随机树生成从根节点到目标节点的轨迹[9-12].完成整个规划过程，从该节点位置返回到初始节点连接成路径.

RRT 算法的搜索方式如图4 所示.

图4 RRT 算法示意图Fig.4 The schematic diagram of RRT algorithm

RRT 算法生成路径的过程为：选定初始点qinit，在此基础上随机出现一个新的节点qrand.然后，选定树节点中qrand 距离最近的节点qnearest.判定若qnearest 节点出现在目标位置范围内，得出结果.否则，在此基础上寻找新节点qnew，直到逼近目标节点.

算法表示如下：

BuildRRT（qinit，K，Δq）

1）T.init（qinit）

2）for k=1 to K

3）qrand=Sample（）

4）qnearest=Nearest（T，qrand）

5）if Distance（qnearest，qgoal）

6）return true

7）qnew=Extend（qnearest，qrand，Δq）

8）if qnew≠NULL then

9）T.AddNode（qnew）

10）return false

3.2 机械臂避障运动规划

启动MoveIt! 功能包中包含模型的launch 文件，进入Rviz 仿真界面，显示出由SolidWorks 构建并简化的机械臂模型.在仿真环境中，创建一个长方体作为障碍物，进行轨迹规划，并在Rviz 中发布到环境中.可设置特定的姿势作为初始位置和目标位置，也可通过拖动机械臂末端直接改变姿态设置初始和目标位置.选择RRT 算法，在当前环境开始规划，此过程会在终端中显示进程，且Rviz 仿真窗口会显示一条机械臂可行的避障路径.图5 为Rviz 仿真窗口，机械臂初始姿态置于障碍物右侧，目标姿态置于左侧.机械臂需要绕过竖在中间的长方体才能到达目标位姿，完成避障规划.图6 为机械臂避障轨迹.

图6 机械臂避障仿真轨迹Fig.6 The trajectory of obstacle avoidance simulation of manipulator

通过获取机械臂各关节位置变化的信息，得到机械臂在运动过程中各关节的位置随时间变化的曲线，该曲线记录了机械臂各关节在避障过程中实时的位置变化情况，如图7 所示.

图7 关节位置-时间曲线Fig.7 Joint position-time curve

4 结 论

本文对某液压重载机械臂进行了运动学分析，运用SolidWorks 建立了机械臂空间结构模型，利用ROS 平台中的URDF 文件建立仿真模型.通过配置MoveIt！工具，采用快速扩展随机树算法（RRT）对机械臂的运动路径进行规划，得到机械臂各关节位移随时间变化的曲线，实现了存在障碍物时机械臂的自动避障.本文采用的方法为在复杂障碍环境下进一步改进规划算法提供了直观便捷的方法.