台大机器人学——林沛群

机械手臂轨迹规划实例

Manipulator Trajectory-Planning Example

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

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

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

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

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

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

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的方式去解

因为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}再相同姿态。

详细过程如下:

温故:

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一般具有无限多节,需导入最优化方法(可避障、时间最快、最节能...)