1.2.机器人运动学的数学基础

1.2.1三维空间的位置与姿态描述

机器人工具箱用的是robot-9.10的MATLAB机器人工具箱

MATLAB机器人工具箱网址

用的书籍是《机器人仿真与编程技术》清华大学出版社2018年2月第1版

点的位置描述：

AP=(pxpypz)A_{P} = \begin{pmatrix} p_{x}\\ p_{y}\\ p_{z}\\ \end{pmatrix} AP=⎝⎛pxpypz⎠⎞

RBA=(cos(xB,xA)cos(yB,xA)cos(zB,xA)cos(xB,yA)cos(yB,yA)cos(zB,yA)cos(xB,zA)cos(yB,zA)cos(zB,zA))R_{B}^{A} = \begin{pmatrix} cos(x_B,x_A)& cos(y_B,x_A)&cos(z_B,x_A)\\ cos(x_B,y_A)& cos(y_B,y_A)&cos(z_B,y_A)\\ cos(x_B,z_A)& cos(y_B,z_A)&cos(z_B,z_A)\\ \end{pmatrix} RBA=⎝⎛cos(xB,xA)cos(xB,yA)cos(xB,zA)cos(yB,xA)cos(yB,yA)cos(yB,zA)cos(zB,xA)cos(zB,yA)cos(zB,zA)⎠⎞

旋转矩阵对3个坐标轴的矩阵：

Rx(θ)=(1000cosθ−sinθ0sinθcos⁡θ)R_x(\theta) = \begin{pmatrix} 1&0&0 \\ 0&cos\theta&-sin\theta\\ 0&sin\theta&\cos\theta \end{pmatrix} Rx(θ)=⎝⎛1000cosθsinθ0−sinθcosθ⎠⎞

在matlab机器人工具箱中用命令执行相关操作：
rotx(θ),roty(θ),rotz(θ)rotx(\theta),roty(\theta),rotz(\theta) rotx(θ),roty(θ),rotz(θ)
默认是弧度制，如果要切换的话用：rotx(θ,′deg′)rotx(\theta,'deg')rotx(θ,′deg′)

旋转的可视化：

1.trplot(R)trplot(R)trplot(R)：体坐标系显示对应旋转矩阵的坐标系

2.tranimate(R)tranimate(R)tranimate(R):动画展示动画展示世界坐标系转换为体坐标系

带动画效果的图片

3.上述函数还可以对齐次变换矩阵操作，操作的参数如下：

1.2.2 坐标变换

1.matlab工具箱齐次平移命令 transl(px,py,pz)transl(px,py,pz)transl(px,py,pz)：
T=(100px010py001pz0001)T=\begin{pmatrix} 1&0&0&p_x\\ 0&1&0&p_y\\ 0&0&1&p_z\\ 0&0&0&1 \end{pmatrix} T=⎝⎜⎜⎛100001000010pxpypz1⎠⎟⎟⎞
2.3个轴的齐次旋转命令：trotx(θ),troty(θ),trotz(θ)trotx(\theta),troty(\theta),trotz(\theta)trotx(θ),troty(θ),trotz(θ) 例如：
trotx(θ)=(10000cos(θ)−sin(θ)00sin(θ)cos(θ)00001)trotx(\theta) = \begin{pmatrix} 1&0&0&0\\ 0&cos(\theta)&-sin(\theta)&0\\ 0&sin(\theta)&cos(\theta)&0\\ 0&0&0&1 \end{pmatrix} trotx(θ)=⎝⎜⎜⎛10000cos(θ)sin(θ)00−sin(θ)cos(θ)00001⎠⎟⎟⎞
对于坐标系{A},经过齐次变换后的矩阵为{B}，那么：
PB=TABPAP^B=T_A^{B}P^A PB=TABPA
而:
TAB=(RABPBORG01×31)T_{A}^B=\begin{pmatrix} R_{A}^B&P_{BORG}\\ 0_{1\times3}&1\\ \end{pmatrix}TAB=(RAB01×3PBORG1)
3.用matlab机器人工具箱的实现：

用transl(),trotx(),troty(),trotz()实现旋转变换

