机器人学中的状态估计中文版_机器人学——学习笔记18(Minpulator Traj Planning Example）...

台大机器人学——林沛群

机械手臂轨迹规划实例

Manipulator Trajectory-Planning Example

通过一个example，是对以上所有知识的应用；在真实使用的场景具有实用意义。
顺向+逆向+轨迹规划完成放杯子任务：

Revisit物件取放任务：机械手臂夹住放在桌子上的杯子，移动手臂将杯子挂到墙上的杯架中。

整个过程包含了移动与转动

1. 六轴RRRRRR手臂之DH Table(Review):

前三个自由度负责产生移动，后三个自由度负责产生姿态

2. 在Inverse Kinematics的学习中，进行了夹取任务的计算，以IK计算RRRRRR手臂在任务起始点C的6个转角(Joint Angles)，让手臂能顺利夹住杯子。

何小白：机器人学——学习笔记12(夹取任务)zhuanlan.zhihu.com

3. 这篇blog中的任务：规划手臂【将杯子从桌面上拿起放到杯子架上】件的整段轨迹；

为了防止机器人会不自觉的将杯子下压损坏了杯子，加上一些辅助条件：① 垂直拿起杯子一小段距离；② 到达杯架前，调整到通常姿态，让杯子能顺利放上杯架； （真实在轨迹规划时，需要思考一下是否有必要加上一些via points来保证运动的平滑性与安全性）

最终规划效果

4. 设定-1 清楚杯子在各个点

的

时间、坐标位置、姿态；

初始与末位置；

是中间的via points；

整理成总表的形式，方便后面进行轨迹规划（对world frame；角度以XYZ fixed angle计算）这里是

5. 设定-2：求出各点的Transformation Matrix

(

{cup}对地）

6. 设定-3：求出各点的Transformation Matrix

{CUP}对frame{6}如图中鼠标所指位置所示

7. 设定-4:从

得知

在各点的位置和姿态

这样就明确了Cartesian Space下，机械臂末端点在轨迹中的各点的坐标与姿态；

1-7 这些，我们只是明确了需求，后面需要开始进行规划，需要引入各个手臂的参数。

8. 以linear function with parabolic blends在Cartesian space下规划轨迹

8.1 求出

在各DOF

每段的速度与加速度

速度根据之前位置与姿态的条件可以算出来，得出速度，两两速度相减再除以时间，就可以得到加速度（只是parabolic段的加速段可以方便计算出）。

8.2 建立并绘出各个DOF在每个时间区段轨迹，Linear/Parabolic共7段（每段Parabolic curve时间设定为0.5s）

6个自由度的轨迹规划

8.3 以IK解出轨迹上所有设定点&轨迹内插点的6轴转角

在各设定点的位置和姿态

用Inverse Kinematics，结合手臂参数，求解Joint angles；以第二个via point

为例，进行说明计算；

① 先算出1.2.3轴的角度；详细方程式推导参见逆向运动学的课程内容; 4,5,6轴的角度可以通过Euler Angle的方式去解。

何小白：机器人学——学习笔记9(Inverse Kinematics)zhuanlan.zhihu.com

因为1、2、3轴也是couple的，所以解的过程中，也是一步一步解耦，将

分开来；

找到

相对于

该点的距离是多少；结合DH的参数，所以不同的手臂作IK的过程不一样（DH不一样）。

分离

有：

非负解在下方，有可能撞到基座或者桌面

最后：

到目前为止，解出了前三轴的joint angles(以第二个via point为例）

② 通过Euler Angles ZYZ的方式求出

；

----P.S---- 4、5、6轴交于一点，其不影响轴末端的X,Y,Z位置坐标。所以1、2、3轴负责X,Y,Z的移动，4、5、6轴负责姿态 ----------------

先求出

;

后面方便求出：
- 为让手臂姿态和ZYZ重合，需要先做
  
  之中对X轴之旋转
  
  ：

从下可以发现，要做ZYZ，先得把

转到

的方向，这之后的R才可以与ZYZ对应（做顺时针90°的转动）,接着才方便去解ZYZ

以

求出当中的Euler ZYZ Angles

；

如何把这个

与

做对应？

参照之前的讲义，ZYZ的

与DH的

，在

有+180°的差异，需要补回来

REVIEW: 为啥有180°的差异？

以ZYZ来看，

需要多转180°，下一次的旋转才可以对Y轴

；(对齐

与

)

定义位置不同，但是量值没变

到了第六轴是向前转，为了使第六轴坐标系（green & red）重合，

需要多转180°，ZYZ的{6}才会和DH的{6}再相同姿态。

详细过程如下：

温故：

何小白：机器人学——学习笔记2(Fixed Angles&Euler Angles)zhuanlan.zhihu.com

8.4 针对轨迹归化中的每一个点，解出对应的

将接触设定点&轨迹内插点的6个转角各自对时间表示成轨迹图（在joint space下轨迹不在是直线，但是依然是smooth的...）

8.5 将转角代入Forward Kinematics，绘制出杯子

的坐标及姿态，以确认轨迹规划的正确性（同样以第二个via point

为例，进行说明）

根据解出θ所得到的Trans Matrix与

是一样的否？

将θ代入左右方程，看左右是否相同；

8.6 轨迹模拟

9. 以LFPB方式在Joint Space下规划轨迹

9.1 先以IK计算出各点的6轴转角

直接将4个点，以IK的方式，倒推

，参照上面的方法：

9.2 求出各轴

在每个时间段的速度及加速度

9.3 建立各轴在每个时段的equation(Linear/Parabolic共7段），绘出平滑的轨迹（每段parabolic curve时间为0.5s）

荧光绿部分都是parabolic部分

9.4 以FK绘制出

对时间轨迹，确认轨迹有通过设定点

转回来之后，我们可以发现因为FK过来会乘上cos、sin，会发现轨迹变成了曲线，joint space下的直线，在cartesian space下式曲线轨迹，其中Y的变化尤其剧烈，变动比较大，如果画出手臂轨迹末端点的规划可以发现是明显曲线。

整个空间轨迹中，第二段有比较大的形变。
Cartesian Space下和Joint Space下轨迹比较：

说明：
① 如果真的很在意，比如说，希望真实运动轨迹是直线，那最好在Cartesian Space（CS）下进行LFPB的路径规划；
② 如果说，直线要求没那么强，可以我们希望在Joint上面对电机的扭力、速度有要求，则可以把LFPB的路径规划移到Joint Space（JS）下进行。因为，在Cartesian下，你需要带上内插点，一起做IK，而你如果在Joint Space下，则你需要计算4个点的IK即可。
③ 如果，real-time的需求较高，则在CS下由于要进行更多次IK的计算，这不利于保证实时性。

总结Conclusion ...

1。这个Capstone Example(总体例题）的多个知识点：

1.刚体移动状态的描述；（空间中位置的描述、姿态的描述（转动），基于Trans Matrics进行表达，借由TM使移动转动连续）;
2. 顺向运动学(FK)，知道了
推出手臂末端点的状态；从 Frame 6在通过平移到cup frame；
3. 逆向运动学(IK)，知道杯子位置、姿态，反算关节的角度，使其可以到达；
4. 轨迹规划（CP法，LFPB方法，这里主要用第二个方法）;

2。轨迹规划一般为任务导向，设定手臂末端点和物件（工件）之间的关系，独立于手臂的类型（或自由度配置）

Cubic Polynominals;
Linear Function with parabolic blends；
任务本身和手臂是无关的（杯子的位置时任务需求，与手臂参数无关）；
若有更高的要求，可以位置、速度、加速度..都连续进行规划（高阶）；

3。平面RRR与空间RRRRRR 都属于Open-Chain（开链式）手臂的IK

平面RRR自由度手臂的IK
- 加上移动自由度——SCARA与圆晶机器人可视为RRR的扩展

- PRRR&RRRP
- 这两个机械手臂RRR都可以看成垂直于Z的一个平面内的RRR;
空间RRRRRR自由度手臂IK
- 多关节机器人（Articulated Manipulator）

4。【坐标型】机器人不需要进行FK和IK

第一种：Joint Space== Cartesian Space，末端点任何位置，移动X,Y,Z都可以达到（3D打印机）；第二种、第三种：球坐标机器人与圆柱坐标机器人，FK和IK follow相应的转换规则就可以。

转动方面可以用Euler Angles方式求解。

5。Closed-Chain（闭链）手臂的FK和IK计算方式不同

DELTA机器人，Stewart Platform等...

DELTA机器人

Stewart Platform

6。若手臂有冗余自由度（7个自由度较多）（redudant DOF），轨迹规划和IK一般具有无限多节，需导入最优化方法（可避障、时间最快、最节能...）