交通锥自动回收及摆放装置中的机械臂运动学分析

Kinematics Analysis of a Robotic Arm in a Traffic Cone Automatic Recovery and Placement Device

摘要: 设计了一款交通锥自动回收及摆放的装置，该装置具有自动回收以及摆放交通锥的功能。抓取装置中交通锥的机械臂是重要组成部分，对机械臂进行运动学分析，在Adams中仿真分析机械臂末端执行器的轨迹路线，基于Matlab的蒙特卡罗法模拟机械臂可达工作空间，验证机械臂抓取可行性。

Abstract: A device for automatic recovery and placement of traffic cones has been designed. The device has the functions of automatic recovery and placement of traffic cones. The mechanical arm of the traffic cone in the gripper is an important part. The kinematics analysis of the manipulator is performed, and the trajectory of the end effect or of the manipulator is simulated and analyzed in Adams. The Monte Carlo method based on Matlab simulates the reach of the manipulator to the working space, and verifies the feasibility of the manipulator grasping.

1. 引言

随着汽车等代步工具的快速发展，高速公路及马路上的负担越来越重，人工养护 [1] 将投入更大的人力，而工人们在投放和回收交通锥的时候所面临的交通风险也越来越大 [2]。目前，国内摆放交通锥的方式有：人工摆放交通锥；在工程车的尾部摆放一个半自动化的装置经由人工摆放交通锥；在工程车行驶过程中让工作人员用自己的双手进行摆放和回收交通锥。在交通管制的时候需要在管制路段摆放交通锥来警示过往的行人及车辆，而摆放交通锥会花费大量的人力 [3]，这些通过人工手动摆放交通锥的方法首先会造成工作人员的人身安全难以保障，其次凭借工作人员经验摆放交通锥会造成交通锥摆放不整齐的问题，当车流量大的时候这类问题尤为突出。因此，社会上急需一种能够自动摆放和回收交通锥的装置 [4]。

2010年8月，山东省潍坊市的刘庆亮先生发明了“一种用于高速公路快速摆放锥形标的摆放机”。施工人员只需要在滑板上放交通锥让交通锥自动滑到指定位置，这项发明类似一个半自动化的装置，将其放在工程车的尾部，然而其摆放交通锥的整齐度难以保障。

2018年北京道路运输展期间，北京市首都公路发展集团有限公司展出了国内首款交通锥半自动化收放存储专用车,该装置只需要两个驾驶人员在车厢内进行操作 [5]，可以达到回收以及摆放交通锥的目的，基本实现了自动化。

上述摆放交通锥的装置仍然需要两个及以上的人来进行人工操作。与需要人工摆放的半自动化装置相比，能够实现自动回收及摆放交通锥的装置显然更具优势。本文设计出一种小型可通过货车运载的道路交通锥自动回收及摆放的装置。对装置中关键的机械臂进行理论分析，分析机械臂易出现危险截面的部分，以D-H齐次线性坐标法建立简图，运用运动学知识对机械臂进行运动学分析 [6] [7] [8]，运用Adams仿真软件对机械臂抓手的轨迹进行仿真验证，最后基于Matlab用蒙特卡罗法模拟机械臂抓手可达空间。

2. 工作原理

阶段一：如图1(a)所示，机械臂利用滚珠丝杆升降机构升到指定高度，经过旋转和直线运动后所述机械臂的机械手到达指定位置，所述机械臂机械手外钩钩住了专用交通锥最外节的槽口。

阶段二：如图1(b)所示，机械手外钩钩住交通锥最外节的槽口，在所述机械臂滚珠丝杆升降机构的升降、所述机械臂大臂和小臂的旋转作用下位移到将要摆放的位置上方。

阶段三：如图1(c)所示，所述机械臂机械手外钩钩住交通锥最外节的槽口，所述机械臂滚珠丝杆升降机构向下降低，所述机械臂大臂同时向下摆动直到所述机械臂机械手完全将专用交通锥摆放到达指定地面。

阶段四：如图1(d)所示，所述机械臂机械手将专用交通锥摆放到地面之后，所述机械臂机械手内钩抓住专用交通锥交通锥环，滚珠丝杆升降机构上升，同时所述机械臂大臂向上摆动，直到所述机械臂机械手到达专用交通锥完全伸展状态时的高度，在这个过程中专用交通锥由于加重底盘的重力原因受到向下的力，达到伸展状态，当专用交通锥处于完全伸展状态时所述机械臂机械手内钩松开。这时一个专用交通锥的摆放工作完成。

