6轴机械臂正逆解运算实现

利用Gluon-6L3机械臂模型的参数，对机械臂进行运动学分析。
这里采用标准DH坐标系，并将d6设置为0，方便后续计算。
首先，SDH的变换矩阵为：
ii−1T=Ai=^{i-1}_iT=A_i=ii−1T=Ai=[ci−sicαisisαiaicisicicαi−cisαiaisi0sαicαidi0001]\begin{bmatrix} c_i & -s_ic\alpha_i & s_is\alpha_i& a_ic_i \\ s_i & c_ic\alpha_i & -c_is\alpha_i& a_is_i\\ 0 & s\alpha_i & c\alpha_i&d_i\\ 0&0&0&1\end{bmatrix}⎣⎢⎢⎡cisi00−sicαicicαisαi0sisαi−cisαicαi0aiciaisidi1⎦⎥⎥⎤
我们将机械臂参数表中的数据带入，得到6个变换矩阵

10T=^{0}_1T=10T=[c10s10s10−c10010d10001]\begin{bmatrix} c_1& 0 & s_1 &0\\ s_1 & 0 & -c_1 & 0\\ 0 & 1 & 0 &d_1\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡c1s1000010s1−c10000d11⎦⎥⎥⎤ 21T=^{1}_2T=21T=[c2−s20a2c2s2c20a2s200100001]\begin{bmatrix} c_2& -s_2 & 0 &a_2c_2\\ s_2 & c_2 & 0 & a_2s_2\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡c2s200−s2c2000010a2c2a2s201⎦⎥⎥⎤

32T=^{2}_3T=32T=[cθ3−sθ30a3cθ3sθ3cθ30a3sθ300100001]\begin{bmatrix} c\theta_3& -s\theta_3 & 0 &a_3c\theta_3\\ s\theta_3 & c\theta_3 & 0 & a_3s\theta_3\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡cθ3sθ300−sθ3cθ3000010a3cθ3a3sθ301⎦⎥⎥⎤ 43T=^{3}_4T=43T=[cθ40sθ40sθ40−cθ40010d40001]\begin{bmatrix} c\theta_4& 0 & s\theta_4 &0\\ s\theta_4 & 0 & -c\theta_4 & 0\\ 0 & 1 & 0 &d_4\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡cθ4sθ4000010sθ4−cθ40000d41⎦⎥⎥⎤

54T=^{4}_5T=54T=[cθ50−sθ50sθ50cθ500−10d50001]\begin{bmatrix} c\theta_5& 0 & -s\theta_5 &0\\ s\theta_5 & 0 & c\theta_5 & 0\\ 0 & -1 & 0 &d_5\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡cθ5sθ50000−10−sθ5cθ50000d51⎦⎥⎥⎤ 65T=^{5}_6T=65T=[cθ6−sθ600sθ6cθ60000100001]\begin{bmatrix} c\theta_6& -s\theta_6 & 0 &0\\ s\theta_6 & c\theta_6 & 0 & 0\\ 0 & 0 & 1 &0\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡cθ6sθ600−sθ6cθ60000100001⎦⎥⎥⎤

机械臂的位姿矩阵T为：
T=10T21T32T43T54T65T=T=^{0}_1T^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T=T=10T21T32T43T54T65T=[nxoxaxpxnyoyaypynzozazpz0001]\begin{bmatrix} 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{bmatrix}⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤

