常用的轨迹规划方法有两种:一种是Cubic Polynomials即三次多项式插值;另一种是Linear Function with Parabolic Blends带抛物线混合的线性函数。两个方法各有千秋,本文先介绍三次多项式插值方法。

Joint Angles/Cartesian下的路径规划

1.原则

  • 轨迹:不同轨迹区段 [公式] 不同参数的函数来规划。
针对t_i和t_i+1时间段内的函数
  • Smooth:需要定义个函数的边界条件(包含未知和速度, [公式][公式] ,一共有4个条件。保证边界处速度与上一速度保持连续)
一次:Linear;二次:Quadratic;三次:Cubic;四次:Quartic;五次:Quintic;六次:Hexic(sextic) ;七次:Heptic(septic);八次:Octic;九次:Nonic;十次:Decic。
  • 以cubic polynomial三次多项式来规划轨迹(四个条件)
  • 解:
    • 通式: [公式] ;4 unknowns; [公式]
    • 变换:对每一个区段: [公式] : 有: [公式]
    • 这样做的好处是: [公式] and [公式] 恒正
    • 所以转换后: [公式] 注意每一个区段[公式]的△t可能不同,这取决于via points 的设定。
    • 边界条件:
      • [公式] # 通过变换范围,可以直接解出 [公式] ;---------①
      • [公式]------②
      • 速度条件 [公式] # 可以直接求出 [公式] ; ----------------③
      • 速度条件 [公式] ------④
    • 联立方程式①②③④解得:
      • [公式]
      • [公式]
      • [公式]
      • [公式]
  • 以Matrix Operation方式运算:
  • 注意,Inverse要存在,要满足:
△t≠0,有逆矩阵存在!
  • 所以,可以求A:

2.多段Cubic Polynomials

1中,我们假设两侧位置和速度条件均是已知的,有四个已知条件解四个未知数,真实情况下,如何定义via points的速度条件 [公式] 

  1. 直接定义,无论在Cartesian Space或Joint Space;(不建议,过于复杂,轨迹落在起一点附近时)
  2. 自动生成:
  •  [公式]  [公式] 前后变号,选择 [公式] 
  •  [公式]  [公式] 前后同号,选择平均;
  • 这种方式,方便我们知道每一段的边界条件;而且以此方式,不同区段的Cubic Polynomials可以分开求解独立处理。
  • 最重要的是:规划速度使过程中的加速度也是CONTINUOUS!
  • CONS: 速度和前一段有关,不同区段的Cubic Polynomials需要整合子一起,一并求解(多个Polynomial做联立,不能像之前那样单独求解)

EXAMPLE: a trajectory with one via point

三个点需要生成两个三次式即8个未知数
  • 先看第一段: [公式]
这样保证在运算时将时间平移到0开始
  • 再看第二段: [公式]
  • 一共有8个未知数,代入已知条件,列出方程式:
  • 4个position的边界条件(B.C.s):
θ1对左侧来说是终点,对右侧来说是起点
  • 开始和最后的2个速度边界条件:
在本例中是0,但不一定是0
  • 在via point上,速度和加速度都是连续的!(左侧与右侧多项式来算应该相等才能保证连续):
  • 一共有8个方程式,8个未知数,可解。
  • 1. 代数解法(△t相等)
此时△t相等
  • 2. 矩阵解法

先确认 [公式] 行列式不为0,才能去求解逆矩阵。


General Cubic Polynomials

我们希望在所有的via points上速度加速度都是连续的广义情况。

中间有N个线段,有N个cubic functions;

可以发现,上式总共有4N个unknown coefficients需要去求解。(线性联立方程,需要4N个方程式才可以。)
→ Position conditions at both ends of each [公式]
==> 2N conditions;
→ Velocity & acceleration continuity conditions at via points
==> 2(N-1) conditions;
还需要2个CONDITIONS以求解

只有一个 via point的情景

在做上面这一题一个via point的时侯,8个未知数,6个条件,但我们偷偷预设了initial和final的速度作为已知,所以方便解出。

最后2个conditions的选择方法:

(1) Nature Cubic Spline ---

(2) Clamped Cubic Spline ---

(3) Periodic Cubic Spline --- (周期性的,手臂做圆圈运动)

首尾 initial 状态与 final 状态(位置、速度、加速度)相同

Example: A RRR Manipulator 利用上面知识进行求解

平面RRR手臂长度: [公式]  [公式] ,而且 [公式] 。下表定义initial,via,via和final points的位置:

三个自由度,可以到达平面上紫色圆环内的任何位置

前两个手臂产生移动,最后一个产生转动;希望手臂到达空间中的4个红点; X,Y的点为第二杆件手腕的末端,所以不需要考虑 [公式] ,并非 [公式] 的末端点。范围是1的圆环~7的圆环之间;所以x,y必须在这两个紫色圆圈之间,才有规划意义,否则没有解

方法一:以cubic polynomials在Cartesian-space下规划轨迹

1.求出3个DOF(X,Y, [公式] )各自cubic polynomials的coefficients需要通过4个点;每个DOF有3个cubic polynomials,共12个未知数。

前六个条件:位置条件; 第七第八是initial v和final v的条件(假设为0),最后这4个row,是4个速度和加速度连续的条件。针对X,Y,θ,中间的T基本都相同,中间T只需要求解一次即可。

2. 对所有DOF规划smooth trajectories

3. 做Inverse Kinematics,找出各Joints的对应轨迹。
4. 将Joints代入手臂模拟动作,确认手臂末端点轨迹如规则运作。

方法二:以Cubic Polynomials在Joint-space下规划轨迹

1.Inverse Kinematics, 求出initial,via,final points的Joint Angles [公式] 
2. 求出各个 [公式] cubic polynomials的coefficients,需要通过4个点,每个Joint Angle有3个Cubic Polynomials,共12个未知数:

不管在哪个space做,△t的矩阵是一样的

3. 对所有DOF规划smooth trajectories

4. 将Joints代入手臂模拟动作,确认手臂末端点轨迹如规则操作:(在Joint Space下与Cartesian Space下规划的轨迹形状有较大的差别)


Final Note: High-order Polynomials

如果位置、速度、加速度都必须要规划:则用Quintic Polynomial (五次多项式规划)

已知开始位置、速度、加速度的解法