机器人理论课程笔记

机器人理论

介绍

指路B站机器人理论课程

结构：

空间变换

坐标系变换

位置平移${}^A{P}_{B\_origin}$ & 位姿旋转${}^A{R}_B$

其中平移可交换，旋转非同轴不可交换

平移 旋转

机器人关节变换

$x:n,y:o,z:a$

${}^A{P}_2=T\ast {}^A{R}_1$

$Inversion:{}^B{T}_A={}^A{T}_B^{-1}$

前/后乘法

后乘法($postmultiply$)：$Y=A\times B$ 变换B时对A的坐标轴frame of A（机器臂采用）

前乘法($premultiply$)：$Y=B\times A$ 变换B时对基座坐标轴base coordinate frame

旋转矩阵

旋转变换关系$R_K(\theta )T=C{R}_Z(\theta )X=C{R}_Z(\theta )C^{-1}T$

其中：地面T，坐标系C，二者变换关系$T=CX$，C的Z轴在T中是K轴

可解出系数K

三轴PRY旋转：$RPY(\varphi ,\theta ,\psi )=R_Z(\varphi )R_Y(\theta )R_X(\psi )$

圆柱坐标Cylindrical Coordinates

$cyl(z,\alpha ,\gamma )=trans(0,0,z)\ast R_Z(\alpha )\ast Trans(\gamma ,0,0)$

球坐标Spherical Coordinates

$S(\alpha ,\beta ,\gamma )=R_Z(\alpha )\ast R_Y(\gamma )\ast Trans(0,0,\gamma )$

运动学

关注位置，速度，加速度，不考虑受力分析

运动方式：旋转Revolute 平移Prismatic

DH模型

六个参数$\Rightarrow$四个参数

关节角$\theta$：关节旋转角度（关节旋转变量），${\theta }_i$为$x_i$轴与$x_{i-1}$轴夹角

连杆长度$a$：两相邻关节轴的公垂线长度（固定参数），$a_{i-1}$为$z_i$轴与$z_{i-1}$轴公垂线长度

连杆偏置$d$：两相邻x轴的距离（关节平移变量），$d_i$为$x_i$轴与$x_{i-1}$轴距离

连杆扭转角$\alpha$：连杆两端关节扭转角度（固定参数），${\alpha }_{i-1}$为$z_i$轴与$z_{i-1}$轴夹角

可计算相邻关节坐标变化关系：

