图片来源：here

1 概述

机器人的连续路径规划主要涉及到基座姿态、机械臂末端位置或者姿态的规划，在此过程中，位置可以通过三维矢量唯一表示，因此对于机械臂末端位置的规划主要是针对三维向量坐标的规划，而对于姿态的规划，由于姿态表示的方法不唯一，因此会衍生出多种姿态规划方式。但是不管是针对位置以及姿态的规划或者插值，其相应的规划算法具有通用性。

由于多刚体的旋转可以有多种方式表示，一般采用欧拉角、轴角以及四元数等表示，欧拉角都由于涉及到被除数是正余弦，因此在实际使用中会出现姿态奇异，而采用基于四元数的方法则可以有效避免姿态奇异问题。

结合各种表示方式的优缺点，本文对于姿态规划的处理一般采用轴角即根据期望姿态计算出一次旋转轴以及相应的转角，在参与机械臂实际姿态运算的时候选择四元数作为姿态的表示方式，且由于欧拉角相对来说较为直观，实际姿态的显示以欧拉角来表示。

机器人的连续路径规划可以分为点到点的连续路径以及多点之间的连续路径。对于空间机器人笛卡尔连续路径，需要合理的规划其位置或者姿态轨迹，笛卡尔的轨迹一般是和时间相关的参数化的函数。一般笛卡尔连续路径中会采用诸如多项式、三角函数以及指数函数作为基函数，以此表示一段位置连续，速度和加速度满足要求的轨迹。从数学的角度来说，在时间$t_0-t_1$内一维的轨迹可以表示如下：

$$q=q(t),t\in[t_0,t_1]$$

式中$q$:某方向的位置或者姿态标量值;$t_0,t_1$ 起始和终止时刻;

2 点到点的连续路径规划

若上述轨迹以多项式为基函数的，且在初始和终止时刻的速度和加速度均有初始值，则轨迹描述如下

$$\begin{cases} q(t_0)=q_0,\dot q(t_0)=v_0,\ddot q(t_1)=a_0 \\[5ex] q(t_f)=q_f,\dot q(t_f)=v_f,\ddot q(t_f)=a_f \end{cases}$$

式中$q_0,q_f$经过规划的某方向的位置或者姿态标量,$v_0,v_f$起始和终止时刻的速度值；$a_0,a_f$起始和终止时刻的加速度值.

在上述条件下，定义$T=t_f-t_0,h=q_f-q_0$,则基于五次多项式的轨迹可任意表示如下

$$q(t)=q_0+c_1(t-t_0)+c_2(t-t_0)^2++c_3(t-t_0)^3++c_4(t-t_0)^4++c_5(t-t_0)^5$$

其中

式中$c_i$多项式的常数,$T$总时间，$h$终止值与初始值的差值.

3 多点之间的连续路径规划

实际机械臂在运动中，可能会经过多个中间节点，这样的路径可以规避障碍物，且可以更平稳地完成设定的跟踪任务。理论上，对于多点之间的轨迹可以采用一个多项式去连接各个中间点，但求解过程中存在数值稳定性问题，因此，对于多点的轨迹规划方案，一般采取插值的方式实现。

本文采用样条曲线规划多点之间的轨迹，为了保证轨迹的连续性，一般会规定轨迹的初始与终止点速度。因此，给定n+1个点，如$(t_k,q_k)$.边界速度是标量$v_0$和$v_n$,。期望轨迹根据下述方程所描述的样条曲线进行规划

其中：

式中：

$q_k,\dot q_k，\ddot q_k$，经过第$k$个点对应的标量值、速度值、加速度值；

$s(t)$样条曲线函数

$c_{ki}$样条曲线系数

$v_k$经过第$k$个点速度值

$t_k$经过第$k$个点的时间值

在此过程中，如果各个中间的速度是已知的，那么可以得到

式中 $u_k$ 每两个点之间的时间间隔$u_k=t_{k+1}-t{k}$

此外，如果中间点的速度是未知的，则可以通过插值点的加速度连续等条件来计算多点之间的运动轨迹。连续加速度条件可以表示如下

$$\ddot q_k(k+1)=2c_{k,2}+6c_{k,3}u_k=2c_{k+1,2}=\ddot q_{k=1}(t_{k+1})$$

对于上式，通过代入$c_{k,2},c_{k,3},c_{k+1,2}$的表达式，进一步推导可以得到

上述表达式关系可以写成矩阵表达式具体如下

$v^{'}=[v_0,v_1,......v_n]^T$ ,$d^{'}=[d_0,d_1,......d_{n-2}]^T$ ,$d^{'}$为与时间间隔以及位置相关的矩阵。

由于在上式中，轨迹的初始和终止时刻的速度式已知的，因此，有必要消去和初始速度以及终止速度相关项，从而可以得到

$$A(u)v=d(u，q,v_0,v_n)$$

其中