UR机械臂逆运动学解析解

ur10机械臂的DH（修正）参数为

d1=0.1273;a2=-0.612;a3=-0.5723;d4=0.163941;d5=0.1157;d6=0.6922;

首先，推导机械臂的正向运动学方程，根据DH参数可以计算机械臂的正向运动学方程表示为：
0T6=0T11T22T33T44T55T6=[nxoxaxpxnyoyaypynzozazpz0001]{}^0T_6={}^0T_1 {}^1T_2 {}^2T_3 {}^3T_4 {}^4T_5 {}^5T_6=\left[ \begin{array}{cccc} n_x & o_x &a_x &p_x\\ n_y&o_y &a_y &p_y\\ n_z &o_z & a_z&p_z\\ 0 &0 & 0&1 \end{array} \right]0T6=0T11T22T33T44T55T6=⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
其中
0T1=[c1−s100s1c100001d10001]{}^0T_1 =\left[ \begin{array}{cccc} c1 & -s1 &0 &0\\ s1&c1 &0 &0\\ 0 &0 & 1&d_1\\ 0 &0 & 0&1 \end{array} \right]0T1=⎣⎢⎢⎡c1s100−s1c100001000d11⎦⎥⎥⎤，1T2=[c2−s20000−10s2c2000001]{}^1T_2 =\left[ \begin{array}{cccc} c2 & -s2 &0 &0\\ 0 &0 & -1&0\\ s2&c2 &0 &0\\ 0 &0 & 0&1 \end{array} \right]1T2=⎣⎢⎢⎡c20s20−s20c200−1000001⎦⎥⎥⎤，
2T3=[c3−s30a2s3c30000100001]{}^2T_3 =\left[ \begin{array}{cccc} c3 & -s3 &0 &a_2\\ s3&c3 &0 &0\\ 0 &0 & 1&0\\ 0 &0 & 0&1 \end{array} \right]2T3=⎣⎢⎢⎡c3s300−s3c3000010a2001⎦⎥⎥⎤，3T4=[c4−s40a3s4c400001d40001]{}^3T_4 =\left[ \begin{array}{cccc} c4 & -s4 &0 &a_3\\ s4&c4 &0 &0\\ 0 &0 & 1&d_4\\ 0 &0 & 0&1 \end{array} \right]3T4=⎣⎢⎢⎡c4s400−s4c4000010a30d41⎦⎥⎥⎤，
4T5=[c5−s50000−1−d5s5c5000001]{}^4T_5 =\left[ \begin{array}{cccc} c5& -s5 &0 &0\\ 0 &0 & -1&-d_5\\ s5&c5 &0 &0\\ 0 &0 & 0&1 \end{array} \right]4T5=⎣⎢⎢⎡c50s50−s50c500−1000−d501⎦⎥⎥⎤，5T6=[c6−s600001d6−s6−c6000001]{}^5T_6 =\left[ \begin{array}{cccc} c6 & -s6 &0 &0\\ 0 &0 & 1&d_6\\ -s6&-c6 &0 &0\\ 0 &0 & 0&1 \end{array} \right]5T6=⎣⎢⎢⎡c60−s60−s60−c6001000d601⎦⎥⎥⎤

接下来，根据给定的末端位姿0T6{}^0T_60T6求解对应的关节构型，也就是求解逆运动学：

由于关节2，3，4旋转轴方向相同，因此1T4{}^1T_41T4可以表示为
1T4=[c234−s2340a2c2+a3c2300−1−d4s234c2340a2s2+a3s230001]{}^1T_4 =\left[ \begin{array}{cccc} c234 & -s234 &0 &a_2c2+a_3c23\\ 0 &0 & -1&-d_4\\ s234&c234 &0 &a_2s2+a_3s23\\ 0 &0 & 0&1 \end{array} \right]1T4=⎣⎢⎢⎡c2340s2340−s2340c23400−100a2c2+a3c23−d4a2s2+a3s231⎦⎥⎥⎤

那么1T5{}^1T_51T5可以表示为：
1T5=[c234c5−c234s5s234s234d5+a2c2+a3c23−s5−c50−d4s234c5−s234s5−c234−c234d5+a2s2+a3s230001]{}^1T_5 =\left[ \begin{array}{cccc} c234c5 & -c234s5 &s234 &s234d_5+a_2c2+a_3c23\\ -s5 &-c5 & 0&-d_4\\ s234c5&-s234s5 &-c234 &-c234d_5+a_2s2+a_3s23\\ 0 &0 & 0&1 \end{array} \right]1T5=⎣⎢⎢⎡c234c5−s5s234c50−c234s5−c5−s234s50s2340−c2340s234d5+a2c2+a3c23−d4−c234d5+a2s2+a3s231⎦⎥⎥⎤

同时1T5{}^1T_51T5还可以表示为：

