5-DOF机械臂课程设计（一）之正逆运动学

文章目录

基本要求
基本概念
关节坐标系的建立
正运动学求解
- DH参数表的建立
逆运动学求解
多解下解的选取
源码下载
后续

基本要求

开发上位机程序，要求有良好的界面，能提供关节空间下和笛卡尔坐标下表示的目标位姿点和中间若干经过点的输入功能，进行轨迹规划，并控制机械臂沿轨迹运动，最终完成目标抓取的任务。

基本概念

连杆长度：2个相邻关节轴线之间的距离

连杆扭角：2个相邻关节轴线之间的角度

连杆偏距：2个关节坐标系的X轴之间的距离

关节角度：关节变量计算时需要加初始角度偏移

关节坐标系的建立

根据《机器人学导论》的内容，建立坐标系的规则如下：

坐标系Z轴确定规则：

如果关节是旋转的，Z轴按右手定则大拇指指向为正方向。
如果关节是滑动的，Z轴沿直线运动方向的正方向。
连杆长度D为关节变量。
X轴确定规则：
1）情况1：两关节Z轴既不平行也不相交
取两Z轴公垂线方向作为X轴方向。
2）情况2：两关节Z轴平行
此时，两Z轴之间有无数条公垂线，可挑选与前一关节的公垂线的一条公垂线
3）情况3：两关节Z轴相交
取两条Z轴的叉积方向作为X轴（叉积：向量积）
Y轴确定原则：右手定则

最终建立的机械臂坐标系如下图：

正运动学求解

DH参数表的建立

根据上述坐标系的建立，可以得到DH参数表如下：

i	αi−1\alpha_{i-1}αi−1	ai−1a_{i-1}ai−1	did_idi	θi\theta_iθi
1	0	0	80	θ1\theta_1θ1
2	π/2\pi/2π/2	10	0	θ2+π/2\theta_2+\pi/2θ2+π/2
3	0	105	0	θ3\theta_3θ3
4	0	96	-11	θ4\theta_4θ4
5	π/2\pi/2π/2	0	60	θ5+π/2\theta_5+\pi/2θ5+π/2
6	0	0	95	0

根据书本中公式，ii−1T{ }_{i}^{i-1} Tii−1T的一般表达式如公式(1)所示：
ii−1T=[cθi−sθi0ai−1sθicαi−1sθicαi−1−sαi−1−sαi−1disθisαi−1cθisαi−1cαi−1cαi−1di0001]{ }_{i}^{i-1} T=\left[\begin{array}{cccc} c \theta_{i} & -s \theta_{i} & 0 & a_{i-1} \\ s \theta_{i} c \alpha_{i-1} & s \theta_{i} c \alpha_{i-1} & -s \alpha_{i-1} & -s \alpha_{i-1} d_{i} \\ s \theta_{i} s \alpha_{i-1} & c \theta_{i} s \alpha_{i-1} & c \alpha_{i-1} & c \alpha_{i-1} d_{i} \\ 0 & 0 & 0 & 1 \end{array}\right] ii−1T=⎣⎢⎢⎡cθisθicαi−1sθisαi−10−sθisθicαi−1cθisαi−100−sαi−1cαi−10ai−1−sαi−1dicαi−1di1⎦⎥⎥⎤
可以计算得到工具坐标系相对于固定坐标系的位置和姿态，如下列所示
60T=10T⋅21T⋅32T⋅43T⋅54T⋅65T{ }_{6}^{0} T={ }_{1}^{0} T \cdot{ }_{2}^{1} T \cdot{ }_{3}^{2} T \cdot{ }_{4}^{3} T \cdot{ }_{5}^{4} T \cdot{ }_{6}^{5} T 60T=10T⋅21T⋅32T⋅43T⋅54T⋅65T

逆运动学求解