Figure 1. Device work flow chart

图1. 所述装置工作流程图

2. 整体结构设计

2.1. 收集箱

收集箱长800 mm、宽600 mm、高610 mm。收集箱内部设计，包含四十个仿“鱼竿”式交通锥、机械臂和定位柱。收集箱立体示意图如下图2所示，内部布局图如下图3所示。

Figure 2. Collection box three-dimensional schematic diagram

图2. 收集箱立体示意图

Figure 3. Collection box internal layout

图3. 收集箱内部布局图

2.2. 交通锥

实现所述装置小型化，使装置易于运载携带。目前，交通锥的设计向着稳定型、缓冲型、多功能型以及提高警示效果型发展 [9]。本文设计一款可以伸缩的仿“鱼竿”式交通锥(如图4)，所述专用交通锥处于收缩状态时高度为6 cm (详见表1)，所述仿“鱼竿”式交通锥包含11节交通锥节；所述交通锥最外节包含加重的交通锥底盘，其中，在该节壁上开槽口(图5)，交通锥加重底盘起拉伸交通锥的作用，所述机械臂机械手外钩钩住槽口处；所述交通锥的顶部包含一个环(图6)，由机械臂的机械手内钩抓取环处；由此可以在收集箱横向摆2摞，纵向摆3摞，一共5摞，每摞8个交通锥，这样总数可以达40个交通锥。

2.3. 整体装配

运用Solidworks对所述装置装配(本文主要研究所述装置回收及摆放交通锥，所以其一体的小车只是设计了简易的模型)，整体装配图如图7所示。整个装置体积小于1 m³，方便运载。

Table 1. Parameters of traffic cone cone diameter

表1. 交通锥锥节直径参数表

(a) (b)

Figure 4. Three-dimensional schematic view of traffic cone. ((a) shows the stretched state, (b) shows the compressed state)

图4. 交通锥立体示意图((a) 图为伸展状态，(b) 图为压缩状态)

Figure 5. Enlarged view of the outermost section of the traffic cone (chassis)

图5. 交通锥最外节(底盘)放大图

Figure 6. Top view of traffic cone

图6. 交通锥顶部节示意图

Figure 7. Three-dimensional schematic diagram of automatic recovery and placement device of traffic cone

图7. 交通锥自动回收及摆放装置立体示意图

3. 机械臂设计

3.1. 机械臂结构设计

3.1.1. 机械臂结构设计

本文设计了一款3自由度机械臂 [10]，2个旋转自由度是连接滚珠丝杆和大臂之间的旋转自由度以及大臂和小臂之间的旋转自由度；所述机械臂的滑动自由度是连接大臂和滚珠丝杆之间的滑动自由度。机械臂的具体参数如下表2所示：

Table 2. Manipulator parameter table

表2. 机械臂参数表

使用solidworks制图软件建立机械臂三维模型图(见下图8)。

Figure 8. Schematic diagram of the mechanical arm

图8. 机械臂立体示意图

机械臂的工作方式如下(图9)。

Figure 9. Flow chart of robotic arm operation

图9. 机械臂运作流程图

3.1.2. 机械臂机械手指设计

为了能够满足抓取交通锥环以及交通锥末节槽口，机械手指的形状如图10所示，抓手材料为304不锈钢，手指外钩长度为8 cm，与内钩的夹角为30˚。本次设计的专用交通锥一个的重量约为2 KG，所以单侧手指在a点处承受的压力约为10 N。手指部分最容易出现危险截面，对该手指进行有限元分析，分析结果如图11。手指材料选用304不锈钢，304不锈钢的最大屈服应力为2.068e+008 N/m²，手指危险截面段即红色部分的屈服应力为1.579e+008 N/m²，显然手指部分选用的材料符合设计要求。

Figure 10. Mechanical finger

图10. 机械手指

Figure 11. Mechanical finger stress diagram

图11. 机械手指应力图

3.2. 运动学分析

3.2.1. D-H坐标法