可用r2t(T)提取旋转矩阵分量

用tranl(T)提取平移变换分量
(p3×1B1)=transl(px,py,pz)trotx(α)troty(β)trotz(γ)(p1×3A1)\begin{pmatrix}p^{B}_{3\times1}\\1\\ \end{pmatrix} =transl(p_x,p_y,p_z)trotx(\alpha)troty(\beta)trotz(\gamma)\begin{pmatrix}p_{1\times3}^{A}\\1\\ \end{pmatrix}(p3×1B1)=transl(px,py,pz)trotx(α)troty(β)trotz(γ)(p1×3A1)

1.2.3姿态的表示方法

1.Euler角坐标系法：

(1)X-Y-Z固定角坐标系

使得{B}沿着XAX_AXA转γ\gammaγ角度，再使得沿YAY_AYA转β\betaβ,再沿着ZAZ_AZA转α\alphaα,得到最终的等价旋转矩阵：
RB,xyzA(γ,β,α)=RZ(α)RY(β)RX(γ)R_{B,xyz}^{A}(\gamma,\beta,\alpha) = R_Z(\alpha)R_Y(\beta)R_X(\gamma) RB,xyzA(γ,β,α)=RZ(α)RY(β)RX(γ)

TBA=trotz(α)troty(β)trotx(γ)T_B^A=trotz(\alpha)troty(\beta)trotx(\gamma) TBA=trotz(α)troty(β)trotx(γ)

γ\gammaγ:回转角

β\betaβ:俯转角

α\alphaα:偏转角

或者直接用rpy2r,例如：

R1 = rpy2r(pi/3,pi/4,pi/2,'zyx');

R1 = rotz(pi/3)*roty(pi/4)*rotx(pi/2);

用tr2rpy求得α,β,γ\alpha,\beta,\gammaα,β,γ

TB = tr2rpy(R1,'zyx');

(2)Z-Y-Z表达法： …

2.向量表达法

…

1.2.4实例

注意：得到每个齐次变换矩阵(主要考虑其旋转部分)的过程中既有左乘又有右乘，如果{B}相对于{A}的轴旋转是沿世界坐标系的旋转，那么就是左乘，若沿着上一个固定轴的旋转就是右乘。

G1 = [0 1 0 2;1 0 0 6;0 0 -1 2;0 0 0 1];
tranimate(G1);
tranimate(G1,'frame','A','color','b');
>> G2 = trotz(pi/2)*G1;
>> G3 = G1*trotz(pi/2);
>> tranimate(G1,G2,'frame','A','color','b');
>> tranimate(G1,G2,'frame','A','color','b');
>> tranimate(G1,G2,'frame','A','color','b');
>> hold on
>> tranimate(G1,G3,'frame','A','color','b');
>> tranimate(G1,G3,'frame','A','color','b');

1.3机器人运动学

1.3.2 DH参数法的机器人工具箱实现

standard：

对于每一个关节iii：

基本参数：

a.连杆长度ai−1a_{i-1}ai−1

b.连杆转角αi−1\alpha_{i-1}αi−1

c.连杆偏距did_{i}di

d.关节角θi\theta_{i}θi

对于转动关节而言：

连杆长度，连杆转角，连杆偏距固定不变。

对于移动关节而言：

连杆长度，连杆转角，关节角固定不变。

用**Link()**表示构建的一个机械臂单元格式：L=Link([θi,di,ai−1,αi−1,σi])L=Link([\theta_i,d_i,a_{i-1},\alpha_{i-1},\sigma_{i}])L=Link([θi,di,ai−1,αi−1,σi])
机械臂结构体的参数获取：

获取连杆的关节类型：L.RPL.RPL.RP

获取连杆的连杆偏距：L.dL.dL.d

获取连杆的关节角：L.thetaL.thetaL.theta

获取连杆的连杆长度：L.aL.aL.a

获取连杆的连杆转角：L.alphaL.alphaL.alpha
- 对机械臂对象命名：SerialLink()SerialLink()SerialLink()可以对创建的机械臂命名。
```
three_Link = SerialLink(L,'name','threelink');
```
- 访问机械臂的基本结构参数：
1.3.3机器人正运动学