nx=c1c5c6c234−c1s6s234+s1s5s6n_x=c_1c_5c_6c_{234}-c_1s_6s_{234}+s_1s_5s_6nx=c1c5c6c234−c1s6s234+s1s5s6
ny=s1c5c6c234−c1c6c6−s1s5s234n_y=s_1c_5c_6c_{234}-c_1c_6c_{6}-s_1s_5s_{234}ny=s1c5c6c234−c1c6c6−s1s5s234
nz=c5c6s234+s6c234n_z=c_5c_6s_{234}+s_6c_{234}nz=c5c6s234+s6c234
ox=−c1c5s6c234−s1s5s6−c1c6s234o_x=-c_1c_5s_6c_{234}-s_1s_5s_6-c_1c_6s_{234}ox=−c1c5s6c234−s1s5s6−c1c6s234
oy=−s1c5s6c234+c1c5s6−s1c6s234o_y=-s_1c_5s_6c_{234}+c_1c_5s_6-s_1c_6s_{234}oy=−s1c5s6c234+c1c5s6−s1c6s234
oz=c6c234−s6c5s234o_z=c_6c_{234}-s_6c_5s_{234}oz=c6c234−s6c5s234
ax=c5s1−c1c5c234a_x=c_5s_1-c_1c_5c_{234}ax=c5s1−c1c5c234
ay=−c5c1−s1s5c234a_y=-c_5c_1-s_1s_5c_{234}ay=−c5c1−s1s5c234
az=−s5s234a_z=-s_5s_{234}az=−s5s234
px=c1d5s234+s1d4+a3c1c23+a2c2c1p_x=c_1d_5s_{234}+s_1d_4+a_3c_1c_{23}+a_2c_2c_1px=c1d5s234+s1d4+a3c1c23+a2c2c1
py=s1d5s234−c1d4+a3s1c23+a2c2s1p_y=s_1d_5s_{234}-c_1d_4+a_3s_1c_{23}+a_2c_2s_1py=s1d5s234−c1d4+a3s1c23+a2c2s1
pz=−c234d5+a3s23+a2s2+d1p_z=-c_{234}d_5+a_3s_{23}+a_2s_2+d_1pz=−c234d5+a3s23+a2s2+d1

运动学正解
Euler Angles-由T推算按angles

 bata = atan2(sqrt(pow(T.matric[2][0], 2) + pow(T.matric[2][1], 2)), T.matric[2][2]);alpha = atan2(T.matric[1][2] / sin(bata), T.matric[0][2] / sin(bata));gama = atan2(T.matric[2][1] / sin(bata), -T.matric[2][0] / sin(bata));

运动学逆解
将末端位姿输入，反求T中的元素

CaculateThta(double x, double y, double z, double alpha, double bata, double gama)

根据末端位姿写出的T矩阵
（根据台大机器人运动学讲解P11，P12写的T,详细了解https://www.bilibili.com/video/BV1v4411H7ez）
T=[cαcβcγ−sαsγ−cαcβsγ−sαcγcαsβpxsαcβcγ+cαsγ−sαcβcγ+cαcγsαsβpy−sβcγsβsγcβpz0001]T=\begin{bmatrix} c_\alpha c_\beta c_\gamma-s_\alpha s_\gamma& -c_\alpha c_\beta s_\gamma-s_\alpha c_\gamma &c_\alpha s_\beta &p_x\\ s_\alpha c_\beta c_\gamma+c_\alpha s_\gamma& -s_\alpha c_\beta c_\gamma+c_\alpha c_\gamma& s_\alpha s_\beta & p_y \\ -s_\beta c_\gamma& s_\beta s_\gamma &c_\beta & p_z \\ 0 &0 &0 &1 \end{bmatrix}T=⎣⎢⎢⎡cαcβcγ−sαsγsαcβcγ+cαsγ−sβcγ0−cαcβsγ−sαcγ−sαcβcγ+cαcγsβsγ0cαsβsαsβcβ0pxpypz1⎦⎥⎥⎤ =[nxoxaxpxnyoyaypynzozazpz0001]\begin{bmatrix} 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{bmatrix}⎣⎢⎢⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦⎥⎥⎤
首先，我们对矩阵 10T^{0}_1T10T进行逆变换

