最近需要解算六轴机器人的解析解,算法已完成,这里记录一下.
一、机器人模型
PUMA560:
全称:Programmable Universal Manipulation Arm
1978年由Unimation 机器人公司的Victor Scheinman研发.
本文使用的模型:
D-H矩阵:
二、机器人正运动学
机器人正运动学就是给定机器人各关节位置,计算机器人连杆上任意点的位姿.
位姿矩阵
机器人位置和姿态可以由一个方阵描述:
其中,n,o,a为末端姿态,p为末端位置.
正运动学
为k关节坐标系在i关节坐标系下的描述.
对于确定的机器人结构,如上图中所示,每个关节在上一关节的描述是确定的,因此正运动学直接计算即可.
三、机器人逆运动学
求解关节一
求解关节三
由上式解出的角度范围是[−π, 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π.
评论(0)
您还未登录,请登录后发表或查看评论