建立标准坐标系的步骤

1.确定两个关节轴方向，分别为坐标系i和i−1i和i-1i和i−1的ZZZ轴，为ZiZ_{i}Zi和Zi−1Z_{i-1}Zi−1。

2.将ZiZ_{i}Zi和Zi−1Z_{i-1}Zi−1的公垂线方向定义为iii坐标系的XXX轴XiX_{i}Xi，而公垂线上与ZiZ_{i}Zi的交点作为原点OiO_iOi。

同理确定Xi−1X_{i-1}Xi−1和Oi−1O_{i-1}Oi−1。

3.通过右手定则，由XiX_{i}Xi和ZiZ_{i}Zi确定YiY_{i}Yi:Yi=Xi×ZiY_{i} = X_{i}\times Z_{i}Yi=Xi×Zi

DH参数的确定

其中a0=0a_0 = 0a0=0,α0=0\alpha_0 = 0α0=0。

ai−1a_{i-1}ai−1:xix_ixi轴方向上ziz_izi和zi+1z_{i+1}zi+1轴之间的距离。

…

坐标系的齐次变换矩阵

Ti−1i=(cosθi−sinθicosαi−1sinθisinαi−1ai−1cosθisinθicosθicosαi−1−cosθisinαi−1ai−1sinθi0sinαi−1cosαi−1di0001)T_{i-1}^{i}=\begin{pmatrix} cos\theta_i & -sin\theta_icos\alpha_{i-1} & sin\theta_isin\alpha_{i-1} & a_{i-1}cos\theta_i \\ sin\theta_i & cos\theta_icos\alpha_{i-1} & -cos\theta_isin\alpha_{i-1} & a_{i-1}sin\theta_i \\ 0 & sin\alpha_{i-1} & cos\alpha_{i-1} & d_i\\ 0 & 0& 0&1 \end{pmatrix} Ti−1i=⎝⎜⎜⎛cosθisinθi00−sinθicosαi−1cosθicosαi−1sinαi−10sinθisinαi−1−cosθisinαi−1cosαi−10ai−1cosθiai−1sinθidi1⎠⎟⎟⎞

用L.A(θiordi)L.A(\theta_{i} \ or\ d_i)L.A(θi or di)可求出连杆变换矩阵。

实现坐标系旋转60°的代码：
```
L(1) = Link([0,0,1,0]);
x1 = L(1).A(0);
x2 = L(1).A(pi/3);
trplot(x1,'frame','A','color','b');hold on
trplot(x2,'frame','B','color','r');
axis([-2 2 -2 2]);
```
相关的的图片：

异常重要的一段话

通过将每一个连杆的变换矩阵连乘能得到坐标{N}相对于坐标{0}的变换矩阵：
TN0=T10T21...Ti+1i...TNN−1T_N^0=T_1^0T_2^1...T_{i+1}^i...T_N^{N-1} TN0=T10T21...Ti+1i...TNN−1

matlab的实现过程

用L.fkine([θ1θ2θ3])L.fkine([\theta_1\ \theta_2\ \theta_3])L.fkine([θ1 θ2 θ3])实现对连杆的旋转不同角度得到齐次变换矩阵的操作
用L.plot([θ1θ2θ3])L.plot([\theta_1\ \theta_2\ \theta_3])L.plot([θ1 θ2 θ3])实现对连杆的旋转不同角度得到图像的操作

代码

L(1) = Link([0,0,1,0]);
L(2) = Link([0,0,0.8,0]);
L(3) = Link([0 0 0.6 0]);
three_Link_robot = SerialLink(L,'name','threelink');
T = three_Link_robot.fkine([0 pi/6 pi/4]);
three_Link_robot.plot([pi/2 0 0]);

1.3.3机器人逆向运动学

matlab工具箱数值方法

用qn=L.ikine(T,inital_value)q_n = L.ikine(T,inital\_value)qn=L.ikine(T,inital_value)实现对已知位姿的齐次矩阵和初始位置进行求解机械臂角度的操作

