机器人单关节力矩控制

XXX已失联

发布时间 2021.11.26阅读数 5226 评论数 0

　　对于自由运动机器人来说，控制的目的是要控制机器人末端的位置和姿态（统一简称为位置），即所谓的位置控制问题。期望机器人末端达到的位置称为期望位置或期望轨迹，期望轨迹可以在机器人任务空间中给出，也可以通过逆运动学转化为机器人关节空间中的期望轨迹。期望轨迹通常有两种形式：一种是一个固定位置（setpoint），另一种是一条随时间连续变化的轨迹（trajectory）。

　　对于运动受限的机器人来说，其控制问题要复杂得多。由于机器人与环境接触，这时不仅要控制机器人末端位置，还要控制末端作用于环境的力。也就是说，不仅要使机器人末端达到期望值，还要使其作用于环境的力达到期望值。更广泛意义下的运动受限机器人还包括机器人协同工作的情况，这时控制还应包括各机器人间运动的协调，负荷的分配以及所共同夹持的负载所受内力的控制等复杂问题。

　　如下图所示，考虑单个电机与连杆的模型，其中 $\tau$ 为电机施加的力矩， $\theta$ 是连杆与水平线的夹角：

　　则系统的动力学方程为：

τ = M θ ¨ + m g r cos θ

$\tau=M\ddot{\theta}+mgr\cos\theta$

　　 $M$ 是连杆绕转轴的转动惯量， $m$ 是连杆的质量， $r$ 是转轴到连杆质心的距离， $g$ 是重力加速度。实际情况中转轴处会存在摩擦力，这里简单假设摩擦力为粘滞阻力，即 $τ_{f r i c} = b \dot{θ}$ ，将阻力项加入上面的动力学方程中，模型变为：

τ = M θ ¨ + m g r cos θ + b θ ˙

$\tau=M\ddot{\theta}+mgr\cos\theta+b\dot{\theta}$

　　可将其改写为：

τ = M θ ¨ + h (θ, θ ˙)

$\tau=M\ddot{\theta}+h(\theta,\dot{\theta})$

　　为进行仿真，设参数 $M = 0.5 k g \cdot m^{2}$ ， $m = 1 k g$ ， $r = 0.1 m$ ， $b = 0.1 N \cdot m \cdot s / r a d$ ，当连杆在平面上运动时，当连杆垂直水平面运动时 $g=9.81m/s^2$

PID控制

　　PID控制器时是最常用的反馈控制器。

τ = K p θ e + K i \int θ e (t) d t + K d θ e ˙

$\boxed{\tau=K_p\theta_e+K_i\int\theta_e(t)dt+K_d\dot{\theta_e}}$

　　其中比例系数 $K_p$ 的作用就像一个虚拟弹簧，试图减小位置误差 $θ_{e} = θ_{d} - θ$ ；微分系数 $K_{d}$ 的作用像虚拟阻尼器，试图减小速度误差 $\dot{θ_{e}} = \dot{θ_{d}} - \dot{θ}$ ；积分系数可以减小稳态误差。

PD控制器与误差的二阶方程

　　忽略积分项，即 $K_{i} = 0$ ，这就是PD控制。我们假设机器人在水平面上运动（g=0），将PD控制规则带入动力学方程中可得到 $M \ddot{θ} + b \dot{θ} = K_{p} (θ_{d} - θ) + K_{d} (\dot{θ_{d}} - \dot{θ})$ ，如果给定的控制量 $θ_{d}$ 恒定，则 $\dot{θ_{d}} = \ddot{θ_{d}} = 0$ ，于是 $θ_{e} = θ_{d} - θ$ ， $\dot{θ_{e}} = - \dot{θ}$ ， $\ddot{θ_{e}} = - \ddot{θ}$ 。动力学方程可改写为误差 $θ_{e}$ 的二阶微分方程：

M θ e ¨ + (b + K d) θ e ˙ + K p θ e = 0

$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_e=0$

　　将其改写为标准二阶形式：

θ e ¨ + b + K d M θ e ˙ + K p M θ e = 0 \to θ e ¨ + 2 ξ ω n θ e ˙ + ω 2 n θ e = 0

$\ddot{\theta_e}+\frac{b+K_d}{M}\dot{\theta_e}+\frac{K_p}{M}\theta_e=0\rightarrow\ddot{\theta_e}+2\xi\omega_n\dot{\theta_e}+\omega_n^2\theta_e=0$

　　阻尼比 $\xi$ 和阻尼自然频率 $ω_{n}$ 为：、 $\omega_n=\sqrt{\frac{K_p}{M}}$