对地面$T_n=A_1{A}_2{A}_3...A_n=\left[\begin{array}{cccc}{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{array}\right]=\left[\begin{array}{cc}r& p\\ 0& 1\end{array}\right]$，其中n为连杆link数

逆运动学IK

Inverse Kinematics

通过末端Tool Frame对应的$T_n$求解${\theta }_1$ ~ ${\theta }_n$

工作空间Workspace：所有末端Tool能够达到的位置

Dexterous Workspace：能够向所有方向运动的末端位置$\Rightarrow$灵活

Configuration：使末端到达一个点的方式（IK的解集）

反解算方式：代数法Algebraic(矩阵乘法解方程) vs 几何法Geometric(空间求解，四维以上需要拆分空间)

计算

三轴机器人中，为保证三个自由度，只能用矩阵第四列position求解(P6 49:46 没太听明白)

将$A_1^{-1}$提前，$A_1^{-1}{T}_3=A_2{A}_3$简化第四列计算

六轴机器人拆分成一二三轴确定位置，四五六轴球形轴确定方向

其中e表示end-effector，w表示wrist

通过末端位姿导出$h_y$、$h_z$，得到w坐标，得到${}^w{h}_y$、${}^w{h}_z$，通过${}^w{h}_z$确定${\theta }_4$、${\theta }_5$使其影响被消除，通过${}^w{h}_y$确定${\theta }_6$

逆运动学对重复率、精确度非常重要 $\Rightarrow$ 校正

微分关系

速度

坐标系变换：绝对=相对+牵连

极坐标：

$\vec{x}=r\overrightarrow{e_r}$

$\vec{v}=\frac{d\vec{x}}{dt}=\frac{dr}{dt}\overrightarrow{e_r}+r\frac{d\overrightarrow{e_r}}{dt}=\dot{r}\overrightarrow{e_r}+\omega r\overrightarrow{e_{\theta }}$

$\vec{a}=\frac{d\vec{v}}{dt}=\frac{d\dot{r}}{dt}\overrightarrow{e_r}+\dot{r}\frac{d\overrightarrow{e_r}}{dt}+\frac{d\omega }{dt}r\overrightarrow{e_{\theta }}+\omega \frac{dr}{dt}\overrightarrow{e_{\theta }}+\omega r\frac{d\overrightarrow{e_{\theta }}}{dt}=(\ddot{r}-{\omega }^{2}r)\overrightarrow{e_r}+(2\omega \dot{r}+\dot{\omega }r)\overrightarrow{e_{\theta }}$

矩阵：

平动${}^A{V}_Q=\frac{d^{A}Q}{dt}={}^A{(}^B{V}_Q)+{}^A{V}_{Borg}=\frac{{}_B^{A}R{d}^{B}Q}{dt}+{}^A{V}_{Borg}={}_B^{A}R{}^B{V}_Q+{}^A{V}_{Borg}$

旋转${}^A{V}_Q={}^A{(}^B{V}_Q)+{}^A{\Omega }_B\times {}^{A}Q={}_B^{A}R{}^B{V}_Q+{}^A{\Omega }_B\times {(}^A{R}_B{}^{B}Q)$

总速度

KaTeX parse error: \tag works only in display equations

若连杆$i+1$相对于连杆$i$的加速度绕$Z_i$轴，用矩阵$P_{i+1}^{\ast }=P_{i+1}-P_i$表示连杆长度

角速度$W_{i+1}=W_i+W_s=W_i+\overrightarrow{Z_i}\dot{q_{i+1}}$

连杆$i+1$速度

KaTeX parse error: \tag works only in display equations

Jacobian矩阵

在向量微积分中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式称为雅可比行列式。

上图中端点位置坐标KaTeX parse error: \tag works only in display equations

雅各比矩阵$J=\left[\begin{array}{cc}\frac{\partial X}{\partial {\theta }_1}& \frac{\partial X}{\partial {\theta }_2}\\ \frac{\partial Y}{\partial {\theta }_1}& \frac{\partial Y}{\partial {\theta }_2}\end{array}\right]$是函数$X,Y$对于${\theta }_1,{\theta }_2$的偏导数矩阵

则有微分形式$dV=\left[\begin{array}{c}dX\\ dY\end{array}\right]=J\left[\begin{array}{c}d{\theta }_1\\ d{\theta }_2\end{array}\right]$，简写为$dV=Jd\theta$

本课程中表示为$dX=Jdq$

其中$X=f(q)$，$dX=[dxdydz\delta x\delta y\delta z{]}^T$，$dq=[d{\theta }_{1}d{\theta }_1...d{\theta }_n{]}^T$

$\frac{dX}{dt}=\frac{df(q)}{dt}=\frac{df(q)}{dq}\times \frac{dq}{dt}$可得到$\dot{X}=J\dot{q}$

$POS=BASE\ast T_N\ast TOOL$

解$J^{-1}$可以得到$dq=J^{-1}dX$，或者找微分关系求$d{\theta }_i$

$J^{wrist}=\left[\begin{array}{cc}{J}_1& 0\\ J_2& 0\end{array}\right]$，$J^{-1}=[J_1{]}^{-1}[J_3{]}^{-1}$

神经网络算法求解IK或者$J^{-1}$：相近输入会导致相近输出；规则越多越精细但计算复杂

微分

$T+dT=Trans(dx,dy,dz)\ast Rot(k,d\theta )\ast T$得到${\Delta }^{\ast }T$

$d\theta$小角近似得

若$Rot(k,d\theta )$表示为$Rot(x,\delta x)\ast Rot(y,\delta y)\ast Rot(z,\delta z)$

其中$\delta x=k_{x}d\theta$，$\delta y=k_{y}d\theta$，$\delta z=k_{z}d\theta$

在六轴机器人中$T_n=A_1{A}_2{A}_3...A_n$

旋转轴：(1)式对关节角求微分

平行轴：(1)式对连杆偏置求微分

${}^{i-1}{\Delta }_i$：旋转轴${}^{i-1}{d}_i=0$，${}^{i-1}{\delta }_i=[001{]}^T$；平行轴${}^{i-1}{\delta }_i=0$，${}^{i-1}{d}_i=[001{]}^T$

$$\begin{gathered} T_n+d{T}_n=A_1\ast A_2...\ast (A_i+d{A}_i)\ast ...\ast A_n \\ =T_n+(A_1\ast A_2...\ast A_{i-1}\ast {}^{i-1}{\Delta }_i{A}_i\ast A_{i+1}\ast ...\ast A_n)d{q}_i \end{gathered}$$

写成形式：
$$d{T}_n=T_n\ast {}^{T_n}{\Delta }_i\ast d{q}_i\text{\hspace{1em}(4)}$$
则有${}^{T_n}{\Delta }_i=U_{i-1}^{-1}\ast {}^{i-1}{\Delta }_i\ast U_{i-1}$，其中$U_{i-1}=A_i\ast \dots \ast A_n$

代入旋转轴和平行轴(P10 27.04 没看懂)

$x:n,y:o,z:a$


工业上粗略估计：已知$d{T}_6$，$T_6^n=T_6+d{T}_6\Rightarrow T{}^T\Delta =\frac{\delta T\Delta }{\delta q}\delta q$

然后通过$T_6^n$计算新的关节方案$dq=q^n-q$

运动规划

考虑因素：易于指定，位置速度连续，奇点的处理，configuration统一，坐标系(关节/笛卡尔)

路径描述：起点、终点、经过点的位姿(position&orientation)

多项式路径规划

约束：起点、终点的位置、速度$\Rightarrow$三次及以上多项式

路径规划：最短路径/最短时间，相切得平滑曲线

系统复杂度提高：B-spline planning提高多项式次数，Bezier Cune增加路径点数

对称：减少一个约束

A、B位置连续、速度连续、加速度为0，共6个约束，对称减少一个，五次多项式解方程即可

坐标系

通常在笛卡尔坐标系中做路径规划，逆运动学计算复杂，需要考虑奇点($J$不可逆)。

关节坐标系虽然计算调用方便，没有奇点和configuration问题，但是转化为笛卡尔坐标位置时较复杂。将路径点映射到关节坐标系进行规划，但是受限于关节之间的牵连，速度取决于最慢的关节。

从POS1到POS2的规划：

$T_6^1=BAS{E}^{-1}\ast POS1\ast TOO{L}^{-1}$

$T_6^2=BAS{E}^{-1}\ast POS2\ast TOO{L}^{-1}$

$T_6=BAS{E}^{-1}\ast POS1\ast D(r)\ast TOO{L}^{-1}$

令自变量$r=\frac{t}{T}$，有$0⩽r⩽1$，$D(0)=1$，$D(1)=POS{1}^{-1}\ast POS2$

$D(r)=Tran{s}_{line}(r)\ast R{a}_z(r)\ast R{o}_{xy}(r)$

其中从POS1到POS2的平移$Tran{s}_{line}(r)=\left[\begin{array}{cccc}1& 0& 0& r_x\\ 0& 1& 0& r_y\\ 0& 0& 1& r_z\\ 0& 0& 0& 1\end{array}\right]$

使得两个$Z$轴重合的旋转$R{a}_z(r)=$

其中旋转角度为$\theta$，旋转轴为$K$轴(为$POS1$的$Y$轴绕$Z$轴旋转$\psi$角度)

使得两个$Y$轴重合的旋转$R{o}_z(r)=\left[\begin{array}{cccc}C(r\varphi )& -S(r\varphi )& 0& 0\\ S(r\varphi )& C(r\varphi )& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

变量：$x,y,z,\psi ,\theta ,\varphi$，表达式$D(r)=\left[\begin{array}{cccc}{n}_x& (...)& C\psi S(r\theta )& r_x\\ n_y& (...)& S\psi S(r\theta )& r_y\\ n_z& (...)& C(r\theta )& r_z\\ 0& 0& 0& 1\end{array}\right]$

$R{a}_z(1)$代入$r=1$求 $\psi ,\theta$；$R{o}_y(1)$代入$r=1$求 $\varphi$；

($x:n,y:o,z:a$)

例：

位姿空间方法

运动规划，路径规划，轨迹规划

躲避障碍方式：

①根据障碍形状抽象为一系列圆形

②人工势场法：建立势场模型，障碍物为高势能，目的地为低势能

缺点：容易被势阱或者壁垒困住$\Rightarrow$给一个“冲量”

③地毯式搜索：末端连杆开始向下，每个连杆左右来回试探，recursive递归$基础上优化

优化：只需要精确无障碍空间，遇到障碍物时增大试探间隔；放弃探索狭窄区域，选择宽敞路径；对不同类型道路加权，最后选择分数最高路径；456短轴球空间不考虑，只用123轴试探。

总结：先建地图，根据地图找路径比较 vs 物理模型