机器人轨迹规划

1、轨迹规划的目的：生成运动控制系统的参考输入，以确保机械手完成规划的轨迹。

路径和轨迹
运动率：执行器施加到关节的广义力，不违反饱和度限制且不激发结构的典型谐振模式。
路径：在关节空间和操作空间中，机械手在执行指定运动时必须跟随的点的轨迹。
轨迹：一条指定了时间率的路径。

轨迹规划算法的输入：路径描述、路径约束、机械手动力学约束
输出：按时间顺序给出的位置、速度、加速度序列

2、路径规划的要求：

(1)、在环境地图中寻找一条路径，机器人沿该路径移动时不与外界发生碰撞；
(2)、 能够处理用传感器感知的环境模型中的不确定因素和路径执行中出现的误差；
(3)、 通过使机器人避开外界物体而使其对机器人传感器感知范围的影响降到最小；
(4)、 能够按照目标点的需要寻找最优路径。

3、关节空间轨迹

特性：
（1）、计算简单
（2）、位置、速度是时间的连续函数

4、点对点运动

（1）、直线插补
设$v$ 为沿直线运动的速度， $t_s$ 为时间间隔
直线长度：

$$L=\sqrt{ (X_e-X_0)^2+(Y_e-Y_0)^2+(Z_e-Z_0)^2 }$$
行程：
$$d=vt_s$$

步数：

$$N=L/d+1$$

各轴增量：

$$\Delta X=(X_e-X_0)/N$$
$$\Delta Y=(Y_e-Y_0)/N$$
$$\Delta Z=(Z_e-Z_0)/N$$

各插补点坐标值：

$$X_{i+1}=X_i + \Delta X$$
$$Y_{i+1}=Y_i + \Delta Y$$
$$Z_{i+1}=Z_i + \Delta Z$$

（2）、圆弧插补

行程：

$$\Delta \theta=vt_s/R$$

步数：

$$N=L/\Delta \theta+1$$

各插补点坐标值：

$$X_{i+1}=X_i cos(\Delta \theta) - Y_i sin(\Delta \theta)$$

$$Y_{i+1}=Y_i cos(\Delta \theta) - X_i sin(\Delta \theta)$$

$$\theta _{i+1} = \theta _i + \Delta \theta$$

三维插补转化为二维再求解。

5、插补方法：

（1）、三次多项式插值

起始点和终止点的速度为零，角度由任务决定。可唯一确定一个三次多项式：

$$\theta (t) = a_0 +a_1 t+ a_2 t^2 + a_3 t^3$$

求解可得系数矩阵：
$a_0 = \theta _0$
$a_1 = 0$
$a_2 = \frac{3}{t_f^2} (\theta _f - \theta _0)$
$a_3 = -\frac{2}{t_f^2} (\theta _f - \theta _0)$

（2）、过路径点的三次多项式插值
速度的约束条件变为了：

$\dot \theta (0) =\dot \theta _0$
$\dot \theta (t_f) =\dot \theta _f$
即：中间段的初始速度是前一段的终止速度。
带入三项式方程中，可求出个系数的值。

另外还有高次多项式插补等。

关节空间的规划还有抛物线等。