5-1 机械手臂轨迹规划引言
机械手臂轨迹规划在机械臂任务操作过程当中不可或缺，抓取物体拿到指定位置，这个过程除了IK，就还包括机械臂的轨迹规划。这里给出轨迹的定义，轨迹：机械手臂（的末端点或操作点）的位置、速度、加速度对时间的历程，可以进一步地定义成{T}相对{G}的状态历程，这个状态历程和手臂类型无关，{G}也可随时间变动（物体随着传送带动）。
下面这张PPT更加形象的描述了轨迹的定义，当机械臂夹爪夹住杯子挂到墙上的这一运动历程就是一条轨迹，物件取放任务中手臂末端点（或物件）的运动历程，即是轨迹规划所进行的内容。
理想轨迹：是一条平滑的路径（即轨迹是连续的，其轨迹的一阶导数也是连续的），同时，在一条轨迹当中，会有起始点和终点，起始点和终点之间存在一个或多个途径点。
5-2 Joint Space下的轨迹规划
这里我们重申一下Joint Space(关节空间)和Cartesian Space(笛卡尔空间)，Joint Space描述的是整个机械臂每一个关节分别处于什么角度，所有关节角度变量组成的空间就是Joint Space；Cartesian Space又叫工作空间，它描述的是机械臂末端执行器在三维空间中的位置和姿态。在Joint Space下的轨迹规划整体流程描述如下：第一，在Cartesian Space定义机械臂末端需要经过的关键点（起始点、途径点和终止点），用数学描述就是末端点坐标系{T}相对于目标点坐标系{G}起始点，途径点和终止点，并且不再是用旋转矩阵的方式进行描述，而是通过六个参数的方式来表达：
GXT=[GPTorgROT(GKT,θ)]=[xyzϕθψ]; {}^{G}X_{T}= \begin{bmatrix} {}^{G}P_{Torg}\\ ROT({}^{G}K_T,\theta) \end{bmatrix}= \begin{bmatrix} x\\ y\\ z\\ \phi\\ \theta\\ \psi \end{bmatrix};GXT=[GPTorgROT(GKT,θ)]=xyzϕθψ;
第二，将每个末端点状态(起始点、途径点和终止点)转换到机械臂的关节状态(角度)，这一过程就是IK，举个例子，假设有一个六关节的机械臂系统，其中一个末端点状态为：
GXT=[0.50.20.490∘0] {}^{G}X_{T}= \begin{bmatrix} 0.5\\ 0.2\\ 0.4\\ 90^\circ\\ 0 \end{bmatrix}GXT=0.50.20.490∘0这需要用IK算法解算机械臂系统的每一个关节应该是多少角度，假设计算为：q=[20∘35∘−10∘50∘30∘20∘]; q= \begin{bmatrix} 20^\circ\\ 35^\circ\\ -10^\circ\\ 50^\circ\\ 30^\circ\\ 20^\circ \end{bmatrix};q=20∘35∘−10∘50∘30∘20∘;
第三，由步骤二，一个途径点就会对应一个关节状态向量，此步骤需要将所有途径点对应的关节状态向量中的每一个关节做平滑轨迹处理。假设，所有途径点对应的关节状态向量为q1,q2,q3,⋯ ,qN+1q_1,q_2,q_3,\cdots,q_{N+1}q1,q2,q3,⋯,qN+1，对于第一个关节，假设其途径点的角度为0—>20—>40—>…—>10(终止点)，需要根据这些角度规划一条平滑轨迹，对所有关节都是如此；
第四，将关节状态转换到手臂末端点状态，这一步正是FK，步骤三中知道了所有所有途径点对应的关节状态向量，通过关节状态向量转换到末端点的状态，检查末端点在笛卡尔空间下轨迹的可行性（可能会有障碍物）。
下面，有一个六自由度机械臂（六个关节），只有三个途径点的例子进行说明上述步骤：首先，将每个末端点状态(起始点、途径点和终止点)转换到机械臂的关节状态(角度)，这在下方PPT中体现为IK变换；其次，将途径点当中的所有关节状态规划出一条平滑轨迹，这体现为Trajectory planning；然后，将关节状态转换到手臂末端点状态，下方PPT中体现为FK变换；最后，检查末端点形成的轨迹是否会有障碍物。