PID控制器与误差的三阶方程

　　考虑连杆在垂直水平面的平面上转动（g>0），用上面的PD控制器，误差动力学方程可写为：

M θ e ¨ + (b + K d) θ e ˙ + K p θ e = m g r cos θ

$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_e=mgr\cos\theta$

　　这意味着稳态时 $\theta$ 满足 $K_{p} θ_{e} = m g r \cos θ$ ，即当 $θ_{d} \neq \pm π / 2$ 时最终误差 $θ_{e}$ 不为零，因为机器人需要提供一个非零的力矩使连杆保持在 $θ_{d} \neq \pm π / 2$ 这个位置。

　　设 $K_{p} = 2.205 N \cdot m / r a d$ ， $K_{d} = 2 N \cdot m \cdot s / r a d$ ，连杆初始角度 $θ (0) = - π / 2$ ，初始角速度 $\dot{θ} (0) = 0$ ，期望角度 $θ_{d} = 0$ ，期望角速度 $\dot{θ_{d}} = 0$ 。根据所设置的参数在Mathematica中求解微分方程，结果如下：

　　稳态误差可求解方程 $K_{p} (θ_{d} - θ) = m g r \cos θ$ 求得，约为-0.408弧度，即稳态时与期望值相差23.4°

　　可以在VREP中仿真该过程，新建一个长度为0.2m的圆柱体，质量设为1kg，在圆柱体一段添加旋转关节，圆柱体X、Y轴的转动惯量设为0.49(根据转动惯量的平行轴定理，绕关节转动的转动惯量为0.5)。关节设为力矩模式，打开关节动力学属性对话框，设置PID参数和目标位置（90°）：

　　选择ODE物理引擎进行仿真，打开圆柱体动力学属性对话框，点击编辑材料（Edit material），在Angular damping中输入0.1（Angular damping：an angular movement damping value, that adds angular drag, and that can increase stability）。给关节添加阻力可参考Set internal friction of joints和Material properties。

　　添加Graph记录关节转角进行仿真，可以看出存在稳态误差：

　　为了消除稳态误差，设 $K_{i} > 0$ ，即使用PID控制器。积分控制的作用是消除稳态误差，因为系统只要存在误差，积分作用就不断地积累，输出控制量，直到偏差为零，积分作用才会停止。但积分作用太强会使系统超调加大，甚至使系统出现振荡。加入积分项后关于误差的微分方程为： $M \ddot{θ_{e}} + (b + K_{d}) \dot{θ_{e}} + K_{p} θ_{e} + K_{i} \int θ_{e} (t) d t = τ_{d i s t}$ ，其中 $τ_{d i s t}$ 是重力项 $m g r \cos θ$ 。对该方程两边求导，可以得到误差的三阶微分方程：

M θ (3) e + (b + K d) θ e ¨ + K p θ e ˙ + K i θ e = τ d i s t ˙

$M\theta^{(3)}_e+(b+K_d)\ddot{\theta_e}+K_p\dot{\theta_e}+K_i\theta_e=\dot{\tau_{dist}}$

　　如果 $τ_{d i s t}$ 为常量（比如稳态时），方程右边 $\dot{τ_{d i s t}}$ 项为零。使用拉普拉斯变换将时域中的微分方程转换为复数域中的代数方程：

s 3 + b + K d M s 2 + K p M s + K i M = 0

$s^3+\frac{b+K_d}{M}s^2+\frac{K_p}{M}s+\frac{K_i}{M}=0$

　　如果该特征方程根的实部全为负，则该系统稳定，且 $θ_{e}$ 趋于零。根据系统特征方程的劳斯判据，为了保证方程所有根的实部都为负，PID控制器的系数必须满足下列条件：

K d K p ( b + K d ) K p M > K i > - b > 0 > 0

$\begin{align*}K_d&>-b\\K_p&>0\\\frac{(b+K_d)K_p}{M}>K_i&>0\end{align*}$

　　因此 $K_{i}$ 的值必须保证落在上下界限之内。通常先选择合适的 $K_{p}$ 、 $K_{d}$ 值加快暂态过程，然后调整 $K_{i}$ 使其能大到消除稳态误差，又不至于过大而影响稳定性。画出根轨迹图（根轨迹是指系统开环传递函数中某个参数从零变到无穷时，闭环特征根s在平面上移动的轨迹），可以看出当 $K_{i} = (b + K_{d}) K_{p} / M$ 时两个根将到达虚轴， $K_{i}$ 继续变大根会进入复平面的右半部分，此时系统将变得不稳定。

