1.动力学方程

六自由度机械臂动力学方程形式如下：

进行重力补偿，就是在驱动力矩中对重力G进行补偿，从而消除重力的影响，这样就能够在进行闭环控制的时候避免重力影响带来的大超调问题，使得机器人更好的实现轨迹跟踪控制。消除重力影响后，机器人的动力学方程变成如下：

2.四种控制状态分析

2.1只进行重力补偿时机械臂的运动状态

分析上述公式可知，当机械臂刚开始处于静止状态时，即速度dq和加速度ddq均为0，惯性项M*ddq和科式力项C*dq均为0，此时机器人的动力学方程变为：

当摩擦较小时，可以忽略摩擦的影响，当施加在机器人末端的外力fe=[0,0,0,0,0,0]'且施加的驱动力矩tol=G时，机械臂达到平衡状态，此时机械臂会静止不动，其动态过程如下：

2.2重力补偿并在末端施加外力

分析如下方程：

当对机械臂的末端施加外力时，即fe不等于0，上述方程达到动态平衡状态，此时机械臂将按照所施加力的方向进行运动，下图展示了机械臂沿y轴运动的过程：

2.3重力补偿控制

 我们在2.1节中只是对机械臂进行了重力补偿，并没有施加控制作用。为了使得机械臂能够进行轨迹跟踪，采用重力补偿加PD的控制方式对机械臂进行控制，控制率如下:

其中qe是机械臂的关节位置误差，dqe是机械臂的关节速度误差，此时在机械臂动力学方程中，不考虑摩擦和外力的影响。此时机械臂的动力学方程如下：

 

 下图展示了机械臂进行轨迹跟踪的效果：

 

由上图可知，由于重力补偿的作用，机械臂在PD控制作用下能较好的实现轨迹跟踪。但是观察机械臂的关节力矩图可知，在开始阶段，由于存在较大的初始误差，机械臂的驱动力矩较大，达到了上百，这显然超出了电机的实际性能，与实际过程不符合，因此在实际控制时，需要对关节最大驱动力矩进行限幅处理。

2.4施加摩擦扰动时的重力补偿控制

在2.3节没有考虑摩擦的影响，本节为了验证重力补偿控制的控制效果，考虑了库伦摩擦和粘滞摩擦，摩擦力的数学表达式如下：

此时机械臂的动力学方程如下：

 

由于没有扰动观测器，此时机械臂的控制率仍如下：

 

接下来，看一下考虑存在摩擦时候的轨迹跟踪效果：

 

3.仿真效果展示

 仿真效果如下：

技术交流邮箱（欢迎交流、讨论、私信）：3531225003@qq.com