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

Rosen.

已于 2023-03-30 22:33:57 修改

阅读量3.6k

点赞数 5

分类专栏：项目记录文章标签：机器人机械臂正逆运动学

于 2022-02-07 10:01:13 首次发布

本文链接：https://blog.csdn.net/rosen_er/article/details/122804664

版权

项目记录专栏收录该内容

6 篇文章 2 订阅

订阅专栏

文章目录

基本要求

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

基本概念

连杆长度：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	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$
1	0	0	80	$\theta_1$
2	$\pi/2$	10	0	$\theta_2+\pi/2$
3	0	105	0	$\theta_3$
4	0	96	-11	$\theta_4$
5	$\pi/2$	0	60	$\theta_5+\pi/2$
6	0	0	95	0

根据书本中公式， ${ }_{i}^{i-1} T$ 的一般表达式如公式(1)所示：
$}_{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]$
可以计算得到工具坐标系相对于固定坐标系的位置和姿态，如下列所示
$}_{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$

逆运动学求解

将工具坐标系相对于固定坐标系的位姿齐次矩阵, 转化成笛卡尔坐标形式, 姿态采用绕定轴 X-Y-Z 旋转 (RPY 角) 的形式描述, 其转化过程可描述如下:
$}_{B}^{A} T=\left[\begin{array}{cc} { }_{B}^{A} R & { }_{B}^{A} P \\ 0 & 0 & 0 \end{array}\right]$
其中,
$}_{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]$
其转化为 $RP Y$ 角对应的公式为:
$\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.$
对应的笛卡尔坐标为:
$\text { Carte }=[x, y, z, \theta, \psi, \phi]$
已知工具坐标系末端相对于固定坐标系的位置和姿态, 通过几何法对机械臂的各关节角度进行求解。首先可以直接确定第一个关节和第五个关节的角度值, 第一个关节的角度可以通过 $\mathrm{x}$ 和 $\mathrm{y}$ 直接确定, 第四个关节的角度值等于 $\phi$ , 即
$\begin{gathered} \theta_{1}=\operatorname{atan} 2(y, x) \\ \theta_{5}=\phi \end{gathered}$
由于该机械辟只有五个自由度, 最后一个关节为空制夹爪的末端, 所以首先要对工具坐标系末端相对于固定坐标系的位置进行处理, 分别减去夹爪在 $x 、 y 、 z$ 方向上的分量，如下所示：

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

对于三连杆所在的平面，有如下几何关系：
$\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}$

$\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}$

根据书本公式[1]，可以解得
$\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}$
最终，根据 $\theta_1'+\theta_2'+\theta_3=\psi$ 可以求解出平面三连杆中各个关节的倾斜角度。

多解下解的选取

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

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

源码下载

多功能视觉引导5-DOF机械臂实现

Rosen.

关注

5
点赞
踩
45

收藏

觉得还不错? 一键收藏
打赏
2
评论
5-DOF机械臂课程设计（一）之正逆运动学

文章目录基本要求基本概念关节坐标系的建立正运动学求解DH参数表的建立逆运动学求解多解下解的选取源码下载后续基本要求开发上位机程序，要求有良好的界面，能提供关节空间下和笛卡尔坐标下表示的目标位姿点和中间若干经过点的输入功能，进行轨迹规划，并控制机械臂沿轨迹运动，最终完成目标抓取的任务。基本概念连杆长度：2个相邻关节轴线之间的距离连杆扭角：2个相邻关节轴线之间的角度连杆偏距：2个关节坐标系的X轴之间的距离关节角度：关节变量计算时需要加初始角度偏移关节坐标系的建立根据《机
复制链接

扫一扫