>> mdl_puma560
>> qn = [0 pi/2 pi/3 pi/4 pi/5 pi/6];
>> T_n = p560.fkine(qn);
>> T_nT_n =-0.3774    0.9250   -0.0446   -0.23350.8490    0.3263   -0.4156   -0.1501-0.3699   -0.1947   -0.9084    0.06800         0         0    1.0000>> p560.ikine(T_n)
警告: Initial joint configuration results in a (near-)singular configuration, this may slow convergence
> In SerialLink.ikine at 156
警告: ikine: iteration limit 1000 exceeded (row 1), final err 0.439728
> In SerialLink.ikine at 179 ans =-0.0000   -2.1376    2.1884   -1.1571   -2.7185    0.0461>> x1 =ans;
>> x2 = p560.ikine(T_n,[0 0 3 0 0 0]);
警告: Initial joint configuration results in a (near-)singular configuration, this may slow convergence
> In SerialLink.ikine at 156
警告: solution diverging at step 919, try reducing alpha
> In SerialLink.ikine at 260
警告: ikine: iteration limit 1000 exceeded (row 1), final err 0.633709
> In SerialLink.ikine at 179
>> p560.plot(x1)
>> p560.plot(x2)

用的是puma560机器人

1.4机械臂的瞬态运动学

机械臂的位姿xxx与关节变量qqq,有个函数关系：
x=f(q)x = f(q) x=f(q)

(x1x2...xm)=(f1(q)f2(q)...fm(q))\begin{pmatrix}x_1 \\x_2 \\... \\x_m \end{pmatrix}=\begin{pmatrix}f_1(q)\\f_2(q)\\... \\f_m(q) \end{pmatrix} ⎝⎜⎜⎛x1x2...xm⎠⎟⎟⎞=⎝⎜⎜⎛f1(q)f2(q)...fm(q)⎠⎟⎟⎞

若对两边计算相应的速度，需要微分和偏微分，用矩阵形式表达：
KaTeX parse error: Undefined control sequence: \part at position 47: …pmatrix} \frac{\̲p̲a̲r̲t̲ ̲f_1}{\part q_1}…

若进行化简，那么：
x˙=J(q)q˙\boldsymbol{\dot{x}}=J(\boldsymbol{q})\boldsymbol{\dot{q}} x˙=J(q)q˙

(vxvyvzωxωyωz)=J0(6×n)q˙n×1\begin{pmatrix} v_x\\ v_y\\ v_z\\ \omega_x\\ \omega_y\\ \omega_z\\ \end{pmatrix} = \boldsymbol{J}_{0\ (6\times n)}\boldsymbol{\dot{q}}_{n\times1} ⎝⎜⎜⎜⎜⎜⎜⎛vxvyvzωxωyωz⎠⎟⎟⎟⎟⎟⎟⎞=J0 (6×n)q˙n×1

而J\boldsymbol{J}J为基本雅可比矩阵：
J=(JvJw)\boldsymbol{J}= \begin{pmatrix} \boldsymbol{J_v}\\ \boldsymbol{J_w} \end{pmatrix}J=(JvJw)
Jv\boldsymbol{J_v}Jv：联系关节角速度和执行器末端线速度

Jω\boldsymbol{J_{\omega}}Jω：联系关节角速度和执行器末端角速度

而在齐次变换矩阵TTT中，Px,Py,PzP_x,P_y,P_zPx,Py,Pz是末端执行器在世界坐标系上的位置。

若将上述三者统一成变量xpx_pxp:
xp=(PxPyPz)x_p=\begin{pmatrix}P_x\\P_y\\P_z\end{pmatrix} xp=⎝⎛PxPyPz⎠⎞
可以得到：
KaTeX parse error: Undefined control sequence: \part at position 34: …m_{i=1}^n\frac{\̲p̲a̲r̲t̲ ̲x_p}{\part q_n}…
所以可以得到：
KaTeX parse error: Undefined control sequence: \part at position 42: …matrix} \frac{\̲p̲a̲r̲t̲ ̲x_p}{\part q_1}…
而对于JωJ_\omegaJω：
Jω=(ϵ1z1,ϵ2z2,...,ϵnzn)\boldsymbol{J_\omega}=\begin{pmatrix} \epsilon_1z_1,\epsilon_2z_2,...,\epsilon_nz_n \end{pmatrix} Jω=(ϵ1z1,ϵ2z2,...,ϵnzn)
对于每一个关节，ϵi\epsilon_iϵi=0时为移动关节，当ϵi\epsilon_iϵi=1时为转动关节

