五次多项式对2R平面机器人关节的轨迹控制

1、2R平面机器人及控制条件

  2R平面机器人的两个连杆长度分别为 l₁=l₂=1m，两个关节的角度，即状态变量为θ=[θ₁ θ₂]^T。
  控制条件如下：
（1）初始位置、速度和加速度：

（2）终点位置、速度和加速度：

（3）在运动过程中，所有关节的速度和加速度满足以下条件：

2、规划函数

  利用五次多项式控制机器人的轨迹。
（1）两个关节的转动角的函数：

（2）速度的表达式：

（3）加速度的表达式：

3、确定规划时间tf

  利用控制条件可以得到各个参数关于t_f的函数式：

4、求解参数

  前面已经得到了各个参数关于t_f的函数式：

  代入t_f即可得到各个参数值

5、绘制关节曲线和机器人运动状态图

（1）关节1的关节曲线

（2）关节2的关节曲线

（3）机器人运动状态图

6、代码提取

  相关代码可在https://github.com/Hny1216/2R_Robot_Five-foldPolynomialTrajectoryPlanning.git提取。