1T5=(0T1)−10T6(5T6)−1=[c1s100−s1c100001−d10001][nxoxaxpxnyoyaypynzozazpz0001][c60−s60−s60−c60010−d60001]=[c1nxc6−c1oxs6+s1nyc6−s1oys6c1ax+s1ay−c1nxs6−c1oxc6−s1nys6−s1oyc6−c1axd6+c1px−s1ayd6+s1py−s1nxc6+s1oxs6+c1nyc6−c1oys6−s1ax+c1ays1nxs6+s1oxc6−c1nys6−c1oyc6s1axd6−s1px−c1ayd6+c1pynzc6−ozs6az−nzs6−ozc6−azd6+pz−d10001]{}^1T_5=({}^0T_1)^{-1} \;{}^0T_6\;({}^5T_6)^{-1}=\left[ \begin{array}{cccc} c1 & s1 &0 &0\\ -s1&c1 &0 &0\\ 0 &0 & 1&-d_1\\ 0 &0 & 0&1 \end{array} \right]\left[ \begin{array}{cccc} n_x & o_x &a_x &p_x\\ n_y&o_y &a_y &p_y\\ n_z &o_z & a_z&p_z\\ 0 &0 & 0&1 \end{array} \right]\left[ \begin{array}{cccc} c6 & 0&-s6 &0\\ -s6 &0 & -c6&0\\ 0&1 &0 &-d_6\\ 0 &0 & 0&1 \end{array} \right]= \left[ \begin{array}{cccc} c1n_xc6-c1o_xs6+s1n_yc6-s1o_ys6 & c1a_x+s1a_y&-c1n_xs6-c1o_xc6 -s1n_ys6-s1o_yc6&-c1a_xd_6+c1p_x-s1a_yd_6+s1p_y\\ -s1n_xc6+s1o_xs6 +c1n_yc6-c1o_ys6& -s1a_x+ c1a_y&s1n_xs6+s1o_xc6-c1n_ys6-c1o_yc6 &s1a_xd_6-s1p_x-c1a_yd_6+c1p_y\\ n_zc6-o_zs6 & a_z&-n_zs6-o_zc6 &-a_zd_6+p_z-d1\\ 0 &0 & 0&1 \end{array} \right]1T5=(0T1)−10T6(5T6)−1=⎣⎢⎢⎡c1−s100s1c100001000−d11⎦⎥⎥⎤⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤⎣⎢⎢⎡c6−s6000010−s6−c60000−d61⎦⎥⎥⎤=⎣⎢⎢⎡c1nxc6−c1oxs6+s1nyc6−s1oys6−s1nxc6+s1oxs6+c1nyc6−c1oys6nzc6−ozs60c1ax+s1ay−s1ax+c1ayaz0−c1nxs6−c1oxc6−s1nys6−s1oyc6s1nxs6+s1oxc6−c1nys6−c1oyc6−nzs6−ozc60−c1axd6+c1px−s1ayd6+s1pys1axd6−s1px−c1ayd6+c1py−azd6+pz−d11⎦⎥⎥⎤

通过两个1T5{}^1T_51T5矩阵对比可以得到：
−s1(px−axd6)+c1(py−ayd6)=−d4-s1(p_x-a_xd_6)+c1(p_y-a_yd_6)=-d4−s1(px−axd6)+c1(py−ayd6)=−d4

通过上图的角度关系可以计算θ1\theta_1θ1
θ1=atan2(py−ayd6,px−axd6)±acos(d4(px−axd6)2+(py−ayd6)2)+π2\theta_1=atan2(p_y-a_yd_6,p_x-a_xd_6)\pm acos(\frac{d4}{\sqrt{(p_x-a_xd_6)^2+(p_y-a_yd_6)^2}})+\frac{\pi}{2}θ1=atan2(py−ayd6,px−axd6)±acos((px−axd6)2+(py−ayd6)2d4)+2π
这里解存在要求 (px−axd6)2+(py−ayd6)2>d4\sqrt{(p_x-a_xd_6)^2+(p_y-a_yd_6)^2}>d_4(px−axd6)2+(py−ayd6)2>d4；

θ5=±acos(pxs1−pyc1−d4d6)\theta_5=\pm acos(\frac{p_xs1-p_yc1-d_4}{d_6})θ5=±acos(d6pxs1−pyc1−d4)
这里解存在要求 ∣pxs1−pyc1−d4∣≤d6|p_xs1-p_yc1-d_4|\le d_6∣pxs1−pyc1−d4∣≤d6；

θ6=atan2(−oxs1+oyc1s5,nxs1−nyc1s5)\theta_6=atan2(\frac{-o_xs1+o_yc1}{s5},\frac{n_xs1-n_yc1}{s5})θ6=atan2(s5−oxs1+oyc1,s5nxs1−nyc1)

关节234是平行的，通过两个矩阵对比可以得到 −c234s5=c1ax+s1ay-c234s5=c1a_x+s1a_y−c234s5=c1ax+s1ay; −s234s5=az-s234s5 =a_z−s234s5=az

θ234=atan2(−azs5,−c1ax+s1ays5)\theta_{234}=atan2(-\frac{a_z}{s5},-\frac{c1a_x+s1a_y}{s5})θ234=atan2(−s5az,−s5c1ax+s1ay)

通过两个矩阵对比还可以得到

