基于ROS环境的大中型报废客车壳体拆解机械臂动力学模型分析及轨迹规划

李小虎，张 华，周 稳，姜 涛

(安徽科技学院 机械工程学院, 安徽 滁州 233100)

随着碳达峰时代的来临，大量的汽车、燃油公交车面临报废待拆解的境地。目前国内外对报废客车拆解的方式主要为氧切和暴力切割，但氧气切割对人伤害极大，且暴力拆解严重破坏了客车部分关键零部件的回收及使用。由于客车体积非常庞大、因此需将其进行切割成固定大小的矩形块，以方便其进行回收利用。故需对壳体进行切割运动规划，以减少在切割过程中所造成的空行程。所以现急需研发一种智能化绿色拆解手段，来解决当下报废客车的拆解难题[1-3]。

如今，随着智能化机器人的高速发展，对机器人的控制系统要求也愈来愈高。而目前市场上的机器人厂商都在开发各自的软件接口，故导致了市场上的机器人可移植性差，复杂的建模及对大量冗余代码的编写严重影响开发者的工作效率。而目前流行的机器人操作系统ROS(Robot opreating system)具有跨平台(Windows、Ubuntu)、多语言支持(Python、C++、Java等)、代码复用等优点，能够很好地解决此问题[4-5]。现采用高压水枪搭载至拆解机械臂上，使用ROS操作平台控制拆解机械臂对报废客车壳体进行切割运动规划，并在Moveit+rviz+gazebo进行仿真试验。

1 机械臂模型分析与功能包配置

由于报废客车体积大、车身重、含易燃物质，在拆解时不易移动，传统的机械切割与氧气在拆解时会产生火花，会引发火灾等危险。故采用高压水枪搭载至机械臂上，机械臂控制水枪并对报废客车壳体进行拆解，具体操作示意如图1所示。

图1 报废客车拆解示意

1.1 机械臂运动学建模过程

对机械臂进行运动学建模，主要是为了后续对其进行运动轨迹规划，而对于工业机械臂来说，其构造就是由许多连杆之间经过关节联结起来的运动链条，工业机械手臂末端执行机构和工业机械臂基座世界坐标系间通过各连杆上固接的一个连杆坐标系链接，这些连杆之间经过关节联结起来后，连杆坐标系间的相对关系就建立了。在SolidWorks三维建模软件中指定机器人每个连杆对应的三维模型和各关节的旋转轴等几何特征，如图2所示。并为每个关节定义指定运动范围及运动速度限制，形成链式结构。然后在ROS官网下载插件sw2urdf，该插件可将绘制的机器人模型转化为URDF(Unified Robot Description Format)文件，而URDF是一款专门用于描述机器人格式的XML文件，其代码主要定义link及joint。本文中使用的六轴开放源代码机械手臂Rocr6如图3所示。

图2 机械臂关节连杆

图3 Rocr6开源机械臂模型

Rviz三维可视化工具是基于MoveIt框架的插件,可以用来生成自动化机器人工作的场景并实现人机交互。在验证URDF模型语法和构造之后，可以启用joint_state_publisher节点，发送sensor_msgs/JointState信息，调用use_gui:=true监控机械手臂滑膜关节运动，检测机械手臂之间有无相互干涉和关节限位现象，对URDF文件进行设置和调整等[6-8]。

1.2 机械臂力学模型分析与功能包配置

机械臂连杆是一个非常复杂的动力学系统，其动力学方程具有非线性、强耦合和实变等特点。操作过程中，机械臂连杆的动力响应出现刚柔耦合现象，即臂杆在历经大范围转动的同时叠加了自身低频的弹性振动，以致于定位精度严重降低；因此，机械臂连杆系统的动力学分析是柔性机械臂实现工程应用必须要解决的关键技术之一。以下将从柔性系统中最为基础的单连杆机械臂模型及最为典型的双连杆机械臂模型进行分析，如图4～5所示。

(a) 单连杆位置关系

1.2.1 单连杆动力学模型

图4是机械臂单连杆示意图。当机械臂不是作单纯的平动而是作比较复杂的运动时，各关节的运动状态(速度与加速度)不相同。但总可以把物体看成质点组来分析、处理，即想象把物体分成许多的质元，在每一质元范围内，速度和加速度是相同的。于是，对于每个质元，按牛顿第二定律可建立运动方程，各关节质心及速度表达式如式(1)～(2)所示，即

m1c(0 0)，m2c(C2cosθ1C2sinθ2)；

(1)

(2)

式中：m1、m2分别为两质点的质量；c1、c2分别为第一个质点与坐标原点的距离及第一个质点与第二个质点的距离。

根据拉格朗日公式可建立关节控制扭矩τ及拉格朗日函数L之间的关系，即

(3)

根据系统动能可知

T=T1+T2，

(4)

式中：T1是关节1总动能；T2是关节2总动能；JZ1是关节1转动惯量；JZ2是连杆的转动惯量；m2是连杆的总质量；C2是从关节轴到连杆质量中心的距离；ω是其相应角速度。

1.2.2 双连杆动力学模型

图5是双连杆机械臂示意图，各关节质心和速度表达式如式(5)～(6)所示，即

m1c(0 0)、m2c(C2cosθ1C2sinθ1)、m3c(C3cosθ1C3sinθ1)、m4c(C3cosθ1+C4cos(θ1+θ2)C3sinθ1+C4sin(θ1+θ2))，

(5)

(6)

(a) 双连杆位置关系

转动惯量如图6所示。

