机器人单关节力矩控制

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

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

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

　　则系统的动力学方程为：$$\tau=M\ddot{\theta}+mgr\cos\theta$$

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

　　可将其改写为：$$\tau=M\ddot{\theta}+h(\theta,\dot{\theta})$$

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

•  PID控制

　　PID控制器时是最常用的反馈控制器。$$\boxed{\tau=K_p\theta_e+K_i\int\theta_e(t)dt+K_d\dot{\theta_e}}$$

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

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

 　　忽略积分项，即$K_i=0$，这就是PD控制。我们假设机器人在水平面上运动（g=0），将PD控制规则带入动力学方程中可得到$M\ddot{\theta}+b\dot{\theta}=K_p(\theta_d-\theta)+K_d(\dot{\theta_d}-\dot{\theta})$，如果给定的控制量$\theta_d$恒定，则$\dot{\theta_d}=\ddot{\theta_d}=0$，于是$\theta_e=\theta_d-\theta$，$\dot{\theta_e}=-\dot{\theta}$，$\ddot{\theta_e}=-\ddot{\theta}$。动力学方程可改写为误差$\theta_e$的二阶微分方程：$$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_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$和阻尼自然频率$\omega_n$为：$\xi=\frac{b+K_d}{2\sqrt{K_pM}}$、$\omega_n=\sqrt{\frac{K_p}{M}}$

 

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

　　考虑连杆在垂直水平面的平面上转动（g>0），用上面的PD控制器，误差动力学方程可写为：$$M\ddot{\theta_e}+(b+K_d)\dot{\theta_e}+K_p\theta_e=mgr\cos\theta$$

　　这意味着稳态时$\theta$满足$K_p\theta_e=mgr\cos\theta$，即当$\theta_d\neq\pm\pi/2$时最终误差$\theta_e$不为零，因为机器人需要提供一个非零的力矩使连杆保持在$\theta_d\neq\pm\pi/2$这个位置。