　　PID控制的伪代码如下：

time = 0                          // dt = servo cycle time
eint = 0                          // error integral
qprev = senseAngle                // initial joint angle q

loop
    [qd,qdotd] = trajectory(time) // from trajectory generator
   
    q = senseAngle                // sense actual joint angle
    qdot = (q - qprev)/dt         // simple velocity calculation
    qprev = q
   
    e = qd - q
    edot = qdotd - qdot
    eint = eint + e*dt
   
    tau = Kp*e + Kd*edot + Ki*eint
    commandTorque(tau)
   
    time = time + dt
end loop

　　使用之前建立好的VREP模型，设置积分系数 $K_{i} = 1$ 后再进行仿真：

　　可以看出添加积分项后稳态误差消除（但是产生了超调量）：

　　实际上在许多机器人控制器中设 $K_{i} = 0$ ，即不使用积分项。因为相比稳态误差，稳定性才是最重要的考虑因素。

前馈控制（Feedforward Control）

　　PID控制不考虑机器人的动力学特性，只按照偏差进行负反馈控制，即只有在得到误差信号后才能输出控制量。另一种控制策略是根据机器人动力学模型预先产生控制力/力矩。动力学模型为：

τ = M ˜ (θ) θ ¨ + h ˜ (θ, θ ˙)

$\tau=\widetilde{M}(\theta)\ddot{\theta}+\widetilde{h}(\theta,\dot{\theta})$

　　根据轨迹生成器输出的期望位置 $θ_{d}$ 、速度 $\dot{θ_{d}}$ 、加速度 $\ddot{θ_{d}}$ ，前馈力矩为：

τ (t) = M ˜ (θ d (t)) θ d ¨ (t) + h ˜ (θ d (t), θ d ˙ (t))

$\tau(t)=\widetilde{M}(\theta_d(t))\ddot{\theta_d}(t)+\widetilde{h}(\theta_d(t),\dot{\theta_d}(t))$

　　前馈控制伪代码如下：

time = 0                                           // dt = servo cycle time
loop
    [qd,qdotd,qdotdotd] = trajectory(time)         // trajectory generator
    tau = Mtilde(qd)*qdotdotd + htilde(qd,qdotd)   // calculate dynamics
    commandTorque(tau)
    time = time + dt
end loop

　　如果动力学模型完全准确，那么用前馈控制即可直接计算力矩。但实际上模型总是存在各种误差，因此前馈控制一般总是与反馈控制结合起来使用。比如下面这个例子，如果 $\tilde{r} = 0.08 m$ ，即动力学公式中转轴到质心距离这个参数为0.08m（实际上这个值为0.1m）。因为存在测量和制造误差，准确的动力学模型很难得到。考虑2个任务：任务1连杆运动轨迹为 $θ_{d} (t) = - \frac{π}{2} - \frac{π}{4} \cos (t)$ ，运动时间为 $0 ⩽ t ⩽ π$ ；任务2连杆运动轨迹为，运动时间为 $0\leqslant t \leqslant \pi$

　　可以看出模型存在误差的情况下，根据动力学方程计算力矩而得到的结果与期望值之间存在偏差。对于任务2重力会阻碍期望的运动，导致产生较大的跟踪误差。

　　对任务2，在MATLAB/Simulink中进行仿真也可以得到同样的结果：

　　其中arm dynamics子模块根据输入的力矩计算并输出角度或角速度：

　　期望角度轨迹（蓝线）与根据不准确的动力学模型计算的实际轨迹（橙线）如下图所示：

前馈-反馈控制（feedforward and feedback control）

　　前馈控制是一种预测控制，通过对系统当前工作状态的了解，预测出下一阶段系统的运行状况。前馈的缺点是在使用时需要对系统有精确的了解，只有了解了系统模型才能有针对性的给出预测补偿。但在实际工程中并不是所有的对象都是可得到精确模型的，而且很多控制对象在运行的同时自身的结构也在发生变化。所以仅用前馈并不能达到良好的控制品质。这时就需要加入反馈，反馈的特点是根据偏差来决定控制输入，不管对象的模型如何，只要有偏差就根据偏差进行纠正，可以有效的消除稳态误差。

　　前馈-反馈综合控制结合二者的优点，可以提高系统响应速度。从前馈控制角度看，由于增加了反馈控制，降低了对前馈控制模型精度的要求；从反馈控制角度看，前馈控制作用对主要干扰及时进行粗调，大大减少反馈控制的负担。

　　结合前馈与反馈，实际角加速度 $\ddot{\theta}$ 可写为：

