机器人运动学笔记3——林沛群

提示：本文上接 机器人运动学笔记2——林沛群

---

前言

本文将涉及到机器人学的轨迹规划相关内容。这些内容均来自学习过程中记录的笔记。笔记摘自台大机器人学之运动学——林沛群

---

一、机械手臂轨迹规划

1.1 机械手臂轨迹规划引言

• 轨迹：机械手臂的末端在一段时间内的位置速度加速度情况。
• 理想轨迹：光滑路径，速度加速度变化连续

1.2 Joint-Space下轨迹规划

步骤：

1. 定义{T}刀具坐标系相对于{G}物体坐标系的起始点、过程点和终点，齐次变换矩阵为${}_T^G{T}_i$(包含移动转动自由度)
2. 将${}_T^G{T}_i$以6个参数来表达${}_{}^G{X}_T=\left[\begin{array}{c}{}_{}^G{P}_{Torg}\\ ROT({}_{}^G{\hat{K}}_T,\theta )\end{array}\right]$(非rotation matrix的方式表达)
3. 逆运动学：将手臂末端点状态转换到joint的状态：${}_{}^G{X}_T\to {\Theta }_i$
4. 在joint空间下规划${\Theta }_i$光滑轨迹(smooth trajectories)
5. 正运动学：将joint状态转换到手臂末端状态，检查末端点在Cartesian-space下轨迹的可行性。
   
   

2.3 Cartesian-Space下轨迹规划

步骤：

1. 定义{T}刀具坐标系相对于{G}物体坐标系的起始点、过程点和终点，齐次变换矩阵为${}_T^G{T}_i$(包含移动转动自由度)
2. 将${}_T^G{T}_i$以6个参数来表达${}_{}^G{X}_T=\left[\begin{array}{c}{}_{}^G{P}_{Torg}\\ ROT({}_{}^G{\hat{K}}_T,\theta )\end{array}\right]$(非rotation matrix的方式表达)
3. 对所有手臂末端点状态规划光滑轨迹
4. 将规划好手臂末端点状态的轨迹点转换到joint状态：${}_{}^G{X}_T\to {\Theta }_i$
5. 检查joint状态在Joint-space下轨迹的可行性

注释(采用笛卡尔坐标系下的轨迹规划)：

1. 具物理直观的轨迹
2. 较高的运算负载

1.4 三次多项式(Cubic-Polynomials)

• 轨迹：不同的轨迹区段$\left[t_i,t_{i+1}\right]$以不同参数的函数来规划。
• 光滑轨迹：需定义各函数的边界条件(包含位置速度$\theta \left(t_i\right),\theta \left(t_{i+1}\right),\dot{\theta }\left(t_i\right),\dot{\theta }\left(t_{i+1}\right)$，4个条件，即$\left[t_i,t_{i+1}\right]$轨迹中$t_i$点所在位置和速度需要和$\left[t_{i-1},t_i\right]$轨迹规划的终点$t_i$相等，$t_{i+1}$点同理)

上图右边表示各阶次多项式的命名方式。