将工具坐标系相对于固定坐标系的位姿齐次矩阵, 转化成笛卡尔坐标形式, 姿态采用绕定轴 X-Y-Z 旋转 (RPY 角) 的形式描述, 其转化过程可描述如下:
BAT=[BARBAP000]{ }_{B}^{A} T=\left[\begin{array}{cc} { }_{B}^{A} R & { }_{B}^{A} P \\ 0 & 0 & 0 \end{array}\right] BAT=[BAR0BAP00]
其中,
BAR=[r11r12r13r21r22r23r31r32r33]BAP=[xyz]{ }_{B}^{A} R=\left[\begin{array}{lll} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{array}\right]{ }_{B}^{A} P=\left[\begin{array}{l} x \\ y \\ z \end{array}\right] BAR=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤BAP=⎣⎡xyz⎦⎤
其转化为 RPYR P YRPY 角对应的公式为:
{θ=−asin⁡(r31)ψ=atan⁡2(r32/cos⁡(θ),r33/cos⁡(θ))ϕ=atan⁡2(r21/cos⁡(θ),r11/cos⁡(θ))\left\{\begin{array}{c} \theta=-a \sin \left(r_{31}\right) \\ \psi=\operatorname{atan} 2\left(r_{32} / \cos (\theta), r_{33} / \cos (\theta)\right) \\ \phi=\operatorname{atan} 2\left(r_{21} / \cos (\theta), r_{11} / \cos (\theta)\right) \end{array}\right. ⎩⎨⎧θ=−asin(r31)ψ=atan2(r32/cos(θ),r33/cos(θ))ϕ=atan2(r21/cos(θ),r11/cos(θ))
对应的笛卡尔坐标为:
Carte =[x,y,z,θ,ψ,ϕ]\text { Carte }=[x, y, z, \theta, \psi, \phi] Carte =[x,y,z,θ,ψ,ϕ]
已知工具坐标系末端相对于固定坐标系的位置和姿态, 通过几何法对机械臂的各关节角度进行求解。首先可以直接确定第一个关节和第五个关节的角度值, 第一个关节的角度可以通过 x\mathrm{x}x 和 y\mathrm{y}y 直接确定, 第四个关节的角度值等于 ϕ\phiϕ, 即
θ1=atan⁡2(y,x)θ5=ϕ\begin{gathered} \theta_{1}=\operatorname{atan} 2(y, x) \\ \theta_{5}=\phi \end{gathered} θ1=atan2(y,x)θ5=ϕ
由于该机械辟只有五个自由度, 最后一个关节为空制夹爪的末端, 所以首先要对工具坐标系末端相对于固定坐标系的位置进行处理, 分别减去夹爪在 x、y、zx 、 y 、 zx、y、z 方向上的分量，如下所示：

对于剩下的三个关节，求解其角度值可以看作是已知第三连杆的位姿求解平面三连杆中三个关节的角度值，其中，第三连杆末端的位置为x1,y1)x_1, y_1)x1,y1) ,倾斜角度为 ψ\psiψ 。三连杆分别对应的角度为 θ1′,θ2′,θ3′\theta_1',\theta_2',\theta_3'θ1′,θ2′,θ3′,

对于三连杆所在的平面，有如下几何关系：
x1=l1⋅cos⁡θ1′+l2⋅cos⁡(θ1′+θ2′)+l3⋅cos⁡(θ1′+θ2′+θ3′)y1=l1⋅sin⁡θ1′+l2⋅sin⁡(θ1′+θ2′)+l3⋅sin⁡(θ1′+θ2′+θ3′)令 m=l1⋅cos⁡ψ−x1,n=l2⋅sin⁡ψ−y1,对于 θ1有 θ1=(l1⋅cos⁡θ1′+m)2+(l1⋅sin⁡θ1′+n)2(θ1′+θ2′)=(l1⋅cos⁡(θ1′+θ2′)+m)2+(l1⋅sin⁡(θ1′+θ2′)+n)2进而有, m⋅cos⁡θ1′+n⋅sin⁡θ1′=k1k1=l22−l12−m2−n22l1m⋅cos⁡(θ1′+θ2′)+n⋅sin⁡(θ1′+θ2′)=k2k1=l12−l22−m2−n22l2\begin{gathered} x_{1}=l_{1} \cdot \cos \theta_{1}{ }^{\prime}+l_{2} \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+l_{3} \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}+\theta_{3}{ }^{\prime}\right) \\ y_{1}=l_{1} \cdot \sin \theta_{1}{ }^{\prime}+l_{2} \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+l_{3} \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}+\theta_{3}{ }^{\prime}\right) \\ \text { 令 } m=\mathrm{l}_{1} \cdot \cos \psi-x_{1}, n=\mathrm{l}_{2} \cdot \sin \psi-y_{1}, \text { 对于 } \theta_{1} \text { 有 } \\ \qquad \begin{array}{c} \theta_{1}=\left(l_{1} \cdot \cos \theta_{1}{ }^{\prime}+m\right)^{2}+\left(l_{1} \cdot \sin \theta_{1}{ }^{\prime}+n\right)^{2} \\ \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)=\left(l_{1} \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+m\right)^{2}+\left(l_{1} \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+n\right)^{2} \\ \text { 进而有, } \\ m \cdot \cos \theta_{1}{ }^{\prime}+n \cdot \sin \theta_{1}{ }^{\prime}=k_{1} \\ k_{1}=\frac{l_{2}^{2}-l_{1}^{2}-m^{2}-n^{2}}{2 l_{1}} \\ m \cdot \cos \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)+n \cdot \sin \left(\theta_{1}{ }^{\prime}+\theta_{2}{ }^{\prime}\right)=k_{2} \\ k_{1}=\frac{l_{1}^{2}-l_{2}^{2}-m^{2}-n^{2}}{2 l_{2}} \end{array} \end{gathered} x1=l1⋅cosθ1′+l2⋅cos(θ1′+θ2′)+l3⋅cos(θ1′+θ2′+θ3′)y1=l1⋅sinθ1′+l2⋅sin(θ1′+θ2′)+l3⋅sin(θ1′+θ2′+θ3′) 令 m=l1⋅cosψ−x1,n=l2⋅sinψ−y1, 对于 θ1 有 θ1=(l1⋅cosθ1′+m)2+(l1⋅sinθ1′+n)2(θ1′+θ2′)=(l1⋅cos(θ1′+θ2′)+m)2+(l1⋅sin(θ1′+θ2′)+n)2 进而有, m⋅cosθ1′+n⋅sinθ1′=k1k1=2l1l22−l12−m2−n2m⋅cos(θ1′+θ2′)+n⋅sin(θ1′+θ2′)=k2k1=2l2l12−l22−m2−n2

