常用的轨迹规划方法有两种:一种是Cubic Polynomials即三次多项式插值;另一种是Linear Function with Parabolic Blends带抛物线混合的线性函数。两个方法各有千秋,本文先介绍三次多项式插值方法。
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要存在,要满足:
- 所以,可以求A:
2.多段Cubic Polynomials
1中,我们假设两侧位置和速度条件均是已知的,有四个已知条件解四个未知数,真实情况下,如何定义via points的速度条件 ?
- 直接定义,无论在Cartesian Space或Joint Space;(不建议,过于复杂,轨迹落在起一点附近时)
- 自动生成:
- 若 在 前后变号,选择 ;
- 若 在 前后同号,选择平均;
- 这种方式,方便我们知道每一段的边界条件;而且以此方式,不同区段的Cubic Polynomials可以分开求解独立处理。
- 最重要的是:规划速度使过程中的加速度也是CONTINUOUS!
- CONS: 速度和前一段有关,不同区段的Cubic Polynomials需要整合子一起,一并求解(多个Polynomial做联立,不能像之前那样单独求解)。
EXAMPLE: a trajectory with one via point
- 先看第一段:
- 再看第二段:
- 一共有8个未知数,代入已知条件,列出方程式:
- 4个position的边界条件(B.C.s):
- 开始和最后的2个速度边界条件:
- 在via point上,速度和加速度都是连续的!(左侧与右侧多项式来算应该相等才能保证连续):
- 一共有8个方程式,8个未知数,可解。
- 1. 代数解法(△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的时侯,8个未知数,6个条件,但我们偷偷预设了initial和final的速度作为已知,所以方便解出。
最后2个conditions的选择方法:
(1) Nature Cubic Spline ---
(2) Clamped Cubic Spline ---
(3) Periodic Cubic Spline --- (周期性的,手臂做圆圈运动)
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个未知数:
3. 对所有DOF规划smooth trajectories
4. 将Joints代入手臂模拟动作,确认手臂末端点轨迹如规则操作:(在Joint Space下与Cartesian Space下规划的轨迹形状有较大的差别)
Final Note: High-order Polynomials
如果位置、速度、加速度都必须要规划:则用Quintic Polynomial (五次多项式规划)
评论(0)
您还未登录,请登录后发表或查看评论