10T−1T=21T32T43T54T65T=[c5c6c234−s6s234−c5s6c234−c6s234−s5c234d5s234+a3c23+a2c2c5c6s234−s6c234−c5s6s234+c6c234−s5s234−d5c234+a3s23+a2s2c6s5−s6s5c5d40001]=^{0}_1T^{-1}T=^{1}_2T^{2}_3T^{3}_4T^{4}_5T^{5}_6T=\begin{bmatrix} c_5c_6c_{234}-s_6s_{234}& -c_5s_6c_{234}-c_6s_{234} & -s_5c_{234} &d_5s_{234}+a_3c_{23}+a_2c_2\\ c_5c_6s_{234}-s_6c_{234}& -c_5s_6s_{234}+c_6c_{234} & -s_5s_{234} &-d_5c_{234}+a_3s_{23}+a_2s_2\\ c_6s_5 &-s_6s_5 & c_5 &d_4\\ 0 &0 &0 &1 \end{bmatrix}=10T−1T=21T32T43T54T65T=⎣⎢⎢⎡c5c6c234−s6s234c5c6s234−s6c234c6s50−c5s6c234−c6s234−c5s6s234+c6c234−s6s50−s5c234−s5s234c50d5s234+a3c23+a2c2−d5c234+a3s23+a2s2d41⎦⎥⎥⎤= [c1nx+s1nyc1ox+s1oyc1ax+s1ayc1px+s1pynzozazpy−d1s1nx−c1nys1ox−c1oys1ax−c1ays1px−c1py0001]\begin{bmatrix} c_1n_x+s_1n_y& c_1o_x+s_1o_y & c_1a_x+s_1a_y &c_1p_x+s_1p_y\\ n_z & o_z& a_z & p_y-d_1 \\ s_1n_x-c_1n_y& s_1o_x-c_1o_y & s_1a_x-c_1a_y &s_1p_x-c_1p_y\\ 0 &0 &0 &1 \end{bmatrix}⎣⎢⎢⎡c1nx+s1nynzs1nx−c1ny0c1ox+s1oyozs1ox−c1oy0c1ax+s1ayazs1ax−c1ay0c1px+s1pypy−d1s1px−c1py1⎦⎥⎥⎤

到这一步某些θ\thetaθ角已经现形了，这里计算尽量统一位atan2。

计算θ1\theta_1θ1
d4=s1px−c1pyd_4=s_1p_x-c_1p_yd4=s1px−c1py (1)
令px=cosϕ,py=sinϕp_x=cos\phi,p_y=sin\phipx=cosϕ,py=sinϕ带入(1)得
sin(θ1−ϕ)=d4sin(\theta_1-\phi)=d_4sin(θ1−ϕ)=d4
cos(θ1−ϕ)=±1−d42cos(\theta_1-\phi)=\pm\sqrt{1-d_4^2} \quadcos(θ1−ϕ)=±1−d42
θ1=atan2(py,px)+atan2(d4,±1−d42)\theta_1=atan2(p_y,p_x)+atan2(d_4,\pm\sqrt{1-d_4^2} \quad)θ1=atan2(py,px)+atan2(d4,±1−d42)

计算θ5\theta_{5}θ5
已知c5=s1ax−c1ayc_{5}=s_1a_x-c_1a_yc5=s1ax−c1ay
s5=±1−c52s_{5}=\pm\sqrt{1-c_5^2} \quads5=±1−c52
θ5=atan2(s5,c5)\theta_{5}=atan2(s_{5},c_{5})θ5=atan2(s5,c5)

计算θ6\theta_6θ6
c6s5=s1nx−c1nyc_6s_5=s_1n_x-c_1n_yc6s5=s1nx−c1ny
−s6s5=s1ox−c1oy-s_6s_5=s_1o_x-c_1o_y−s6s5=s1ox−c1oy
θ6=atan2((s1nx−c1ny)s5,(s1ox−c1oy)−s5)\theta_6=atan2(\frac{(s_1n_x-c_1n_y)}{s_5} \quad,\frac{(s_1o_x-c_1o_y)}{-s_5} \quad)θ6=atan2(s5(s1nx−c1ny),−s5(s1ox−c1oy))

计算θ3\theta_3θ3
已知s234=az−s5,c234=c1ax+s1ay−s5s_{234}=\frac{a_z}{-s_5} \quad,c_{234}=\frac{c_1a_x+s_1a_y}{-s_5} \quads234=−s5az,c234=−s5c1ax+s1ay（2）
已知d5s234+a3c23+a2c2=c1px+s1pyd_5s_{234}+a_3c_{23}+a_2c_2=c_1p_x+s_1p_yd5s234+a3c23+a2c2=c1px+s1py（3）
已知−d5c234+a3s23+a2s2=pz−d1-d_5c_{234}+a_3s_{23}+a_2s_2=p_z-d_1−d5c234+a3s23+a2s2=pz−d1（4）
令k1=c1px+s1py−d5s234=a3c23+a2c2,k2=pz−d1+d5c234=a3s23+a2s2k_1=c_1p_x+s_1p_y-d_5s_{234}=a_3c_{23}+a_2c_2 ,k_2=p_z-d_1+d_5c_{234}=a_3s_{23}+a_2s_2k1=c1px+s1py−d5s234=a3c23+a2c2,k2=pz−d1+d5c234=a3s23+a2s2

