PID

原理

PID控制算法是一种线性控制器，它根据给定值（目标值）和实际输出值之间的偏差，利用比例（Proportional）、积分（Integral）、微分（Derivative）三种控制方式的组合来调整控制量，从而实现对被控对象的精确控制。

比例控制

根据偏差的大小，线性调整控制量。偏差越大，控制作用越强。

优点：响应速度快，能快速对偏差做出反应。
缺点：比例项较大时，会出现震荡，较小时会出现静态误差

积分控制

根据偏差的积分来调整控制量。它会累积过去的偏差，随着时间的推移，即使偏差很小，积分项也会不断积累，从而推动系统消除偏差

优点：能够消除稳态误差（静差），保证系统的精度。
缺点：积分作用较强时，容易导致系统超调，甚至使系统不稳定。

微分控制

是根据偏差的变化率来调整控制量。它能够预测偏差的变化趋势，提前做出调整。

优点：可以抑制偏差的变化，减少系统的超调，提高系统的稳定性。
缺点：对噪声比较敏感。如果测量的偏差信号中存在噪声，微分项会放大噪声的影响，导致控制量出现不必要的波动。微分项较大时也会出现震荡

常见组合

PI控制

优点：将比列调节的快速反应与积分调节消除静差的特点结合；
缺点：不对未来控制误差进行预测，限制了控制性能。

适用于控制通道滞后较小、负荷变化不大、工艺参数不允许有静差的系统。

PD控制

优点：对惯性较大对象，可改善控制质量，减小偏差，缩短控制时间。有利于提高系统响应速度，抑制动态偏差
缺点：抗干扰能力差，一般只能应用于被调参数变化平稳的生产过程；微分作用太强时，容易造成系统振荡。

位置式与增量式

位置式输出：

$u(t) = K_p e(t) + K_i \int_{0}^{t} e(\tau) \,d\tau + K_d \frac{de(t)}{dt}$

增量式输出：

$\Delta u(k) = K_p [e(k) - e(k-1)] + K_i e(k) + K_d ({e(k) - 2e(k-1) + e(k-2)})$ $u(k) = u(k-1) + \Delta u(k)$

位置式PID控制的输出与整个过去的状态有关，用到了误差的累加值;而增量式PID的输出只与当前拍和前两拍的误差有关，因此位置式PID控制的累积误差相对更大;

由于输出为增量形式，可以减小控制器对控制量的冲击，使得系统更加平稳。

抗积分饱和

PID控制器的输出由于积分作用的不断累加而扩大，从而导致控制器输出不断增大超出正常范围进入饱和区。为了防止这种情况，需要在某些情况下限制积分的增长：

积分分离：设置一个误差阈值，只有当误差较小（系统输出接近期望）时，才加入积分控制，平时则使用PD控制
积分限幅：设置一个输出阈值范围，计算U(k)的时候，先判断上一时刻的控制量U(k-1)是否已经超出了限制范围。若U(k-1)>Umax，则只累加负偏差；若U(k-1)<Umin，则只累加正偏差。

串级PID

以电机控制为例，在很多时候，我们的期望是位置（电机转角），但我们最终对电机的控制量都是电流（力矩，或者说是加速度），因此在使用单位置PID时，该控制是间接的，也就是说在该情况下，速度和加速度的变化都是未知的，也就是不平滑的；最终会表现为电机经过了目标位置，然后出现持续震荡，因为仅仅使用位置pid是难以让速度和加速度都达到期望的）

因此，我们引入串级PID：（由外到内）

位置环：根据上层规划算法给出的期望位置进行PID，输出期望速度
速度环：根据位置环得到的期望速度进行PID，输出期望转矩/加速度/电流
电流环：根据速度环输出进行PID，最终控制电机转矩

需要注意的是，位置-速度-电流这个控制顺序是我们主动给定的，给定的理由是这三者两两间存在着较为简单的映射关系，即微积分关系，对于加速度啊转矩啊电流啊，根据牛二、欧拉和电机参数都可以得到明确的对应关系，因此在最内环的控制中，我们可以将他们看作同一控制量，一般用电流的原因可能只是实际电流值要更好测量一点？

在机器人中

由于我们已经可以通过一些轨迹规划算法（模型）平滑地得到关节电机的期望角度、角速度、角加速度，再结合动力学模型便可得到期望力矩，因此在机器人控制中，上述的前馈控制已经完成了绝大部分的控制工作，在理想条件下，前馈足矣

