工业机器人空间曲线实时轨迹规划算法

到关节空间的函数, ω4 (t ) = (t − t0 )(t − t1 )(t − t2 )(t − t3 ) 。
4 编程实现及结果分析(Programming and Analysis)
通过 matlab 软件编写了以上控制算法， 程序对输入的目标点或空间曲线的参数方程就可以自动 生成机器人可以执行的各关节增量指令。设定机器人实际参数 b=255，c=255，d=65。给出一空间 圆的方程 L 的参数方程为：
θ (t ) = a0 + a t + a1t 2 + a3t 3
1
(16) (17)
角度约束位： ⎨
⎧ ⎪θ (0) = θ 0 ， ⎪ ⎩θ (t f ) = θ f
& & ⎧ ⎪θ (0) = θ 0 && && ⎪ ⎩θ (t f ) = θ f
速度约束位： ⎨
(18)
解上面的方程可得多项式系数为：
1
引言（Introduction）
机器人轨迹规划是使机器人在规定时间内，按一定的速度及加速度，从初始状态移动到目标状 态。本文首先用 D-H 法建立了运动学模型，并用酉矩阵法对其进行了求解，得出了机械臂逆运动学 解。使空间曲线转换到机器人关节空间的运动。在机器人关节空间使用多项式插值法使机械臂各关 节运动的速度和加速度连续，保证生成的各关节电机执行的角度增量指令符合物理规律。最后使用 Matlab 对算法进行编程实现，从而验证了文章所设计算法的正确性。
⎧a0 = θ 0 ⎪ & ⎪a1 = θ 0 ⎪ 3 2 & 1 & ⎪ ⎨a2 = 2 (θ f − θ 0 ) − θ 0 − θ f tf tf tf ⎪ ⎪ 2 1 & & ⎪a3 = − 3 (θ f − θ 0 ) + (θ 0 +θ f ) tf tf ⎪ ⎩ 然后等时间的取 t( 0 < t < t f 这里设 t f = 1 )代入多项式：
解得： 其中：
(6) (7) (8 ) (9)
sin(θ 23 + w) = B ⇒
θ 23 = arcsin B − w;θ 23 = π − arcsin B − w
c1 px + s1 p y − d (c1ax + s1a y ) ⎧ ⎪ tan w = pz − daz ⎨ ⎪ B = (bc ) ÷ (c p + s p − dc a − ds a ) 2 + ( p − pa ) 2 3 1 x 1 y 1 x 1 y z z ⎩
（2）求 θ 3 。
⎡n ⎢ x n 1 θ ）A −1(θ ) A −1(θ ) ⎢ y A −（ ⎢ 3 3 2 2 1 1 ⎢n ⎢ z ⎢0 ⎣ o o o x y a a a x y p ⎤ x⎥ p ⎥ y = A (θ ) A (θ ) A (θ ) ⎥ 4 4 5 5 6 6 p ⎥ z⎥ 1 ⎦ ⎥
2
机器人模型(Robot kinematics model)
对一个六轴的串联机械臂，如图 1 所示。
图 1 机器人连杆坐标系 Fig.1 Robot link coordinate system [1] 根据D-H法得到机械臂位姿的变换矩阵 ：
基金颁发部门：国家自然科学基金委；项目名称：焊接机器人柔性系统 petri 网建模；编号：50705041 基金申 请人: 马国红。 基金颁发部门：江西省教育厅项目基金；项目名称：基于机器人汽车薄板焊接的旋转电弧传感器研究；编号： No. [2007]8 基金申请人: 马国红。
5 结论(Conclusion)
本文所设计的六轴机械臂的控制算法使机器人对输入空间曲线，计算出各关节的电机控制命令 使机器人沿曲线运动。 同时计算公式中各参数用符号表示， 能够运用到各种类似结构的机器人控制。 控制算法中多项式插值法使机械臂各关节运动的速度和加速度连续，避免运行生成的机器指令违背 物理规律使不能正确执行任务。本方法计算效率很高，100 个点能在 2s 内计算完成。计算公式是单 变量参数方程的形式，在运动的同时计算后面几点，能适应实时控制的要求。 本文作者创新点用酉矩阵法求解出机械臂逆运动学解析解，使机械臂控制器计算过程由解方程 组简化为代数运算。对于相同结构机械臂，只要代入实际机构参数变可使用本算法实时控制机械臂 运动。
(19)
θ (t ) = a0 + a t + a1t 2 + a3t 3
1
(20)
http://www.elecfans.com 电子发烧友 http://bbs.elecfans.com 电子技术论坛
生成机器指令 ΦΔ =(θ1Δ,θ2Δ,θ3Δ,θ4Δ,θ5Δ,θ6Δ)。其误差为 Rx =
f (3) (ξ ) 2 ω4 (t ) ，其中 f ( x) 为空间路径转换 8!
ψ i =ψ 0 +
ti ， T θ −θ θi = θ 0 + 1 0 ti ， T ϕ1 − ϕ0 ϕi = ϕ 0 + ti T
ψ 1 −ψ 0
(15)
把姿态角和相映的位置值代入文章中求出的机械臂逆运动学方程，就可以得出关节空间下的 θ1 ,L ,θ 6 值。同时注意这些 θ1 ,L ,θ 6 值可能会有多解，由机构的奇异问题产生的无穷个解，或无解 （不可达）。对于多解在我们的程序中会自动选择离上一个解最近的解，保证机械臂各个旋转轴运 动连续。 在关节空间生成机器指令 ΦΔ =(θ1Δ,θ2Δ,θ3Δ,θ4Δ,θ5Δ,θ6Δ)。考虑到机械臂运动的实际情况，要求运动， 速度和加速度都是连续的，否则就和物理规律违背不能精确运动到指定的空间点。如果长时间这样 [4,5] 操作，机械臂就容易损坏 ，所以我们要在关节空间中插值。采用三次多项式插值，其表达式位：
s23 (c1ox + s1o y ) − c23oz = s5 s6 ⇒ sin θ 6 = s23 (c1ox + s1o y ) − c23oz sin θ5 s23 (c1ox + s1o y ) − c23oz
(13)
θ 6 = arcsin
sin θ5
或
θ 6 = π − arcsin
s23 (c1ox + s1o y ) − c23oz
px ⎤ py ⎥ ⎥ pz ⎥ ⎥ 1⎦
(1)
为了使机械臂处于期望的位姿，需要求解其逆运动学解才能确定每个关节变量的值。我们可以 注意到，运动方程中有许多角度的耦合，这就使得无法从矩阵中取出足够的元素来求解单个的正弦 和余弦项以计算角度。逆解方程组是一个关于6个关节变量的由12个方程组成的非线性方程组，采 [2] 用酉矩阵法求解 ： （1）求 θ1 。可用逆变换 A1 (θ1 ) 左乘运动方程式两边：
http://www.elecfans.com 电子发烧友 http://bbs.elecfans.com 电子技术论坛
⎡ nx ox a x ⎢n o a y y 0 T6 = A1 (θ1 ) A2 (θ 2 ) A3 (θ3 ) A4 (θ 4 ) A5 (θ5 ) A6 (θ 6 ) = ⎢ y ⎢ nz oz a z ⎢ ⎣0 0 0 式中 An 矩阵为根据D-H法建立的机械臂各个关节的变换矩阵。
5 cos α 6 z = −17.5 + 87.5sinα (−1800 ≤ α ≤ 1800 ) x = −35 + 175sin α
y = 210 +
(21)
给出实际约束:Z 轴始终指向实体内部。代入逆运动学方程求出各个关节运动角度，各个关节的 角度变化如图 2 所示。
图 2 机械臂各关节运动角度变化曲线 Fig. 2 joint movement perspective curve 通过程序计算，可以得出机器人控制命令。图 2 显示了在沿空间曲线运动的时候各关节运动平 滑。从而说明了本文所用方法的正确性。
Biblioteka Baidusin θ5
(14)
3 机器人运动控制(Robot Motion Control)
对于预先指定的路径曲线我们先要把它离散化，然后根据一定的路径约束和实际情况的约束给 出通过这些点时机械臂的姿态 ，代入逆运动学方程求出通过这些点时的 θ1 ,L , θ 6 ，最后在关节空
[3]
间进行插补给出机器指令。 设在连续轨迹的起始点和终点机器人的姿态为 (ψ 0 , θ 0 , ϕ0 ) 和 (ψ 1 , θ1 , ϕ1 ) ，机器人运动连续平 滑，姿态角度随时间做线性变化。在时刻 ti (t0 < ti < t ) 手部姿态角为：
http://www.elecfans.com 电子发烧友 http://bbs.elecfans.com 电子技术论坛
工业机器人空间曲线实时轨迹规划算法
汪鎏，马国红，张华 （南昌大学机器人研究所，江西南昌 330031） 摘要：本文设计了一种六轴机械臂沿空间曲线行走的控制算法。算法使机器人对输入空间曲线 计算出各关节的电机控制命令使机器人沿曲线运动。算法中推导出机器人逆运动学各关节解析解， 使机械臂控制器计算过程由解方程组简化为代数运算。对类似结构的机械臂都可以使用，具有一定 的通用性。使用 Matlab 对算法进行编程实现，从而验证了文章所设计算法的正确性。 关键词：机器人；控制算法；轨迹规划；运动学 中图分类号：TP242.2 文献标识码：A
由 s1ax − c1a y = S 4 S ，可以解得：
(11)
s4 =
s1ax − c1a y s5
⇒ θ 4 = arcsin
s1ax − c1a y s5
; 或 θ 4 = π − arcsin
s1ax − c1a y s5
(12)
（6）求 θ 6
http://www.elecfans.com 电子发烧友 http://bbs.elecfans.com 电子技术论坛
(4)
z 0
z 0
解得:
1 θ3 = arcsin a + arctan 或 θ 3 = π − arcsin a + arctan
（3）求 θ 2
6
1 6
(5)
c23 (c1ax + s1a y ) + s23az = c4 s5
c23 (c1 px + s1 p y ) + s23 pz − (cc3 ) = dc4 s5
−1
A−1(θ ) 0T = A (θ ) A (θ ) A (θ ) A (θ ) A (θ ) 1 1 6 2 2 3 3 4 4 5 5 6 6
解得:
(2)
θ1 = arctan
p y − da y px − dax
或 θ1 = π + arctan
p y − da y px − dax
(3)
θ 2 = θ 23 − θ3 = arcsin B − w − θ3 或 θ 2 = π − arcsin B − w − θ3 （4）求 θ 5
s23 (c1ax + s1a y ) − c23 az = c5 ⇒
(10)
θ5 = arccos[ s23 (c1ax + s1a y ) − c23 az ] 或 θ5 = 2π − arccos[ s23 (c1ax + s1a y ) − c23az ] （5）求 θ 4
Industrial Robot Space Curve Real-time Path Planning Algorithm
Wang Liu , Ma Guo-hong，Zhang Hua (Robot Institute, Nanchang University, Nanchang City, 330031, China) Abstract:This paper presents a path planning algorithms for six-axis manipulator. The algorithm guide the robot move along the space curves, and figure out joint command of the robot motor. Algorithm including analytical solution of inverse kinematics so similar structure type of the robot arm can be used. The algorithm is realized with Matlab, and the effectiveness of algorithm is verified with programme. Keyword: Robot; Control algorithm; path planning; kinematics