D-H法 [11] 实际上就是将杆与杆之间通过数学表达式关联起来，通过建立D-H坐标系得到各个运动杆件之间的坐标关系，通过引入一系列的规则来确定固定于机器人各个连杆上的坐标系的位姿，然后构造相邻坐标系的齐次变换。通过几何学理论可以得到：空间中的两条直线m和n互为异面，m可以绕x轴旋转θ，然后绕z轴旋转α，最后在Z轴、X轴分别平移a、d，使直线m和直线l重合。用矩阵表示如下：

绕Z轴旋转θ： $G_1=\left[\begin{array}{cccc}\cos \theta & -\sin \theta & 0& 0\\ \sin \theta & \cos \theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ (1)

绕X轴旋转α： $G_2=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& \cos \alpha & -\sin \alpha & 0\\ 0& \sin \alpha & \cos \alpha & 0\\ 0& 0& 0& 1\end{array}\right]$ (2)

沿Z轴平移d： $G_3=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d\\ 0& 0& 0& 1\end{array}\right]$ (3)

沿X轴平移a： $G_4=\left[\begin{array}{cccc}1& 0& 0& a\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ (4)

可以用矩阵 $G^{·}=G_1{G}_2{G}_3{G}_4$，来表示相邻两杆之间的关系，得到末端执行器相对坐标系原点的位姿 [12] ：

${}_n{}^{m}T=G^{•}{G}^{••}$ (5)

由式(1)、(2)、(3)、(4)可得末端执行器的位姿矩阵为：

${}_n{}^{n-1}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos \theta & -\sin \theta \cos \alpha & \sin \theta \sin \alpha & a\cos \theta \\ \sin \theta & \cos \theta \cos \alpha & -\cos \theta \sin \alpha & a\sin \theta \\ 0& \sin \alpha & \cos \alpha & d\\ 0& 0& 0& 1\end{array}\right]$ (6)

3.2.2. 运动学分析

根据机器人机构学理论 [13] [14] [15] [16] 以及D-H法建立机械臂机构简图(图12)：

Figure 12. D-H mechanical arm coordinate system diagram

图12. D-H机械臂坐标系机构简图

D-H参数表如下表3所示：

Table 3. D-H parameter table

表3. D-H参数表

3.2.3. 正运动学分析

将表中关节序号1的参数带入式(6)中可得：

${}_n{}^{n-1}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos {\theta }_1& -\sin {\theta }_1\cos 90& \sin {\theta }_1\sin 90& l_0\cos {\theta }_1\\ \sin {\theta }_1& \cos {\theta }_1\cos 90& -\cos {\theta }_1\sin 90& l_0\sin {\theta }_1\\ 0& \sin 90& \cos 90& 0\\ 0& 0& 0& 1\end{array}\right]$ (7)

由式(7)可得坐标系(X₀Y₀Z₀)到坐标系(X₁Y₁Z₁)的变化矩阵：

${}_1{}^{0}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos {\theta }_1& 0& \sin {\theta }_1& l_0\cos {\theta }_1\\ \sin {\theta }_1& 0& -\cos {\theta }_1& l_0\sin {\theta }_1\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$ (8)

将表中关节序号2的参数带入式(6)中可得：

${}_n{}^{n-1}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos {\theta }_2& -\sin {\theta }_2\cos 0& \sin {\theta }_2\sin 0& l_1\cos {\theta }_2\\ \sin {\theta }_2& \cos {\theta }_2\cos 0& -\cos {\theta }_2\sin 0& l_1\sin {\theta }_2\\ 0& \sin 0& \cos 0& 0\\ 0& 0& 0& 1\end{array}\right]$ (9)

由式(9)可得坐标系(X₁Y₁Z₁)到坐标系(X₂Y₂Z₂)的变化矩阵：

${}_2{}^{1}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos {\theta }_2& -\sin {\theta }_2& 0& l_1\cos {\theta }_2\\ \sin {\theta }_2& \cos {\theta }_2& 0& l_1\sin {\theta }_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ (10)

将表中关节序号3的参数带入式(6)中可得：