c3=k12+k22−a22−a332a2a3c_3=\frac{k_1^2+k_2^2-a_2^2-a_3^3}{2a_2a_3} \quadc3=2a2a3k12+k22−a22−a33

s3=±1−c32s_3=\pm\sqrt{1-c_3^2} \quads3=±1−c32
θ3=atan2(s3,c3)\theta_{3}=atan2(s_{3},c_{3})θ3=atan2(s3,c3)

计算θ2\theta_2θ2
继续利用上面三个公式（2）（3）（4）求解θ2\theta_2θ2

s2=(k2(a2+a3c3)−k1a3s3)a22+a32+2a1a2c3s_2=\frac{(k_2(a_2+a_3c_3)-k_1a_3s_3)}{a_2^2+a_3^2+2a_1a_2c_3} \quads2=a22+a32+2a1a2c3(k2(a2+a3c3)−k1a3s3)

c2=k2−s2(a2+a3c3)a3s3c_2=\frac{k_2-s_2(a_2+a_3c_3)}{a_3s_3} \quadc2=a3s3k2−s2(a2+a3c3)
θ2=atan2(s2,c2)\theta_{2}=atan2(s_{2},c_{2})θ2=atan2(s2,c2)

计算θ4\theta_4θ4
s234=s4c23+s23c4s_{234}=s_4c_{23}+s_{23}c_4s234=s4c23+s23c4
c234=c4c23−s23s4c_{234}=c_4c_{23}-s_{23}s_4c234=c4c23−s23s4
两式联立得
s4=c23s234−s23c234s_4=c_{23}s_{234}-s_{23}c_{234}s4=c23s234−s23c234
c4=c234+s4s23c23c_4=\frac{c_{234}+s_{4}s_{23}}{c_{23}} \quadc4=c23c234+s4s23
θ4=atan2(s4,c4)\theta_{4}=atan2(s_{4},c_{4})θ4=atan2(s4,c4)

至此，六个角全部求完，根据上式子的表达一共八组解

6轴机械臂正逆解运算实现相关推荐

【机器人原理与实践（三）】六轴机械臂正逆解控制
文章目录 3.1 空间转换矩阵的理解 3.1.1平移变换 3.1.2旋转变换 3.2 D-H参数法 3.3 建立机械臂模型 3.3.1 机械臂模型介绍 3.3.2 使用Matlab进行示教仿真 3.4 ...
六轴机械臂正逆解计算
一.机械臂运动学机械臂运动学就是根据未端执行器与所选参考坐标系之间的几何关系,确定末端执行器的空间位置和姿态与各关节变量之间的数学关系.包括正运动学 (Forward Kinematics)和逆运动 ...
如何实现六轴机械臂的逆解计算？
1. 机械臂运动学介绍机械臂运动学机器人运动学就是根据末端执行器与所选参考坐标系之间的几何关系,确定末端执行器的空间位置和姿态与各关节变量之间的数学关系.包括正运动学(Forward Kinema ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
【机器人学】使用解析法求解6轴机械臂的逆运动学解
本文是承接上一篇求3轴拟人机械臂逆解内容(链接),扩展到求6轴机械臂的逆解,研究的仍然是目前比较流行的工业机械臂构型:拟人臂+球形腕关节(如下图1和图2所示),因为这种构型的机械臂具有闭合形式的逆运动 ...
修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
实验一机械臂正逆运动学
实验一机械臂正逆运动学一.实验目的 1.巩固正逆运动学基础概念. 2.了解正逆运动学在机械臂控制中的实际用途. 二.实验内容 1.机械臂模型DH参数的计算. 2.机械臂正运动学的计算. 3.机械臂 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...

6轴机械臂正逆解运算实现

6轴机械臂正逆解运算实现

6轴机械臂正逆解运算实现相关推荐

最新文章

热门文章