- 1【大模型安全】怎么防御提示词注入攻击 --- OWASP 十大威胁之首_大模型 避免提示注入的策略
- 2SwiftUI的认识与使用_swifui登录后切换窗口
- 3git : 无法将“git”项识别为 cmdlet、函数、脚本文件或可运行程序的名称。_git : 无法将“git”项识别为 cmdlet、函数、脚本文件或可运行程序的名称。请检查
- 4【工具】Movielens数据集详细介绍
- 5利用Python与Whisper革新视频翻译：打造高精度字幕翻译流程。_uvr5python调用
- 6tar时出现“目录校验和错误”_unknown suffix -- ignored
- 72021年第五届材料工程与先进制造技术国际会议（MEAMT 2021）EI检索_第五届先进材料及工程应用国际会议
- 8Vue3 源码阅读（10）：组件化 —— 实现原理_vue3组件化开发
- 9OneNET物联网平台介绍
- 10开源TinyFSM状态机适用于嵌入式工业平台吗？
机器人工具箱学习（二）_机械臂改进dh
赞
踩
一、机械臂及运动学
1.1 机械臂构成
机械臂多采用关节式机械结构，一般具有6个自由度，其中3个用来确定末端执行器的位置，另外3个则用来确定末端执行装置的方向(姿态)。
如图所示，一个机械臂是由一组可做相对运动的关节连接的连杆结合体。第一个连杆固定，连接该机械臂的基座，而最后一个连杆连接的是它的末端执行器。
通常可将关节划分为两种：第一种称为转动关节(或称为旋转关节)，转动关节可绕基准轴转动，相应的转动量称为关节角：第二种称为移动关节，移动关节是沿着基准轴移动，相应的位移称为关节偏距。还有一种特殊的关节称为球关节，球关节拥有三个自由度。可以用三个转动关节和一个零长度的连杆来描述一个球关节。
位于机械臂固定基座的坐标系称为基坐标系；位于操作臂末端执行器的坐标系称为工具坐标系，通常用它来描述机械臂的位置。
1.2 机器人运动学介绍
机器人运动学只研究机器人运动，不关注机器人运动过程中各零部件的质量及相关力，也不关注关节驱动力和力矩。
（1）机器人正运动学：给定一组机器人关节变量（转角或位移），求解末端工具坐标系相对于基坐标系的位置和姿态。
（2）机器人逆运动学：给定机器人末端工具箱坐标系的位置和姿态，求解机器人各关节变量。
二、D-H参数法
D-H 参数全称为Denavit-Hartenberg参数，它使用连杆参数来描述机构运动关系。在DH参数法中，描述机械臂中的每一个连杆需要4个运动学参数：
● 连杆长度
a
i
−
1
a _ { i - 1 }
ai−1：关节轴
i
−
1
i-1
i−1与关节轴
i
i
i之间公垂线的长度；
● 连杆转角
α
i
−
1
\alpha _ { i-1 }
αi−1：第
i
−
1
i-1
i−1个关节轴和第
i
i
i个关节轴之间的夹角；
● 连杆偏距
d
i
d_{i}
di：沿两个相邻连杆公共轴线方向的距离；
● 关节角
θ
i
\theta_{i}
θi：两相邻连杆绕公共轴线旋转的夹角。
2.1 标准D-H法（SDH）
建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定
Z
Z
Z轴：与关节轴线重合，其中
Z
i
−
1
Z_{i-1}
Zi−1轴与关节
i
i
i的轴线重合；
（3）确定
X
X
X轴：
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴的公垂线，方向由
Z
i
−
1
Z_{i-1}
Zi−1轴指向
Z
i
Z_{i}
Zi轴
1）如果
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴平行，选取与前一关节的公垂线共线的一条公垂线作为
X
X
X轴；
2）如果
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴相交，
X
X
X轴为
Z
i
−
1
Z_{i-1}
Zi−1轴与
Z
i
Z_{i}
Zi轴的叉积方向；
（4）确定
Y
Y
Y轴：右手定则。
D-H参数含义：
（1）
a
i
a_{i}
ai：关节轴线
i
−
1
i-1
i−1和关节轴线
i
i
i的公垂线长度
（2）
α
i
\alpha_{i}
αi：关节轴线
i
−
1
i-1
i−1和关节轴线
i
i
i的夹角，指向为从轴线
i
−
1
i-1
i−1到轴线
i
i
i；
（3）
d
i
d_{i}
di：关节
i
i
i上的两条公垂线
a
i
−
1
a_{i-1}
ai−1 与
a
i
a_{i}
ai之间的距离，沿关节轴线
i
i
i测；
（4）
θ
i
\theta_{i}
θi：连杆
i
i
i相对于连杆
i
−
1
i-1
i−1绕轴线
i
i
i的旋转角度。
齐次变换矩阵：
从坐标系
i
−
1
i-1
i−1到坐标系
i
i
i，先绕
Z
i
−
1
Z_{i-1}
Zi−1轴旋转角度
θ
i
\theta_{i}
θi，再沿
Z
i
−
1
Z_{i-1}
Zi−1轴移动
d
i
d_{i}
di，然后沿
X
i
X_i
Xi轴平移
a
i
a_i
ai，最后绕
X
i
X_i
Xi轴旋转
α
i
\alpha_i
αi。齐次变换矩阵
i
−
1
i
T
_ { i - 1 } ^ { i } T
i−1iT可以写为：
2.2 改进D-H法（MDH）
建模规则：
（1）找出各关节轴，画出关节轴的延长线；
（2）确定
Z
Z
Z轴：与关节轴线重合，其中
Z
i
Z_{i}
Zi轴与关节
i
i
i的轴线重合；
（3）确定
X
X
X轴：
Z
i
Z_{i}
Zi轴与
Z
i
+
1
Z_{i+1}
Zi+1轴的公垂线，方向由
Z
i
Z_{i}
Zi轴指向
Z
i
+
1
Z_{i+1}
Zi+1轴；
（4）确定
Y
Y
Y轴：右手定则。
D-H参数含义：
（1）
a
i
−
1
a_{i-1}
ai−1：沿
X
i
−
1
X_{i-1}
Xi−1轴，从
Z
i
−
1
Z_{i-1}
Zi−1轴移动到
Z
i
Z_{i}
Zi轴的距离；
（2）
α
i
−
1
\alpha_{i-1}
αi−1：绕
X
i
−
1
X_{i-1}
Xi−1轴，从
Z
i
−
1
Z_{i-1}
Zi−1轴旋转到
Z
i
Z_{i}
Zi轴的角度；
（3）
d
i
d_{i}
di：沿
Z
i
Z_{i}
Zi轴，从
X
i
−
1
X_{i-1}
Xi−1轴移动到
X
i
X_{i}
Xi轴的距离；
（4）
θ
i
\theta_{i}
θi：绕
Z
i
Z_{i}
Zi轴，从
X
i
−
1
X_{i-1}
Xi−1轴旋转到
X
i
X_{i}
Xi轴的角度；
齐次变换矩阵：
从坐标系
i
−
1
i-1
i−1到坐标系
i
i
i，先绕
X
i
−
1
X_{i-1}
Xi−1轴旋转角度
α
i
−
1
\alpha_{i-1}
αi−1，再沿
X
i
−
1
X_{i-1}
Xi−1轴平移
a
i
−
1
a_{i-1}
ai−1，然后绕
Z
i
Z_{i}
Zi轴旋转角度
θ
i
\theta_{i}
θi，最后沿
Z
i
Z_{i}
Zi轴移动
d
i
d_{i}
di。齐次变换矩阵
i
−
1
i
T
_ { i - 1 } ^ { i } T
i−1iT可以写为：
2.3 SDH和MDH的区别及适用场景
2.3.1 SDH和MDH的区别
（其实一直以来笔者都不太能区分这两种方法，笔者最开始学习机器人学是看的《机器人学导论》这本书，上面似乎是偏向于MDH方法，所以后续我都是习惯于用MDH去建模）
（1）区别一：连杆坐标系建立的位置不同。SDH将连杆
i
i
i的坐标系固定在连杆的远端；MDH将连杆
i
i
i的坐标系固定在连杆的近端。
（2）区别二：执行变换的顺序不同。
2.3.2 SDH和MDH的适用场景
对于树形结构或者闭链机构的机器人来说，按照SDH方法建立的连杆坐标系会产生歧义，因为SDH的建系原则是把连杆
i
i
i的坐标系建立在连杆的远端，如图(a)所示，导致连杆0上同时出现了两个坐标系。而MDH把连杆坐标系建立在每个连杆的近端，则不会坐标系重合的情况，如图(b)所示，这就克服了SDH方法建系的缺点。
总结：
（1）SDH适合应用于开链结构的机器人；
（2）当使用SDH表示树状或闭链结构的机器人时，会产生歧义；
（3）MDH法对开链、树状、闭链结构的机器人都适用。
三、机器人运动学
3.1 创建一个连杆对象
在机器人工具箱中，还用变量
σ
i
\sigma _ { i }
σi表示机器人的关节类型，
σ
i
=
0
\sigma _ { i } = 0
σi=0表示转动关节，
σ
i
=
1
\sigma _ { i } = 1
σi=1表示移动关节(若未指定该参数，默认为转动关节)。
在工具箱中，用函数Link( )可以创建一个机械臂对象，其中输入的参数顺序分别是：关节角
θ
i
\theta _ { i }
θi、连杆偏距
d
i
d_{i}
di、连杆长度
a
i
−
1
a_{i-1}
ai−1、连杆转角
α
i
−
1
\alpha_{i-1}
αi−1和关节类型。例如，创建一个关节角为30°，连杆偏距为0.2m，连杆长度为0.5m，连杆转角为60°，关节类型为旋转关节的连杆，代码如下所示：
L = Link([pi/6, 0.2, 0.5, pi/3, 0])
- 1
这里，Revolute表示转动关节，std表示标准D-H参数法，offset表示关节偏移量。可以通过以下命令获得连杆的各个参数：
● 获取连杆的关节类型：L.type( )（注意：老版本的工具箱是L.RP）;
● 获取连杆的关节角：L.theta;
● 获取连杆的连杆偏距：L.d;
● 获取连杆的连杆长度：L.a;
● 获取连杆的连杆转角：L.alpha。
● 获取改连杆的齐次变换矩阵：L.A(
θ
\theta
θ)，例如连杆转动了30°，其齐次变换矩阵T为：
3.2 创建一个平面3-DOF的机械臂
这里创建了一个平面三R机构（三个转动关节），该平面三连杆机构的DH参数表如下所示：
% standard-表示标准DH法
% theta(z) d(z) a(x) alpha(x)
RRR_L(1) = Link([ 0 0 1 0 ],'standard');
RRR_L(2) = Link([ 0 0 0.8 0 ],'standard');
RRR_L(3) = Link([ 0 0 0.6 0 ],'standard');
- 1
- 2
- 3
- 4
- 5
运行结果：
通过构造函数SerialLink( )可以给创建的机械臂对象命名，并显示出对象的信息。输入命令:
three_link = SerialLink(RRR_L, 'name', 'three-link-RRR')
- 1
运行结果：
用以下的命令可以获取已创建机械臂的各个参数：
运行结果：
同时也可以对创建的机械臂对象进行复制，如复制一个名称为“three_link2”的机械臂输人命令:
three_link2 = SerialLink(RRR_L, 'name', 'three-link-RRR_2')
- 1
运行结果：
使用teach( )函数，可以对创建的机器人进行示教，如图所示：
3.3 机器人正运动学
机器人正运动学即给定一组关节角，计算出机器人末端相对于基坐标系的位置和姿态，如下图：
机器人工具箱中，fkine( )可以进行正运动学计算，即给定关节变量，得到末端坐标系关于基坐标系的齐次变换矩阵（位置和姿态）。这里以3.2节的3-DOF平面机械臂为例，当机器人三个关节角都为0°时：
% RRR机械臂
clear;
close all;
clc;
% theta(z) d(z) a(x) alpha(x)
RRR_L(1) = Link([ 0 0 1 0 ],'standard');
RRR_L(2) = Link([ 0 0 0.8 0 ],'standard');
RRR_L(3) = Link([ 0 0 0.6 0 ],'standard');
three_link = SerialLink(RRR_L, 'name', 'three-link-RRR');
q0 = [0 0 0]'*pi/180;
T0 = three_link.fkine(q0)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
运行结果：
当机器人一关节10°，二关节-30°，三关节60°时，机器人末端的位置和姿态如图：
可以用plot( )函数绘制此时机器人的状态，如图：
例子：给定平面3-RRR机械臂各个关节的角度如图所示，求解机器人运动过程中末端点的三维坐标
% RRR机械臂 clear; close all; clc; % theta(z) d(z) a(x) alpha(x) RRR_L(1) = Link([ 0 0 1 0 ],'standard'); RRR_L(2) = Link([ 0 0 0.8 0 ],'standard'); RRR_L(3) = Link([ 0 0 0.6 0 ],'standard'); three_link = SerialLink(RRR_L, 'name', '3-DOF'); t = 0:0.05:4; m = length(t); theta1 = 120*sin(4*pi*t/4); theta2 = 60*sin(2*pi*t/4); theta3 = 30*sin(2*pi*t/4); q = [theta1;theta2;theta3]*pi/180; for i = 1:m T = three_link.fkine(q(:,i)'); x(i) = T.t(1); y(i) = T.t(2); z(i) = T.t(3); subplot(3,3,[1 4 7]) hold on plot3(x(i),y(i),z(i),'r*'); hold on three_link.plot(q(:,i)'); subplot(3,3,2) hold on plot(t(i),q(1,i)*180/pi,'b.') xlabel('time /s') ylabel('\theta_1 /deg') subplot(3,3,5) hold on plot(t(i),q(2,i)*180/pi,'r.') xlabel('time /s') ylabel('\theta_2 /deg') subplot(3,3,8) hold on plot(t(i),q(3,i)*180/pi,'m.') xlabel('time /s') ylabel('\theta_3 /deg') subplot(3,3,3) hold on plot(t(i),x(i),'b.') xlabel('time /s') ylabel('x /m') subplot(3,3,6) hold on plot(t(i),y(i),'r.') xlabel('time /s') ylabel('y /m') subplot(3,3,9) hold on plot(t(i),z(i),'m.') xlabel('time /s') ylabel('z /m') end
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
运行结果：
3.4 机器人逆运动学
机器人逆运动学即给定机器人末端相对于基坐标系的位置和姿态，求解出关节角度。
机器人工具箱中，ikine( )可以进行逆运动学计算，即给定关节变量，得到末端坐标系关于基坐标系的齐次变换矩阵（位置和姿态）。这里同样以3.2节的3-DOF平面机械臂为例，当机器人三个关节角分别为0°、30°和60°时：
% RRR机械臂 clear; close all; clc; % theta(z) d(z) a(x) alpha(x) RRR_L(1) = Link([ 0 0 1 0 ],'standard'); RRR_L(2) = Link([ 0 0 0.8 0 ],'standard'); RRR_L(3) = Link([ 0 0 0.6 0 ],'standard'); three_link = SerialLink(RRR_L, 'name', 'three-link-RRR'); q0 = [0 30 60]'*pi/180; three_link.plot(q0'); T0 = three_link.fkine(q0) % 运动学逆解 mask_vector = [1,1,0,0,0,1]; q_1 = three_link.ikine(T0,'mask',mask_vector)*180/pi T1 = three_link.fkine(q_1*pi/180)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
运行结果：
从图中可以看到，运动学逆解正确。需要注意：代码中出现了mask_vector这个向量，是因为我们这里的是三自由度机器人，属于欠驱动机器人，需要添加额外的掩膜向量参数。该向量前三个元素表示机器人末端执行器分别有相对于基坐标系的x、y、z轴的移动，后三个元素表示机器人末端执行器分别有相对于基坐标系的x、y、z轴的转动。我们的这个平面三自由度机器人相对于基坐标系，只有x、y轴的移动和z轴的转动，因此掩膜向量为[1, 1, 0, 0, 0, 1]
例子：给定平面3-RRR机械臂各个关节的角度，判断机器人运动过程中逆解是否正确。
% RRR机械臂 clear; close all; clc; % theta(z) d(z) a(x) alpha(x) RRR_L(1) = Link([ 0 0 1 0 ],'standard'); RRR_L(2) = Link([ 0 0 0.8 0 ],'standard'); RRR_L(3) = Link([ 0 0 0.6 0 ],'standard'); three_link = SerialLink(RRR_L, 'name', '3-DOF'); t = 0:0.05:4; m = length(t); theta1 = 10*sin(4*pi*t/4); theta2 = 20*sin(2*pi*t/4); theta3 = 30*sin(2*pi*t/4); q = [theta1;theta2;theta3]*pi/180; for i = 1:m T0 = three_link.fkine(q(:,i)'); mask_vector = [1,1,0,0,0,1]; q_1(:,i) = three_link.ikine(T0,'mask',mask_vector)*180/pi; end figure(2) subplot(3,1,1) plot(t,theta1-q_1(1,:),'b') xlabel('time (s)') ylabel('\Delta\theta_1 (deg)') subplot(3,1,2) plot(t,theta2-q_1(2,:),'b') xlabel('time (s)') ylabel('\Delta\theta_2 (deg)') subplot(3,1,3) plot(t,theta3-q_1(3,:),'b') xlabel('time (s)') ylabel('\Delta\theta_3 (deg)') set(gca, 'FontName','Times New Roman') set(gcf, 'color',[1 1 1]);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
运行结果：
可以看到前后误差很小，表明运动学逆解成功！！
四、雅可比矩阵
雅可比矩阵是机械臂位姿的函数，可实现关节速度和机器人末端执行器速度之间的映射、机器人末端执行器所受静力和各关节力矩之间的映射。
式中，
ξ
e
n
d
=
[
ν
x
,
ν
y
,
ν
z
,
ω
x
,
ω
y
,
ω
z
]
T
\boldsymbol{\xi} _ { e n d } = \left[ \nu _ { x } ,\; \nu _ { y }, \; \nu _ { z },\; \omega _ { x },\; \omega _ { y },\; \omega _ { z } \right] ^ { \mathrm{T} }
ξend=[νx,νy,νz,ωx,ωy,ωz]T表示机器人末端执行器的广义速度，包括末端点的速度矢量和末端执行器的角速度矢量；
J
(
q
)
∈
R
6
×
N
\boldsymbol{J} ( q ) \in \mathrm{R} ^ { 6 \times N }
J(q)∈R6×N表示雅可比矩阵，其中N表示关节个数；
q
˙
∈
R
N
\dot {\boldsymbol{q}} \in \mathrm{R} ^ { N }
q˙∈RN表示关节速度。
机器人工具箱提供了两个用于求解雅可比矩阵的函数：jacob0( )函数和jacobn( )函数。
（1）jacob0( )函数求解的雅可比矩阵是将关节速度映射到世界坐标系中的末端执行器广义速度；
（2）jacobn( )函数求解的雅可比矩阵是将关节速度映射到末端执行器自身坐标系中的广义速度；
个人理解的是jacob0( )函数求解的雅可比矩阵是在jacobn( )函数求解的雅可比矩阵的基础上，附加了一个从末端执行器坐标系到世界坐标系的速度变换，笔者更习惯用jacob0( )函数。
例子：给定平面3-RRR机械臂各个关节的角度、速度，判断机器人运动过程中利用雅可比矩阵算得末端执行器的速度是否正确。
%% 雅可比 % RRR机械臂 clear; close all; clc; % theta(z) d(z) a(x) alpha(x) RRR_L(1) = Link([ 0 0 1 0 ],'standard'); RRR_L(2) = Link([ 0 0 0.8 0 ],'standard'); RRR_L(3) = Link([ 0 0 0.6 0 ],'standard'); three_link = SerialLink(RRR_L, 'name', '3-DOF'); t = 0:0.05:4; m = length(t); theta1 = 120*sin(4*pi*t/4); theta2 = 60*sin(2*pi*t/4); theta3 = 30*sin(2*pi*t/4); q = [theta1;theta2;theta3]*pi/180; theta1_d = diff(theta1*pi/180)./0.05; theta2_d = diff(theta2*pi/180)./0.05; theta3_d = diff(theta3*pi/180)./0.05; q_d = [theta1_d; theta2_d; theta3_d]; for i = 1:m T = three_link.fkine(q(:,i)'); x(i) = T.t(1); y(i) = T.t(2); z(i) = T.t(3); J(:,:,i) = three_link.jacob0(q(:,i)'); end v_x = diff(x)./0.05; v_y = diff(y)./0.05; v_z = diff(z)./0.05; for i = 1:m-1 V_end(:,i) = J(:,:,i)*q_d(:,i); end figure(1) subplot(3,1,1) plot(t(1:end-1),v_x,'b') hold on plot(t(1:end-1),V_end(1,:),'r--') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$v_{x}$ (m/s)', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') subplot(3,1,2) plot(t(1:end-1),v_y,'b') hold on plot(t(1:end-1),V_end(2,:),'r--') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$v_{y}$ (m/s)', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') subplot(3,1,3) plot(t(1:end-1),v_z,'b') hold on plot(t(1:end-1),V_end(3,:),'r--') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$v_{z}$ (m/s)', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') set(gcf, 'color',[1 1 1]);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
运行结果：
图中，蓝色线条表示利用正运动学算出的末端执行器的x、y和z轴位置，然后再差分得到的末端速度；红色线条表示利用雅可比矩阵计算得到的末端速度。两者还是有差别的，笔者认为原因是由于差分导致的，因此将时间间隔从原来的0.05s，改成了0.001s后，得到的结果如下所示：
可以看到，两种计算结果几乎完全重合，验证了雅可比矩阵的正确性！！
五、轨迹规划
为了使得机器人末端执行器由初始位姿变换到目标位姿，可以用逆运动学求解出初始位姿和目标位姿所对应的各个关节角度，然后在关节空间进行轨迹规划。（其实在笛卡尔空间进行规划，再利用逆运动学算到关节空间也行）。
机器人工具箱提供了常用的两个轨迹规划函数：jtraj( )函数和ctraj( )函数。
（1）jtraj( )函数用于在关节空间进行插值拟合，格式1：[q, qd, qdd] = jtraj(q0, qf, n)：其中，q0是初始关节角度；qf是终止关节角度；n表示插值点个数；格式2：[q, qd, qdd] = jtraj(q0, qf, T, qd0, qdf)，其中T表示时间，qd0表示初始速度，qdf表示终止速度。
（2）ctraj( )函数用于在关节空间进行插值拟合，格式：tc = ctraj(T0, T1, n)，其中T0为初始末端执行器的位姿矩阵；T1为终止时末端执行器的位姿矩阵；tc为求解出来的一堆姿态变换矩阵。
5.1 关节空间轨迹规划
给定初始位置和终止位置，用jtraj( )函数进行轨迹规划
%% 关节空间轨迹规划 % RRR机械臂 clear; close all; clc; % theta(z) d(z) a(x) alpha(x) RRR_L(1) = Link([ 0 0 1 0 ],'standard'); RRR_L(2) = Link([ 0 0 0.8 0 ],'standard'); RRR_L(3) = Link([ 0 0 0.6 0 ],'standard'); three_link = SerialLink(RRR_L, 'name', '3-DOF'); t = 0:0.1:4; q_0 = [0; 0; 0]; q_f = [45; 60; 90]*pi/180; % 格式1： [q, qd, qdd] = jtraj(q_0, q_f, length(t)); % % 格式2： % qd_0 = [0; 0; 0]; % qd_f = [10; 20; 30]*pi/180; % [q, qd, qdd] = jtraj(q_0, q_f, t, qd_0, qd_f); % 机器人运动动画 figure(1) three_link.plot(q,'trail','b') % 位置、速度、加速度绘图 figure(2) subplot(3,1,1) plot(t, q(:,1)*180/pi, 'b') hold on plot(t, q(:,2)*180/pi, 'r') hold on plot(t, q(:,3)*180/pi, 'm') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$\theta$ (deg)', 'Interpreter', 'latex') legend('$\theta_1$','$\theta_2$','$\theta_3$', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') subplot(3,1,2) plot(t, qd(:,1)*180/pi, 'b') hold on plot(t, qd(:,2)*180/pi, 'r') hold on plot(t, qd(:,3)*180/pi, 'm') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$\dot{\theta}$ (deg)', 'Interpreter', 'latex') legend('$\dot{\theta_1}$','$\dot{\theta_2}$','$\dot{\theta_3}$', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') subplot(3,1,3) plot(t, qdd(:,1)*180/pi, 'b') hold on plot(t, qdd(:,2)*180/pi, 'r') hold on plot(t, qdd(:,3)*180/pi, 'm') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$\ddot{\theta}$ (deg)', 'Interpreter', 'latex') legend('$\ddot{\theta_1}$','$\ddot{\theta_2}$','$\ddot{\theta_3}$', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') set(gcf, 'color',[1 1 1]);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
运行结果：
（1）格式1：
（2）格式2：
5.2 笛卡尔空间轨迹规划
给定机器人末端执行器开始和终止时刻的位姿变换矩阵 T T T，利用ctraj( )函数进行轨迹规划。
%% 笛卡尔空间轨迹规划 % RRR机械臂 clear; close all; clc; % theta(z) d(z) a(x) alpha(x) RRR_L(1) = Link([ 0 0 1 0 ],'standard'); RRR_L(2) = Link([ 0 0 0.8 0 ],'standard'); RRR_L(3) = Link([ 0 0 0.6 0 ],'standard'); three_link = SerialLink(RRR_L, 'name', '3-DOF'); t = 0:0.1:4; q_0 = [0; 0; 0]; q_f = [30; 60; 90]*pi/180; T_0 = three_link.fkine(q_0); T_f = three_link.fkine(q_f); tc = ctraj(T_0, T_f, length(t)); % 末端执行器的x、y和z轴位置变化 for i = 1:length(t) x(i) = tc(i).t(1); y(i) = tc(i).t(2); z(i) = tc(i).t(3); end % 末端位置变化 figure(1) plot(t, x, 'b') hold on plot(t, y, 'r') hold on plot(t, z, 'm') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$x, y, z$ (mm)', 'Interpreter', 'latex') legend('$x$','$y$','$z$', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') set(gcf, 'color',[1 1 1]); % 利用逆运动学算到关节空间 mask_vector = [1,1,0,0,0,1]; q_c = three_link.ikine(tc,'mask',mask_vector)*180/pi; figure(2) three_link.plot(q_c*pi/180,'trail','b') set(gcf, 'color',[1 1 1]); % 关节空间绘图 figure(3) plot(t, q_c(:,1), 'b') hold on plot(t, q_c(:,2), 'r') hold on plot(t, q_c(:,3), 'm') xlabel('time (s)', 'Interpreter', 'latex') ylabel('$\theta$ (deg)', 'Interpreter', 'latex') legend('$\theta_1$','$\theta_2$','$\theta_3$', 'Interpreter', 'latex') set(gca, 'FontName','Times New Roman') set(gcf, 'color',[1 1 1]);
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
运行结果：
（1）笛卡尔空间规划后，机器人末端执行器x、y、z的变化
（2）将在笛卡尔空间规划后的轨迹，利用逆运动学算到关节空间
结语
我是木头人，以上全是个人见解，有问题请大家评论区指出，大家共同进步！！
