摘要
如题。
（转自我的CSDN博客）
原理
机器人控制常采用PD控制和PID控制，其优点是控制律简单，易于实现，无需建模。
当忽略重力和外力干扰时，采用独立的PD控制，能满足机器人定点控制的要求。
根据欧拉-拉格朗日方程建立n连杆机器人的动力学模型： \[ D(q)\ddot q+C(q,\dot q)\dot q+g(q)=\tau \] 其中D代表惯性项，C代表向心力项/哥氏力项，g代表重力项，q代表关节坐标，\[\tau\]代表了系统广义力。
PID控制基于误差，设固定的期望位置为 \[ q_d \]，跟踪误差为\[e=q_d-q\]，由于\[q_d\]为常数，\[\dot q_d=\ddot q_d =0\]。
PD控制律为： \[ \tau = K_d \dot e + K_p e \] 忽略重力，机器人方程表示为： \[ D(q)(0 - \ddot q)+C(q,\dot q)(0 -\dot q)=- \tau \] 进一步化为： \[ D(q)(\ddot q_d - \ddot q)+C(q,\dot q)(\dot q_d -\dot q)+ K_d \dot e =-K_p e \]
考虑如下二连杆机器人系统：
其中：
实例化，假设\[ p=[2.90{\,} 0.76 {\,}0.87]^T, q_0 =[0 {\,}0]^T, \dot q_0 = [0{\,} 0 ]^T\],目标位置\[q_d(0)=[1.0 {\,}1.0]^T\]，取\[K_p=\begin{bmatrix}100 &0\\0&100 \end{bmatrix}, K_d=\begin{bmatrix}100 &0\\0&100 \end{bmatrix}\]。
得到： \[ D(q)=\begin{bmatrix}3.66+1.74\cos q_2 &0.76+0.87\cos q_2\\0.76+0.87\cos q_2&0.76 \end{bmatrix}, C(q,\dot q)=\begin{bmatrix}-0.87 \dot q_2\sin q_2 &-0.87(\dot q_1 + \dot q_2)\sin q_2\\0.87 \dot q_1\sin q_2&0 \end{bmatrix} \] 代入刚刚得到的机器人方程，并选取系统状态变量\[[θ_1,\dot θ_1 ,θ_2,\dot θ_2] = [q_1,\dot q_1, q_2,\dot q_2]\]： \[ \begin{cases} \dot θ_1 = \dot θ_1 \\ \ddot \theta_1 = {τ_1-(0.76+0.87cosθ_2 ) \ddot θ_2 + 0.87sin(θ_2 )\dot θ_1 \dot θ_2 +0.87(\dot θ_1 +\dot θ_2)sinθ_2 \dot θ_2 \over (3.66+1.74cosθ_2 )} \\ \dot θ_2 =\dot θ_2 \\ \ddot \theta_2 = {τ_2-(0.76+0.87cosθ_2 ) \ddot θ_1 -0.87sinθ_2 (\dot θ_1 )^2 \over 0.76}\\ \tau_1 = K_p(q_{d1}-q_1)-K_d \dot q_1 \\ \tau_2 = K_p(q_{d2}-q_2)-K_d \dot q_2 \end{cases} \]
Matlab/simulink 仿真建模
依据上面得到的式子建立matlab simulink仿真模型：
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-r7H7RAFz-1610609990391)(image-20210107132308822.png)]
注意计算\[\ddot q_1, \ddot q_2\]使用函数模块时候不能选择这个：
否则就会报错：
要选择这个：
示波器输出波形为：
修改Kp，Kd，使得\[K_p=\begin{bmatrix}1000&0\\0&1000 \end{bmatrix}, K_d=\begin{bmatrix}100 &0\\0&100 \end{bmatrix}\]，得到：
调节时间缩短，响应变快。
修改Kp，Kd，使得\[K_p=\begin{bmatrix}100&0\\0&100 \end{bmatrix}, K_d=\begin{bmatrix}10 &0\\0&10 \end{bmatrix}\]，得到：
出现过多超调，响应时间变长。
加入重力补偿
事实上完全不受外力，没有干扰的机械手系统是不存在的，独立的PD控制只是基础，可以作为参考来对系统进行分析。如果加入重力补偿，PD控制律变为： \[ \tau = K_d \dot e + K_p e + \hat G(q) \] 其中\[\hat G(q)\]表示对重力矩的估计值。
机器人方程为： \[ D(q)(\ddot q_d - \ddot q)+C(q,\dot q)(\dot q_d -\dot q)+ K_d \dot e + K_p e+\hat G(q)-G(q)=0 \] 对重力矩的估计方法有如下两类：
- 当对重力矩估计准确时候，\[\hat G(q), G(q)\]抵消，方程退化为上面的独立PD控制。
- 当对重力矩的估计值不准确时候，需要设计重力补偿算法。目前代表性的重力补偿PD控制算法有在线估计的和固定的重力补偿PD控制。其中在线补偿更为准确，但是会消耗更多的计算资源，固定的补偿特性与之相反。