令 b1=−2⋅n⋅k1,c1=k12−m2,Δ1=b12−4⋅a⋅c1,当 Δ1<0时, 则表示出现非实数解的情况, 此时机械臂对于输入的笛卡尔坐标工作空间不可达。同理, 令 b2=−2⋅n⋅k2,c2=k22−m2,Δ2=b22−4⋅a⋅c2,当 Δ2<0时, 则表示工作空间不可达。 \begin{aligned} &\text { 令 } b_{1}=-2 \cdot n \cdot k_{1}, c_{1}=k_{1}^{2}-m^{2}, \Delta_{1}=\mathrm{b}_{1}^{2}-4 \cdot a \cdot c_{1}, \text { 当 } \Delta_{1}<0 \text { 时, 则表示出现 } \\ &\text { 非实数解的情况, 此时机械臂对于输入的笛卡尔坐标工作空间不可达。同理, 令 } b_{2}= \\ &-2 \cdot n \cdot k_{2}, c_{2}=k_{2}^{2}-m^{2}, \Delta_{2}=\mathrm{b}_{2}^{2}-4 \cdot a \cdot c_{2}, \text { 当 } \Delta_{2}<0 \text { 时, 则表示工作空间不可 } \\ &\text { 达。 } \\ \end{aligned} 令 b1=−2⋅n⋅k1,c1=k12−m2,Δ1=b12−4⋅a⋅c1, 当 Δ1<0 时, 则表示出现非实数解的情况, 此时机械臂对于输入的笛卡尔坐标工作空间不可达。同理, 令 b2=−2⋅n⋅k2,c2=k22−m2,Δ2=b22−4⋅a⋅c2, 当 Δ2<0 时, 则表示工作空间不可达。

根据书本公式[1]，可以解得
θ1′=atan⁡2(n,m)±atan⁡2(a−k12),k1)θ1′+θ2,=atan⁡2(n,m)±atan⁡2(a−k22),k2)\begin{aligned} &\left.\theta_{1}^{\prime}=\operatorname{atan} 2(n, m) \pm \operatorname{atan} 2\left(a-k_{1}^{2}\right), k_{1}\right) \\ &\left.\theta_{1}{ }^{\prime}+\theta_{2},=\operatorname{atan} 2(n, m) \pm \operatorname{atan} 2\left(a-k_{2}{ }^{2}\right), k_{2}\right) \end{aligned} θ1′=atan2(n,m)±atan2(a−k12),k1)θ1′+θ2,=atan2(n,m)±atan2(a−k22),k2)
最终，根据θ1′+θ2′+θ3=ψ\theta_1'+\theta_2'+\theta_3=\psiθ1′+θ2′+θ3=ψ可以求解出平面三连杆中各个关节的倾斜角度。

多解下解的选取

平面三连杆在求解时会出现多解的情况，经过思考分析，对多解情况提出了一种可行的方案，如下图所示

上式的θ1′\theta_1'θ1′有两组解，由于机械臂的臂长l1l_1l1和 l2l_2l2的长度不同，所以会有：
θ11+θ12>θ21+θ22\theta_{11}+\theta_{12}>\theta_{21}+\theta_{22} θ11+θ12>θ21+θ22
此处 θ22<0\theta_{22}<0θ22<0, 并且 θ21>θ11\theta_{21}>\theta_{11}θ21>θ11, 由此关系可以对两组解进行区分。
则可以求出机械臂第 2、3、4 关节的角度
θ2=θ1′−90θ3=θ2′θ4=θ3′\begin{gathered} \theta_{2}=\theta_{1}^{\prime}-90 \\ \theta_{3}=\theta_{2}^{\prime} \\ \theta_{4}=\theta_{3}^{\prime} \end{gathered} θ2=θ1′−90θ3=θ2′θ4=θ3′
最终可得机械臂逆运动学的解
JVar =[θ1,θ2,θ3,θ4,θ5]\text { JVar }=\left[\theta_{1}, \theta_{2}, \theta_{3}, \theta_{4}, \theta_{5}\right] JVar =[θ1,θ2,θ3,θ4,θ5]

源码下载

方式一：
多功能视觉引导5-DOF机械臂实现
方式二：
关注文章下方公众号，在菜单栏中点击项目分享，即可获得百度网盘链接。

后续

喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！