θ ¨ = θ d ¨ + K p θ e + K d θ e ˙ + K i \int θ e (t) d t

$\ddot{\theta}=\ddot{\theta_d}+K_p\theta_e+K_d\dot{\theta_e}+K_i\int\theta_e(t)dt$

　　将其带入动力学预测方程中可得到前馈-反馈控制器：

τ = M ˜ (θ) (θ d ¨ + K p θ e + K i \int θ e (t) d t + K d θ e ˙) + h ˜ (θ, θ ˙) 前馈-反馈控制器的结构如下图所示，通常 K i 设为零，即使用PD+前馈控制：

　　前馈-反馈控制的伪代码如下：

time = 0                                   // dt = cycle time
eint = 0                                   // error integral
qprev = senseAngle                         // initial joint angle q
loop
    [qd,qdotd,qdotdotd] = trajectory(time) // from trajectory generator
    
    q = senseAngle                         // sense actual joint angle
    qdot = (q - qprev)/dt                  // simple velocity calculation
    qprev = q
    
    e = qd - q
    edot = qdotd - qdot
    eint = eint + e*dt
   
    tau = Mtilde(q)*(qdotdotd+Kp*e+Kd*edot+Ki*eint) + htilde(q,qdot)
    commandTorque(tau)
    
    time = time + dt
end loop

　　单独使用PID控制的Simulink模型如下图所示(参数与任务2中一样)：

　　前馈-反馈控制的模型如下图所示（PID参数与任务2一样）：

　　子模块 $\tilde{h} (θ, \dot{θ})$ 如下图所示：

　　各控制方法输出的曲线与期望轨迹的对比如下图所示（固定步长0.01s，仿真时间t=pi）：

　　可以看出单独使用前馈控制存在很大跟踪误差，单独使用PID反馈控制对轨迹的跟踪效果也不理想。前馈+反馈控制方案结合了二者的优点，动态跟踪误差最小。

参考：

《机械控制工程基础》

《机器人动力学与控制》霍伟

V-rep学习笔记：转动关节2

V-rep学习笔记：关节力矩控制

前馈控制、反馈控制及前馈-反馈控制的对比

基于Mathematica的机器人仿真环境（机械臂篇）

浅显直观地解释机器人控制为什么需要动力学建模

Modern Robotics: Mechanics, Planning, and Control Code Library

Modern Robotics Mechanics, Planning, and Control Chapter 11.4 Motion Control with Torque or Force Inputs

机械臂建模仿真 vrep 力矩控制

打赏 0

上一篇：机器人中的轨迹规划（Trajectory Planning ）

下一篇：机器人关节空间轨迹规划--S型速度规划

机器人单关节力矩控制

XXX已失联

为你推荐

机器人阻抗与导纳控制

旋转矩阵、欧拉角之间转换

【关于四足机器人那些事】腿部运动学代数求解方法

13. 机器人正运动学---雅克比矩阵(1)

机器人学——学习笔记15(Linear Function with ParabolicBlends)

刚体质量分布与牛顿-欧拉方程

关于作者

XXX已失联

34

0

0

2

V-rep中的加速度计与陀螺仪

V-rep中的加速度计与陀螺仪

orocos_kdl学习（二）：KDL Tree与机器人运动学

相关推荐

运动规划入门 | 3. 白话PRM，从原理到Matlab实现

UR机械臂simscape正逆解仿真

机器人运动估计——IMU运动方程与ESKF原理介绍（上）

UR5型六轴机械臂运动学推导及效果演示

机械臂动力学（6）

运动规划入门 | 4. 白话RRT，从原理到Matlab实现

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

机器人单关节力矩控制

XXX已失联

为你推荐

机器人阻抗与导纳控制

旋转矩阵、欧拉角之间转换

【关于四足机器人那些事】腿部运动学代数求解方法

13. 机器人正运动学---雅克比矩阵(1)

机器人学——学习笔记15(Linear Function with ParabolicBlends)

刚体质量分布与牛顿-欧拉方程

评论（0）

关于作者

XXX已失联

34

0

0

2

V-rep中的加速度计与陀螺仪

V-rep中的加速度计与陀螺仪

orocos_kdl学习（二）：KDL Tree与机器人运动学

相关推荐

运动规划入门 | 3. 白话PRM，从原理到Matlab实现

UR机械臂simscape正逆解仿真

机器人运动估计——IMU运动方程与ESKF原理介绍（上）

UR5型六轴机械臂运动学推导及效果演示

机械臂动力学（6）

运动规划入门 | 4. 白话RRT，从原理到Matlab实现

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板