最近需要解算六轴机器人的解析解,算法已完成,这里记录一下.

一、机器人模型

PUMA560:

全称:Programmable Universal Manipulation Arm
1978年由Unimation 机器人公司的Victor Scheinman研发.

本文使用的模型:

在这里插入图片描述

在这里插入图片描述

D-H矩阵:

在这里插入图片描述

二、机器人正运动学

机器人正运动学就是给定机器人各关节位置,计算机器人连杆上任意点的位姿.

位姿矩阵

机器人位置和姿态可以由一个方阵描述:

其中,n,o,a为末端姿态,p为末端位置.

 

正运动学

ki.

对于确定的机器人结构,如上图中所示,每个关节在上一关节的描述是确定的,因此正运动学直接计算即可.

三、机器人逆运动学

 

求解关节一

求解关节三

由上式解出的角度范围是[−π, 3π/2 ],且周期为2π,因此,
当θ>π/2时,取θ=θ−2π,否则,θ不变

求解关节二

θ 2 的有效区间为( −3π/2 , π/2],且周期为2π,因此,
当θ>π/2时,取θ=θ−2π,当θ≤ −3π/2 时,取θ=θ+2π.

求解关节四

θ 4 =atan2(k 1 ,k 2 )
θ 4的有效区间为(−π,π],因此,当θ 4 >0时,取θ 42 =θ 4 −π,否则,取θ 42 =θ 4 +π.

需要从两个解中筛选一个值,通过判断与上一插补值的绝对值大小,选取绝对值小的作为真实解.

 

求解关节五

求解关节六

 θ 6 周期为2π,当θ 6 >0时,取θ 62 =θ 6 −2π,否则,取θ 62 =θ 6 +2π.

以上过程共解出8组解.