(a) 中心轴线转动

m1、m3视作空心圆柱体(绕其中的轴线转动)，空心圆柱体围绕中心轴线转动(见图6(a))时的转动惯量Jz可表示为

(7)

m2、m4视作空心圆柱体，空心圆柱体的转动轴为中心直径时(见图6(b))，它的转动惯量Jz可表示为

(8)

根据拉格朗日公式建立关节控制扭矩τ及拉格朗日函数L之间的关系式，即

(9)

系统动能为

(10)

1.3 机械臂功能包配置

由于机械臂目前只具备模型结构，故在获取URDF文件后，还需对其进行功能包配置以实现其控制功能。本文在RVIZ环境下对其进行配置，使用roslaunch moveit_setup_assistant setup_assistant.launch命令来对机械臂进行配置，在对Self_Collisions进行配置时，需要使用Generate Collision Matrix插件进行碰撞检测，如图7a所示；生成碰撞矩阵，以确保在规划过程中关节不会相互碰撞，若在检测过程中发生碰撞再次进行调试直至不发生碰撞，如图7b所示。在RVIZ环境下配置完成之后会生成一系列文件，如图7c所示[9]。

(a) 机械臂基本配置

2 ROS控制系统设计

2.1 Moveit控制架构设计

传统机械臂需要通过各个厂家定义的不同编程语言来控制机械臂，需要逐个动作进行示教编程，使用起来不方便。而Moveit是一种任务型的编程，比如让机械臂到达某个目标点，只需给机械臂一个指令，达到某个目标点即可。而中间的避障环节以及外部视觉的集成是由Moveit自动帮助使用者完成，使得机器人的交互变得更加友好[9]。

ROS机械臂控制系统基于Moveit架构及其插件，move_group是Moveit的核心节点，相当于一个功能集散地的中心，负责所有功能的调用。其上位机的控制可以通过C++、python这种编程式的方法，也可以通过RVIZ(三维可视化空间)进行机械臂的拖拽以及命令行的方法进行人机交互。move_group随之会接受这些命令，然后告诉机械臂从当前位置到达指定位置。一般的轨迹规划算法包括CHOMP、OMPL、SBPL，而在ROS中默认的算法是OMPL。对于机械臂运动过程中的避障问题，一般有FCL和PCD算法来解决，如图8所示[6-7]。

图8 Moveit流程设计

2.2 Moveit算法封装及功能

具体运动规划算法是由Moveit提供的Moveit_core package标准协议接口planning_interface.h进行封装。其中包含3个重要角色PlannerConfigurationSettings、PlanningContext、PlannerMange需要实现：PlannerConfigurationSettings功能是配置运动规划插件中算法所需要的参数，PlanningContext功能是调用具体算法的位置及规划算法的具体实现，PlannerManger功能是向Moveit注册插件并告诉Moveit此插件的功能，如图9所示。在对PlanningContext进行继承时，需要对其子类功能进行实现。需要实现的4个功能函数包含2个solve函数(MotionPlanResponse和MotionPlanDetailedResponse)，目的是解决不同输入条件下的轨迹规划算法调用功能，terminate函数解决规划过程中服务停止问题，clear函数解决轨迹规划结束后将数据清理问题。

图9 move_group算法封装

3 机械臂运动规划

3.1 机械臂添加控制器插件

由于机械臂的功能要求多，自由度大，而且对运动精度和响应速度的要求较高，因此会造成计算量大、周期短等特点。为此，ROS提供了一系列的控制器接口、硬件接口、传动装置接口、控制器工具箱等，能够高效地提高开发者地工作效率。文中在对Rocr6机械臂进行控制时会加入arbotix插件。先对launch启动文件夹加载arbotix启动节点,然后在config文件夹中修改joint_limits机械臂的关节位置、速度和加速度信息,并在ros_controller.yaml文件夹中编辑配置节点的发布状态和频率信息[10]，如图10所示。

图10 机械臂控制器插件

3.2 机械臂运动规划结果

RVIZ+GAZEBO联合仿真如图11所示，初始位置设为home，机械臂目标位置设为d30，机械臂通过OMPL算法自动到达d30点。通过录制joint_states话题，生成bag文件包，该文件包包括仿真时机械臂在进行运动规划时的关节状态。然后使用clock工具生成时间变化曲线，得到如图12所示的关节变化曲线。结果表明，整个机械臂在运动过程中可以实现平滑的轨迹过渡，从而避免机器震动引发故障。在对报废客车壳体拆解时根据实际切割要求，可直接利用ROS提供的语言接口载入具体的直线、圆弧轨迹规划及相关轨迹插补。

图11 机械臂运动过程

图12 机械臂各关节角度随时间变化曲线

4 结语

通过在ROS环境下对开源机械臂Roc6进行动力学建模及分析，然后使用Moveit对机械臂进行配置启动并使用其运动规划算法插件对其算法进行封装，调用OMPL算法对机械臂进行轨迹运动规划，最后使用rqt_plot插件对机械臂进行轨迹规划分析。试验中对开源机械臂Rocr6进行了碰撞检测，各关节的link、运动学建模、功能包配置，设计了机械臂运动规划程序执行任务、rqt_plot可视化分析关节变化曲线。通过上述一系列工作流程，在ROS中实现了机械臂对报废客车壳体切割运动规划，全程符合运动规划期望。该方法为报废客车壳体拆解提供技术支持，可高效智能地对报废客车壳体进行拆解，同时也对工业机械臂在ROS下的开发应用有重要借鉴意义。