求解cubic-polynomial：
通式：$$\theta \left(\tilde{t}\right)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3$$
四个未知数：$a_0,a_1,a_2,a_3$
对每个区间：$t\in \left[t_i,t_{i+1}\right]$每一段的∆t可以不同，取决于过程点的设定
$\tilde{t}=t-t_i$ 所以 $\tilde{t}{|}_{t=t_i}=0$ ，$\tilde{t}{|}_{t=t_{i+1}}=∆t=ti+1-ti>0$
4个边界条件：
$$\{\begin{array}{c}\theta \left(\tilde{t}{|}_{t=t_i}\right)={\theta }_i=a_0\\ \theta \left(\tilde{t}{|}_{t=t_{i+1}}\right)={\theta }_{i+1}=a_0+a_1∆t+a_2∆t^2+a_3∆t^3\\ \dot{\theta }\left(\tilde{t}{|}_{t=t_i}\right)={\dot{\theta }}_i=a_1\\ \dot{\theta }\left(\tilde{t}{|}_{t=t_{i+1}}\right)={\dot{\theta }}_{i+1}=a_1+{2a}_2∆t+3a_3∆t^2\end{array}$$
最终求解：$$\{\begin{array}{c}{a}_0={\theta }_i\\ a_1={\dot{\theta }}_i\\ a_2=\frac{3}{∆t^2}({\theta }_{i+1}-{\theta }_i)-\frac{2}{∆t}{\dot{\theta }}_i-\frac{1}{∆t}{\dot{\theta }}_{i+1}\\ a_3=-\frac{2}{∆t^3}({\theta }_{i+1}-{\theta }_i)+\frac{1}{∆t^2}({\dot{\theta }}_{i+1}-{\dot{\theta }}_i)\end{array}$$
以矩阵的形式解： $$\left[\begin{array}{c}\begin{array}{c}{\theta }_i\\ {\theta }_{i+1}\end{array}\\ \begin{array}{c}{\dot{\theta }}_i\\ {\dot{\theta }}_{i+1}\end{array}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& ∆t& ∆t^2& ∆t^3\\ 0& 1& 0& 0\\ 0& 1& 2∆t& 3∆t^2\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{a}_0\\ a_1\end{array}\\ \begin{array}{c}{a}_2\\ a_3\end{array}\end{array}\right]$$
$$\Theta =T_{4\times 4}(∆t)\bullet A$$
$$\det \left(T_{4\times 4}\right)=-∆t^4\ne 0可逆$$
$$A=T_{4\times 4}^{-1}\bullet \Theta$$
$$\left[\begin{array}{c}\begin{array}{c}{a}_0\\ a_1\end{array}\\ \begin{array}{c}{a}_2\\ a_3\end{array}\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& 0& 1\\ -\frac{3}{∆t^2}& \frac{3}{∆t^2}& -\frac{2}{∆t}& -\frac{1}{∆t}\\ \frac{2}{∆t^3}& -\frac{2}{∆t^2}& \frac{1}{∆t^2}& \frac{1}{∆t^2}\end{array}\right]\left[\begin{array}{c}\begin{array}{c}{\theta }_i\\ {\theta }_{i+1}\end{array}\\ \begin{array}{c}{\dot{\theta }}_i\\ {\dot{\theta }}_{i+1}\end{array}\end{array}\right]$$

1.5 多段Cubic-Polynomials

如何选择速度条件${\dot{\theta }}_i$和${\dot{\theta }}_{i+1}$？

1. 直接定义(不建议)
2. 自动生成。如果${\dot{\theta }}_i$在$t_i$前后变号，选择${\dot{\theta }}_i=0$；如果${\dot{\theta }}_i$在$t_i$前后不变号，选择平均值

规划速度使过程中加速度也连续(有效使用此可控变量)：
 不同区段cubic-polynomial需要整合一起，一并求解。

加速度调控的例子：


上方示例的矩阵法求解：

1.6 N段路径的三次多项式(General Cubic Polynomials)

N段路径则有n个三次多项式，总共4N个未知参数。
4N个未知参数需要4N个方程求解。

还需要2个条件(方程)有3种选择：

示例1(平面RRR手臂Cartesian-space)：
求解步骤从左到右从上到下依次为，


示例2(平面RRR手臂joint-space)：
求解步骤从左到右从上到下依次为，


当路径规划过程中需要规划位置速度加速度时将使用五次多项式。

二、含抛物线的线性函数

2.1 Linear Function with Parabolic Blends

使用该混合抛物线的线性函数的原因：

1. 许多任务中需要使用直线轨迹。
2. 轨迹中包含多段直线轨迹，线段转折点速度不连续
3. 解决方案：将直线段两端修正为二次方程式，让速度轨迹光滑。

规划方式：

上图中式②=①可得，
$$\ddot{\theta }{t}_b={\dot{\theta }}_{t_b}=\frac{{\theta }_h-{\theta }_b}{t_h-t_b}=\frac{\frac{{\theta }_f-{\theta }_0}{2}-({\theta }_0+\frac{1}{2}\ddot{\theta }{t_b}^2)}{\frac{t_f}{2}-t_b}=\frac{{\theta }_f-{\theta }_0-\ddot{\theta }{t_b}^2}{t_f-{2t}_b}$$
$$\ddot{\theta }{t_b}^2-\ddot{\theta }{t}_f{t}_b+({\theta }_f-{\theta }_0)=0$$
$$t_b=\frac{\ddot{\theta }{t}_f-\sqrt{{\ddot{\theta }}^2{t_f}^2-4\ddot{\theta }({\theta }_f-{\theta }_0)}}{2\ddot{\theta }}$$
判别式内需为$\sqrt{{\ddot{\theta }}^2{t_f}^2-4\ddot{\theta }({\theta }_f-{\theta }_0)}\ge 0$，所得$t_b$才为实数：
$$\ddot{\theta }\ge \frac{4({\theta }_f-{\theta }_0)}{{t_f}^2}$$
$${\ddot{\theta }}_{min}=\frac{4({\theta }_f-{\theta }_0)}{{t_f}^2}$$
加速度状态讨论:
1.如果$\ddot{\theta }={\ddot{\theta }}_{min}$
$t_b=\frac{t_f}{2}=t_h\Rightarrow$无线性线段，两个抛物线曲线段相连，
在$t_b$处：$\dot{\theta }\left(t_b\right)=\ddot{\theta }{t}_b=\frac{4({\theta }_f-{\theta }_0)}{{t_f}^2}\frac{t_f}{2}=2\frac{{\theta }_f-{\theta }_0}{t_f}$，
速度为和原本无规划直接相连的速度：$\dot{\theta }=\frac{{\theta }_f-{\theta }_0}{t_f}$相比为2倍。
2.如果$\ddot{\theta }<{\ddot{\theta }}_{min}$，
加速度不足，在$t_b=t_h$，$\theta <{\theta }_h$，
导致$t_h$时抛物线和直线不连续。

2.2 多段Linear Function with Parabolic Blends

多个中间点的路径，此时每一段类比前一节的始末位置，只不过现在每一段的始末位置的速度不为0。
类似需要规划图中所示的路径，分为第一个线段部分，中间线段部分，最后一个线段部分。

对于中间线段部分：

上图中，
  $t_{{\theta }_j}$，$t_{{\theta }_k}$表示到达点j和点k所在位置的时间
  $t_j$，$t_k$表示点j和点k所在位置部分二次曲线段经历的时间，
  $t_{jk}$表示点j到点k路径中直线段经历的时间，$t_{kl}$同理，
  $t_{djk}$表示从点j到点k路径经历的时间。
对上图中j到k段和k到l段(或者说对于任意一段${\theta }_i$到${\theta }_{i+1}$区间)：
Linea部分：
$${\dot{\theta }}_{jk}=\frac{{\theta }_k-{\theta }_j}{t_{djk}}$$$${\dot{\theta }}_{kl}=\frac{{\theta }_l-{\theta }_k}{t_{dkl}}$$
Parabolic部分：
  方法一：设定加速度解时间
$${\ddot{\theta }}_k=sign({\dot{\theta }}_{kl}-{\dot{\theta }}_{jk})\left|{\ddot{\theta }}_k\right|$$$$t_k=\frac{{\dot{\theta }}_{kl}-{\dot{\theta }}_{jk}}{{\ddot{\theta }}_k}$$
  上式中，
    $|{\ddot{\theta }}_k|$的大小时需要我们设定的参数，
    $sign()$表示取符号操作，仅获取${\dot{\theta }}_{kl}-{\dot{\theta }}_{jk}$结果的符号。
  方法二：设定时间解加速度$${\ddot{\theta }}_k=\frac{{\dot{\theta }}_{kl}-{\dot{\theta }}_{jk}}{t_k}$$
  上式中，
    $t_k$的大小时需要我们设定的参数。
  最后计算出直线部分经历的$t_{jk}$时间，
$$t_{jk}=t_{djk}-\frac{1}{2}{t}_j-\frac{1}{2}{t}_k$$
对于第一个线段部分：

上图中，
  $t_{{\theta }_1}$，$t_{{\theta }_2}$表示到达${\theta }_1$和到达${\theta }_2$时的时间，
  $t_1$表示初始第一个线段二次曲线段经历的时间，
  ${\theta }_1$的位置可以视为将整个起始点为${\theta }_0$的轨迹在时间上向后平移了$\frac{t_1}{2}$时${\theta }_0$所在的位置，目的时为了便于增加一段二次曲线缓加速部分。否则，${\theta }_0$在$\theta$轴上，则没有初始的二次曲线部分。
  $t_{12}$表示点1到点2路径中直线段经历的时间，
  $t_{d12}$表示从起始位置到点2路径(二次曲线段+直线段)经历的时间。
方法一：设定加速度解时间
$${\ddot{\theta }}_1=sign({\theta }_2-{\theta }_1)\left|{\ddot{\theta }}_1\right|$$
$${\dot{\theta }}_{12}=\frac{{\theta }_2-{\theta }_1}{t_{d12}-\frac{1}{2}{t}_1}={\ddot{\theta }}_1{t}_1$$
$$\frac{1}{2}{\ddot{\theta }}_1{t}_1^2-t_{d12}{\ddot{\theta }}_1{t}_1+{(\theta }_2-{\theta }_1)=0$$
$$t_1=t_{d12}-\sqrt{{t_{d12}}^2-\frac{{2(\theta }_2-{\theta }_1)}{{\ddot{\theta }}_1}}$$
上式中，
  $sign()$表示取符号操作，仅获取${\theta }_2-{\theta }_1$结果的符号，
  $|{\ddot{\theta }}_1|$的大小时需要我们设定的参数。
方法二：设定时间解加速度
$${\ddot{\theta }}_1=\frac{{\theta }_2-{\theta }_1}{(t_{d12}-\frac{1}{2}{t}_1)}\frac{1}{t_1}$$
$$t_{12}=t_{d12}-t_1-\frac{1}{2}{t}_2$$
上式中，
  $t_1$的大小时需要我们设定的参数，
  $t_2$是点2所在部分的二次曲线段经历的时间。
对于最后一个线段部分：

上图中：
  $t_{{\theta }_n}$，$t_{{\theta }_{n-1}}$表示到达${\theta }_n$和到达${\theta }_{n-1}$时的时间，
  $t_n$表示最后一个线段二次曲线段经历的时间，
  ${\theta }_n$的位置可以视为将整个终点为${\theta }_f$的轨迹在时间上向前平移了$\frac{t_n}{2}$时${\theta }_f$所在的位置，目的时为了便于增加一段二次曲线缓减速部分。否则，${\theta }_f$在整个轨迹的终点上，则没有最终的二次曲线部分。
  $t_{(n-1)n}$表示点n-1到点n路径中直线段经历的时间，
  $t_{d(n-1)n}$表示从n-1到最终位置路径(二次曲线段+直线段)经历的时间。
方法一：设定加速度解时间
$${\ddot{\theta }}_n=sign({\theta }_n-{\theta }_{n-1})\left|{\ddot{\theta }}_n\right|$$
$${\dot{\theta }}_{(n-1)n}=\frac{{\theta }_n-{\theta }_{n-1}}{t_{d(n-1)n}-\frac{1}{2}{t}_n}={\ddot{\theta }}_n{(-t}_n)$$
$$\frac{1}{2}{\ddot{\theta }}_n{t}_n^2-t_{d\left(n-1\right)n}{\ddot{\theta }}_n{t}_n-{(\theta }_n-{\theta }_{n-1})=0$$
$$t_n=t_{d(n-1)n}-\sqrt{{t_{d(n-1)n}}^2-\frac{{2(\theta }_n-{\theta }_{n-1})}{{\ddot{\theta }}_n}}$$
上式中，
  $sign()$表示取符号操作，仅获取${\theta }_n-{\theta }_{n-1}$结果的符号，
  $|{\ddot{\theta }}_n|$的大小时需要我们设定的参数。
方法二：设定时间解加速度
$${\ddot{\theta }}_n=\frac{{\theta }_n-{\theta }_{n-1}}{(t_{d12}-\frac{1}{2}{t}_n)}\frac{1}{-t_n}$$$$t_{(n-1)n}=t_{d(n-1)n}-t_n-\frac{1}{2}{t}_{n-1}$$
上式中，
  $t_n$的大小时需要我们设定的参数，
  $t_{n-1}$是点n-1所在部分的二次曲线段经历的时间。

2.3 Linear Function with Parabolic Blends注解

一、真实系统中可达到的加速度$\ddot{\theta }$取决于许多因素
  1.马达规格
  2.手臂姿态：手臂在不同姿态下，各轴所需承载(如重力)的扭力不同
  3.手臂动态状态：手臂在不同动态下，各轴需承载惯性力不同

二、规划后轨迹未通过中间过程点
  仅加速度无限大时轨迹有通过中间过程点(via points)
  如果通过中间过程点为必须，需要建立伪中间过程点(pseudo via points) ，让原本的中间过程点落在直线段上。

三、若有Cartesian space 下有某些部分有直线轨迹的需求，轨迹规划需要在Cartesian space下进行。
四、编程里，仔细定义好某时间t所属的线段或曲线段。

2.4 Cartesian Space 下轨迹几何极限

一、中间某些区域无法到达。(在工作空间(work space)之外)
二、轨迹需要高加减速。(接近singular configuration)
三、针对特定起始和终点姿态，无法产生连续轨迹(几何上不可能)
如下图，除非$l_1-l_2=d$

2.5 回顾R-R-R机械臂(Revisit the RRR Manipulator)

总结

以上就是今天要讲的内容，本文是本人观看机器人运动学所作的笔记，本文主要包含轨迹规划部分。整个笔记到此都已经完结。