而ziz_izi表示T0iT^{i}_0T0i中的第三列的三个变量
zi=(r13r23r33)rij∈Ti0z_i=\begin{pmatrix} r_{13}\\ r_{23}\\ r_{33} \end{pmatrix}\\ r_{ij} \in T_{i}^0 zi=⎝⎛r13r23r33⎠⎞rij∈Ti0

matlab的实现：

雅可比矩阵的函数语言：

世界坐标系下的雅可比矩阵

J=SerialLink.jacb0([θ1,θ2,...θn])J = SerialLink.jacb0([\theta_1,\theta_2,...\theta_n]) J=SerialLink.jacb0([θ1,θ2,...θn])

>>>mdl_kR5
q = [0 pi/4 pi 0 pi/4 0];
J0 = KR5.jacob0(q);

机器人仿真技术学习笔记(一)相关推荐

机器人仿真技术学习笔记(二)
机器人的动力学和多项式路径规划 1.刚体机器人加速度 2.机器人刚体的质量分布 3.牛顿欧拉递推动力学方程 4.状态空间方程 1.定义 2. P u m a 560 Puma560 Puma560参数 ...
智能网联汽车自动驾驶仿真技术学习笔记（一）
智能网联汽车自动驾驶仿真技术学习笔记(一) 绪论一.自动驾驶汽车分级二.先进驾驶辅助系统三.环境感知传感器四.自动驾驶仿真系统的构成五.自动驾驶功能测试绪论一.自动驾驶汽车分级以下是由 ...
知识图谱技术学习笔记
知识图谱技术学习笔记从一开始的Google搜索,到现在的聊天机器人.大数据风控.证券投资.智能医疗.自适应教育.推荐系统,无一不跟知识图谱相关.它在技术领域的热度也在逐年上升. 本文以通俗易懂的方式 ...
java mvc框架代码_JAVA技术学习笔记：SpringMVC框架（内附入门程序开发代码）
原标题:JAVA技术学习笔记:SpringMVC框架(内附入门程序开发代码) JavaEE体系结构包括四层,从上到下分别是应用层.Web层.业务层.持久层.Struts和SpringMVC是Web层的 ...
小五思科技术学习笔记之SSH
下面介绍一下相关概念: SSH的概念: SSH的英文全称为Secure Shell,SSH使用TCP的22号端口,其目的是要在非安全的网络上提供安全的远程登陆和其他安全的网络服务, 为什么要使用SSH ...
ssm 转发请求_千呼万唤！阿里内部终于把这份SSM框架技术学习笔记分享出来了...
SSM SSM(Spring+SpringMVC+MyBatis)框架集由Spring.MyBatis两个开源框架整合而成(SpringMVC是Spring中的部分内容).常作为数据源较简单的web项 ...
SpringBoot整合第三方技术学习笔记（自用）
SpringBoot整合第三方技术学习笔记搬运黑马视频配套笔记 KF-4.数据层解决方案 KF-4-1.SQL 回忆一下之前做SSMP整合的时候数据层解决方案涉及到了哪些技术?MySQL数据库与My ...
深度强化学习控制六足机器人移动个人学习笔记（一）
深度强化学习控制六足机器人移动个人学习笔记(一) 注意事项 ubuntu18对应的ros版本不是Kinect gym算法执行中部分包要求Python不低于3.6 conda虚拟环境安装gym-gaze ...
Feko仿真偶极子天线学习笔记
Feko仿真偶极子天线学习笔记文章目录 1. 新建工程 2. 创建模型 3. 激励方式设置 4. 扫频设置 5. 网格剖分设置 6. 设计检查,仿真分析 7. 数据后处理,查看结果设计要求:仿真工 ...

机器人仿真技术学习笔记(一)