pid和lqr两轮平衡车公式推导

PID是一种常用的控制算法，用于控制系统的运动控制。平衡车是一种典型的非线性系统，PID控制算法可以帮助平衡车实现平稳的运动。

首先，我们先将平衡车建模为一个非线性系统。假设平衡车的状态由两个变量θ和ω表示，分别代表车身的倾斜角度和角速度。根据平衡车的运动学和动力学方程，可以得到如下状态方程：

θ' = ω
ω' = -g/l * sin(θ) - k1 * ω + k2 * u

其中，θ'和ω'表示对应变量的导数，g为重力加速度，l为平衡车的质心高度，k1和k2为控制器的参数，u为控制器的输出。

接下来，我们考虑设计PID控制器来控制平衡车的倾斜角度。PID控制器由比例项、积分项和微分项组成，根据控制理论的知识，可以得到如下控制器输出的公式：

u = Kp * e + Ki * ∫e dt + Kd * de/dt

其中，e为期望倾斜角度与实际倾斜角度的差值，Kp、Ki和Kd为控制器的参数。

代入状态方程，我们可以得到如下PID控制器的输出方程：

u = Kp * (θd - θ) + Ki * ∫(θd - θ) dt + Kd * (ωd - ω)

在实际控制中，我们可以根据系统的需求和实际情况来选择合适的PID参数。

LQR（线性二次型调节）是一种优化控制方法，用于设计最优的线性控制器。它能够最小化系统输出和期望输出之间的差距，并尽量减小控制器的输出。对于平衡车问题，LQR可以用来设计一个最优的线性控制器。

首先，我们将平衡车的状态方程线性化，得到一个线性系统的状态方程：

θ' = ω
ω' = -g/l * θ - k1 * ω + k2 * u

然后，根据LQR的理论，我们可以定义一个性能指标J，表示系统输出与期望输出之间的差距的平方和。J的定义如下：

J = ∫(θ - θd)^2 + q * ω^2 + r * u^2 dt

其中，q和r是权重参数，用来调整角速度和控制器输出的权重。我们的目标是最小化J。在LQR中，我们采用二次型的形式，将J表示为一个矩阵的二次型。

接下来，我们利用最小二乘原理求解LQR问题，得到最优的线性控制器K。具体的推导过程涉及到矩阵运算和最优化的方法，这里不做详细展开。

最后，根据计算得到的最优线性控制器K，我们可以用于实际的平衡车控制中。通过将实际状态代入线性控制器中，就可以得到控制器的输出u，从而实现平衡车的平稳运动。