独立关节PD控制及其matlab simulink仿真

原理

机器人控制常采用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.900.760.87{]}^T,q_0=[00{]}^T,{\dot{q}}_0=[00{]}^T$$,目标位置$$q_d(0)=[1.01.0{]}^T$$，取$$K_p=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right],K_d=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right]$$。

得到：
$$D(q)=\left[\begin{array}{cc}3.66+1.74\cos q_2& 0.76+0.87\cos q_2\\ 0.76+0.87\cos q_2& 0.76\end{array}\right],C(q,\dot{q})=\left[\begin{array}{cc}-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{array}\right]$$
代入刚刚得到的机器人方程，并选取系统状态变量$$[{\theta }_1,{\dot{\theta }}_1,{\theta }_2,{\dot{\theta }}_2]=[q_1,{\dot{q}}_1,q_2,{\dot{q}}_2]$$：
$$\{\begin{array}{l}{\dot{\theta }}_1={\dot{\theta }}_1\\ {\ddot{\theta }}_1=\frac{{\tau }_1-(0.76+0.87cos{\theta }_2){\ddot{\theta }}_2+0.87sin({\theta }_2){\dot{\theta }}_1{\dot{\theta }}_2+0.87({\dot{\theta }}_1+{\dot{\theta }}_2)sin{\theta }_2{\dot{\theta }}_2}{(3.66+1.74cos{\theta }_2)}\\ {\dot{\theta }}_2={\dot{\theta }}_2\\ {\ddot{\theta }}_2=\frac{{\tau }_2-(0.76+0.87cos{\theta }_2){\ddot{\theta }}_1-0.87sin{\theta }_2({\dot{\theta }}_1{)}^2}{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{array}$$

Matlab/simulink 仿真建模

依据上面得到的式子建立matlab simulink仿真模型：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-r7H7RAFz-1610609990391)(image-20210107132308822.png)]

注意计算$${\ddot{q}}_1,{\ddot{q}}_2$$使用函数模块时候不能选择这个：

否则就会报错：

要选择这个：

示波器输出波形为：

修改Kp，Kd，使得$$K_p=\left[\begin{array}{cc}1000& 0\\ 0& 1000\end{array}\right],K_d=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right]$$，得到：

调节时间缩短，响应变快。

修改Kp，Kd，使得$$K_p=\left[\begin{array}{cc}100& 0\\ 0& 100\end{array}\right],K_d=\left[\begin{array}{cc}10& 0\\ 0& 10\end{array}\right]$$，得到：

出现过多超调，响应时间变长。

加入重力补偿

事实上完全不受外力，没有干扰的机械手系统是不存在的，独立的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$$
对重力矩的估计方法有如下两类：

1. 当对重力矩估计准确时候，$$\hat{G}(q),G(q)$$抵消，方程退化为上面的独立PD控制。
2. 当对重力矩的估计值不准确时候，需要设计重力补偿算法。目前代表性的重力补偿PD控制算法有在线估计的和固定的重力补偿PD控制。其中在线补偿更为准确，但是会消耗更多的计算资源，固定的补偿特性与之相反。