a2c2+a3c23=A1;A1=−c1axd6+c1px−s1ayd6+s1py−s234d5a_2c2+a_3c23=A_1; A_1=-c1a_xd_6+c1p_x-s1a_yd_6+s1p_y-s234d_5a2c2+a3c23=A1;A1=−c1axd6+c1px−s1ayd6+s1py−s234d5
a2s2+a3s23=A2;A2=−azd6+pz−d1+c234d5a_2s2+a_3s23=A_2; A_2=-a_zd_6+p_z-d1+c234d_5a2s2+a3s23=A2;A2=−azd6+pz−d1+c234d5
上式可以消除23（c232+s232=1c23^2+s23^2=1c232+s232=1）得到
2a2A2s2+2a2A1c2=A12+A22+a22−a322a_2A_2s2+2a_2A_1c2=A_1^2+A_2^2+a_2^2-a_3^22a2A2s2+2a2A1c2=A12+A22+a22−a32

根据万能公式可以计算得到
θ2=atan2(2a2A1,−2a2A2)−atan2(C,±(−2a2A2)2+(2a2A1)2−C2)\theta_2=atan2(2a_2A_1,-2a_2A_2)-atan2(C,\pm \sqrt{(-2a_2A_2)^2+(2a_2A_1)^2-C^2})θ2=atan2(2a2A1,−2a2A2)−atan2(C,±(−2a2A2)2+(2a2A1)2−C2)
其中C=A12+A22+a22−a32C=A_1^2+A_2^2+a_2^2-a_3^2C=A12+A22+a22−a32;

将θ2\theta_2θ2代回可以得到
a3c23=A1−a2c2a_3c23=A_1-a_2c2a3c23=A1−a2c2
a3s23=A2−a2s2a_3s23=A_2-a_2s2a3s23=A2−a2s2

θ23=atan2(A2−a2s2a3,A1−a2c2a3)\theta_{23}=atan2(\frac{A_2-a_2s2}{a_3},\frac{A_1-a_2c2}{a_3})θ23=atan2(a3A2−a2s2,a3A1−a2c2)

解存在要求：∣∣a2∣−∣a3∣∣≤(a2c2+a3c23)2+(a2s2+a3s23)2≤∣a2∣+∣a3∣||a_2|-|a_3||\le\sqrt{(a_2c2+a_3c23)^2+(a_2s2+a_3s23)^2}\le|a_2|+|a_3|∣∣a2∣−∣a3∣∣≤(a2c2+a3c23)2+(a2s2+a3s23)2≤∣a2∣+∣a3∣

因此，可以计算得到
θ3=θ23−θ2\theta_3=\theta_{23}-\theta_2θ3=θ23−θ2
θ4=θ234−θ23\theta_4=\theta_{234}-\theta_{23}θ4=θ234−θ23

UR机械臂逆运动学解析解相关推荐

数值法求六轴机械臂逆运动学解
问题描述:已知工具坐标系相对于固定坐标系的期望位置和姿态,如何求解满足期望位姿的关节角? 本篇将通过基于雅克比矩阵的牛顿-拉夫森迭代法求运动学逆解. 牛顿 - 拉夫森法(Newton - Raphso ...
PUAM560机械臂逆运动学实例-梳理
经过努力,对课本中PUAM560机械臂的运动学逆解算的实例进行了梳理,啥事都得动手试试才能学的深刻,整理一下我的理解和思路.由于首次接触逆解对于许多概念理解很不到位,不过随着学习的深入我相信我会越来越 ...
【现代机器人学】基于指数积的机械臂逆运动学
0 引言基于指数积的正运动学现代机器人学名词概念有了以上的基础,我们现在利用指数积来对机器人的逆运动学进行求解,有一点需要注意,需要先对机器人进行指数积的正运动学建模,然后才能利 ...
柔性matlab机械臂运动空间代码,中正平和的机器人学笔记——2. 机械臂逆运动学（附MATAB代码）...
0. 预备知识(别跳过呀) 上一篇我们讲了正运动学,也就是通过齐次变换矩阵,知晓关节角后求得机械臂末端的位姿,并表示在基座标系下.这一篇我们就来讲一讲逆运动学问题--已知工具坐标系{T}相对于固定坐标 ...
关于机械臂逆运动学 jacobian矩阵
机器人逆运动学机器人的逆解问题比较复杂,为了说明问题,下面先以2自由度的机器人为例. 如图所示,已知机器人末端的坐标值(x,y) ,试利用x,y 表示根据图中的几何关系可知: 联立求 ...
【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真
基于PoE公式的UR5机械臂正运动学建模求解与matlab仿真基于PoE公式的UR5机械臂正运动学分析 1.1 运动旋量的螺旋释义(理论基础) 1.2 基于PoE公式的UR5机械臂正运动学模型 1. ...
修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
UR机械臂学习（7-1）：MoveIt简单编程实现机械臂运动（正逆运动学）
主要参考: MoveIt编程实现关节空间机械臂运动(正运动学) https://blog.csdn.net/zzu_seu/article/details/90611186 MoveIt编程实现关节空 ...

UR机械臂逆运动学解析解

UR机械臂逆运动学解析解

UR机械臂逆运动学解析解相关推荐

最新文章

热门文章