但如果机器人在运行过程中受到了干扰呢，前馈控制给定了力矩是在最开始确定的，无法应对额外干扰，因此需要反馈控制来减弱干扰，提高系统稳定性：

joint_states = p.getJointStates(robotId,controllable_joints)
q_actual = np.array([state[0] for state in joint_states])
qd_actual = np.array([state[1] for state in joint_states])


q_e = q[n] - q_actual
qd_e = qd[n] - qd_actual


aq = qdd[n] + 400 * q_e + 40 * qd_e

tau = p.calculateInverseDynamics(robotId,list(q_actual),list(qd_actual),list(aq))
p.setJointMotorControlArray(robotId,controllable_joints,p.TORQUE_CONTROL,
                            forces = tau,
                            )

这里仅对位置环和速度环进行了比例控制，因为在实际中在外环部分添加积分(I)后，导致响应变慢，添加微分(D)容易引入噪声；而由于我们已经有期望位置、速度，因此也不需要像传统串级pid一样进行串联位置环与速度环，而是将两环同时作用于力矩控制

PID参数调节经验

KP：先设Ki=Kd=0，调Kp至系统出现轻微振荡；取此时Kp值的50%作为初始值
KI：逐步增加Ki至静差在3~5个控制周期内消除
KD：从Kp值的10%~20%开始调整

LQR

线性二次型调节器（Linear Quadratic Regulator），其中，线性指线性系统，二次型指目标函数

系统模型构建

首先，LQR作用于线性系统（对于非线性系统，需要先线性化），对于上图系统，其系统模型为

$\dot x = Ax + Bu$

其中，x为当前状态的集合，对于倒立摆，其可以为{位置，速度，角度，角速度}；u为我们的控制输入，A和B为系统决定的参数，系统模型建立了x（当前状态）和 x_dot（状态随时间的变化趋势）之间的关系。

现在我们要设计一个状态反馈u=-Kx，带入系统方程中则有：

$\dot x = (A-BK)x$

由于A和B在LQR设计阶段是已知矩阵，因此我们只需得到一个合适的增益矩阵K，即可实现期望控制

LQR控制目标

相比于PID的尽可能减小误差，LQR定义了一种代价函数，通过让该能量函数最小以实现最优控制：

其中，Q（状态权重矩阵）和R（控制输入权重矩阵）即是我们的期望闭环性能（也就是要调的参数），即当控制目标确定时，我们想要用最小的控制代价u得到最优的控制。通常，Q越大，系统相应越快；R越大，控制力惩罚越大（即力作用越平稳温柔）

现在，我们已经确定了A，B，Q，R，如何根据“代价函数最小”这个目标得到K呢？

线性二次型调节器(LQR)原理详解-CSDN博客

在假设系统稳定的情况下，我们可以通过一个riccati里卡提方程由“J最小”这个条件得到一个满足条件的辅助矩阵P，再由P得到K，对于求解里卡提方程，matlab，scipy都提供了对应函数

Q和R的选择

Q：半正定矩阵，通常选用对角矩阵，对角线元素表示各状态的权重（因此形状应为nxn，n为状态数量），因此需根据各状态的重要性分配权重；重点关注的状态应分配较大权重；如果系统响应较慢也可增加Q

R：正定矩阵，通常选用对角矩阵，对角线元素表示各控制输入的权重（因此形状应为mxm，m为控制量）；初始应选择较小值；如果控制输入幅度较大则增加R，如果系统响应较慢则减小R

倒立摆中的LQR

建立动力学方程

对于每个广义坐标q_i，拉格朗日方程为：

$\frac d {dt}(\frac{\part L}{\part \dot{q_i}})-\frac {\part L}{\part q_i} = Q_i$

其中，Q_i是广义坐标q_i对应广义力。在倒立摆中，q={x,θ}，则Q={F,0}（因为没有非保守力直接对摆杆做功）

则分别将q_i替换为x和θ后则可得到倒立摆的动力学模型（方程组）：

$(M + m)\ddot{x} + m l \ddot{\theta} \cos\theta - m l \dot{\theta}^2 \sin\theta = F$ $(m l^2 + I) \ddot{\theta}+m l \ddot{x} \cos\theta - m g l \sin\theta = 0$

线性化

由于现在的动力学方程是非线性的，因此我们需要先将其线性化。而在倒立摆中，平衡点即为θ=0(cosθ=0,sinθ=θ)，则线性化后有：

$(M + m)\ddot{x} + m l \ddot{\theta} = F$ $(m l^2 + I) \ddot{\theta}+m l \ddot{x} - m g l \theta = 0$

将其转换为线性系统标准模型：