${}_3{}^{2}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos {\theta }_3& -\sin {\theta }_3\cos 0& \sin {\theta }_3\sin 0& l_2\cos {\theta }_3\\ \sin {\theta }_3& \cos {\theta }_3\cos 0& -\cos {\theta }_3\sin 0& l_2\sin {\theta }_3\\ 0& \sin 0& \cos 0& 0\\ 0& 0& 0& 1\end{array}\right]$ (11)

由式(11)可得坐标系(X₂Y₂Z₂)到坐标系(X₃Y₃Z₃)的变化矩阵：

${}_3{}^{2}G=G_1{G}_2{G}_3{G}_4=\left[\begin{array}{cccc}\cos {\theta }_3& -\sin {\theta }_3& 0& l_2\cos {\theta }_3\\ \sin {\theta }_3& \cos {\theta }_3& 0& l_2\sin {\theta }_3\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ (12)

由式(5)可得末端执行器相对坐标系原点的位姿，在此，令，令 $\cos {\theta }_i=c_i$ (其中 $i=1,2,3$ )：

${}_3{}^{1}G={}_1{}^{0}G{}_2{}^{1}G{}_3{}^{2}G=\left[\begin{array}{cccc}•& •& •& l_{2}c{\theta }_{1}c{\theta }_{2}c{\theta }_3-l_{2}c{\theta }_{1}s{\theta }_{2}s{\theta }_3+l_{1}c{\theta }_{1}c{\theta }_2+l_{0}c{\theta }_1\\ •& •& •& l_{2}s{\theta }_{1}c{\theta }_{2}c{\theta }_3-l_{2}s{\theta }_{1}s{\theta }_{2}s{\theta }_3+l_{1}s{\theta }_{1}c{\theta }_2+l_{0}s{\theta }_1\\ •& •& 0& l_{2}s{\theta }_{2}c{\theta }_3+l_{2}c{\theta }_{2}s{\theta }_3+l_{1}s{\theta }_2\\ 0& 0& 0& 1\end{array}\right]$ (13)

由式(13)可得到交通锥自动回收及摆放装置机械臂的末端执行器位置的位姿坐标，即交通锥自动回收及摆放装置机械臂的运动学正解：

$\left[\begin{array}{c}{P}_{x3}\\ P_{y3}\\ P_{z3}\end{array}\right]=\left[\begin{array}{c}{l}_{2}c{\theta }_{1}c{\theta }_{2}c{\theta }_3-l_{2}c{\theta }_{1}s{\theta }_{2}s{\theta }_3+l_{1}c{\theta }_{1}c{\theta }_2+l_{0}c{\theta }_1\\ l_{2}s{\theta }_{1}c{\theta }_{2}c{\theta }_3-l_{2}s{\theta }_{1}s{\theta }_{2}s{\theta }_3+l_{1}s{\theta }_{1}c{\theta }_2+l_{0}s{\theta }_1\\ l_{2}s{\theta }_{2}c{\theta }_3+l_{2}c{\theta }_{2}s{\theta }_3+l_{1}s{\theta }_2\end{array}\right]$ (14)

3.2.4. 逆运动学分析

逆运动学分析与正运动学相反 [17]，逆运动学分析即在已知机械臂末端执行机构的位置，通过机械臂关节以及连杆之间的几何关系来求出机械臂各个关节处的转动角度。逆运动学分析可以通过解析法以及数值法来求解，具体的步骤 [18] 可以参考下表4：

Table 4. Inverse kinematic analysis methods [18]

表4. 逆运动学分析方法 [18]

由式(14)可得：

$\{\begin{array}{l}{P}_{x3}=l_{2}c{\theta }_{1}c{\theta }_{2}c{\theta }_3-l_{2}c{\theta }_{1}s{\theta }_{2}s{\theta }_3+l_{1}c{\theta }_{1}c{\theta }_2+l_{0}c{\theta }_1\\ P_{y3}=l_{2}s{\theta }_{1}c{\theta }_{2}c{\theta }_3-l_{2}s{\theta }_{1}s{\theta }_{2}s{\theta }_3+l_{1}s{\theta }_{1}c{\theta }_2+l_{0}s{\theta }_1\\ P_{z3}=l_{2}s{\theta }_{2}c{\theta }_3+l_{2}c{\theta }_{2}s{\theta }_3+l_{1}s{\theta }_2\end{array}$ (15)

其中，关节序号3处是末端执行器抓手，所以θ₃是已知值，要求的参数中P_x3、P_y3、P_z3、l₁和l₂是已知的,l₀是滚珠丝杆升降机的滑动长度。在此，令 $\sin {\theta }_i=s_i$，令(其中 $i=1,2,3$ )。

式(15)中第三式只有一个未知数θ₂，即可求出θ₂的值。式(15)中第一式除以第二式可得θ₁：

${\theta }_1=\arctan \frac{P_{y3}}{P_{x3}}$ (16)

由式(15)中的第一式与第二式相加变化后可得l₀：

$l_0=\frac{P_{x3}+P_{y3}-\left(c_1+s_1\right)\left(l_2{c}_2{c}_3-l_2{s}_2{s}_3+l_1\right)}{c_1+s_1}$ (17)

通过运动学逆解解得机械臂抓手在抓取交通锥时的关节转角大小。

3.3. 小结

本节对整个机械臂容易出现危险截面的手指部分进行了有限元分析，验证了可行性。通过对机械臂的逆运动学分析可以求出机械臂末端执行器到达各个交通锥抓取点时关节转角的大小，验证机械臂末端执行器可以到达各个抓取点以及摆放点的位置，为之后机械臂仿真分析提供理论依据。

4. 机械臂仿真分析

4.1. 机械臂运动轨迹

将在Solidworks中建模完成后的机械臂保存为x_t格式文件，导入到Adams软件中，添加3个转动副和1个移动副，获取目标执行器坐标(交通锥在收集箱内的坐标位置)，将目标执行器坐标代入运动学逆解中求得各个关节处的转角。在Adams中设置各个关节转动副的转动方向，Adams中旋转副的转动方向遵循右手定则，利用 $Step\left(x,x_0,h_0,x_1,h_1\right)$ 函数来控制仿真，其中x是时间的函数，x₀——自变量是Step函数的开始值，h₀是Step函数的初始值，x₁——自变量是Step函数的结束值，h₁是Step函数的最终值。得到机械臂在抓取收集箱中的交通锥时的轨迹，如图13所示，其中选取的轨迹参数如下： ${\theta }_1={75}^{\circ }$， ${\theta }_2={180}^{\circ }$， $l_0=250\text{mm}$，图13中可以看出机械臂抓手可以沿着平滑路线抓取交通锥。

Figure 13. Trajectory map of a mechanical gripper as it grabs a traffic cone

图13. 机械抓手抓取交通锥时的轨迹图

Figure 14. Space reach of gripper

图14. 机械臂抓手可达空间

4.2. 基于Matlab运用蒙特卡罗法模拟机械臂抓手可达空间

蒙特卡罗法 [19] 又称为随机抽样或者统计试验方法，它属于数学计算的一种方法。通过对机械臂实际工作环境的分析，结合第三章对机械臂的正运动学分析，选取关节旋转角度θ₁、θ₂的范围分别是0˚~160˚和0˚~240˚；连杆l₀为滚珠丝杆滑轨长度，取20 mm~460 mm；连杆l₁、l₂长度为机械臂大臂、小臂长度360 mm、360 mm。在角度范围内选取100000个值代入第三章求出的运动学正解中，求出末端执行器位姿坐标，绘制空间散点图，即机械臂抓手可达空间图14。

收集箱的长800 mm、宽600 mm、高610 mm，实际交通锥在收集箱内的矩形分布空间会小于收集箱的矩形空间从图14中可见机械臂抓手可达空间可以完全覆盖收集箱的内部空间。

5. 总结

本文提出了一种用于危险路段的交通锥自动回收及摆放装置，构思设计了一种仿“鱼竿”式交通锥以及设计了一个2轴3自由度的机械臂。对机械臂机械手指进行有限元分析，验证机械臂抓手危险截面处符合设计要求。在机器人机构学以及数学理论的支撑下，建立D-H坐标系，对所述装置的机械臂进行正运动学分析和逆运动学分析。通过求解逆运动学解得到关节角，代入Adams中仿真，规划出机械臂抓手抓取交通锥时的轨迹，在Matlab中用蒙特卡罗法模拟绘制了机械臂抓手可达空间图，验证了机械臂装置的抓取可行性。该设计为道路交通锥的回收以及摆放装置提供了一种新型的方案，为之后的研究奠定了基础。