Unity PID 控制算法可视化学习

Unity PID控制算法可视化学习

引言

最近研究单片机时，偶然间对PID算法进行了点研究，PID真可谓是一个很优雅的算法。用它可以搞平衡车平衡、无人机飞行控制、温度控制。。等等，用处很多，很强大。于是打算用Unity写个小程序，来模拟pid的调参过程。

想法，需求

设有1000m³的水池，供某小区用水，该小区用水量随着时间变化而变化（消耗水池中的水），即负载是时刻变化的；另外，有一个注水开关，可以往水池中注水，现要求写一个控制算法，控制注水开关的开合度，以尽量保持水池中的水位在500m³。

出水、注水最大速度均为100m³/秒
水位传感器读取频率为1次/秒

思路

一 消耗水

水的消耗速度，随机进行变化，但这个指标，就像温度一样，它不能突变，所以写了个小算法，模拟水的消耗速度：

private bool bGrow = true;
public float CurrValue { get; private set; }
private void MakeData()
{
    float mid = ( maxValue - minValue ) * 0.5f + minValue;
    float g;
    if (bGrow)
        g = CurrValue > mid ? Mathf.InverseLerp(mid*1.2f, maxValue, CurrValue) : 0.1f;
    else
        g = CurrValue < mid ? Mathf.InverseLerp(mid*0.8f, minValue, CurrValue) : 0.1f;

    // g为概率
    if (RandomBool(g))
        bGrow = !bGrow;
    
    if( bGrow )
        CurrValue += Random.Range(0f, 5f);
    else
        CurrValue -= Random.Range(0f, 5f);
}

注：
实际上这里更优雅的做法是使用柏林噪声来模拟消耗速度的变化，但是写这篇文章时，我还没有了解到柏林噪声这个神奇的玩意，所以自己写了个比较笨拙的算法去模拟的：

网上搜了很久，没找到更优雅的算法，这是自己写的，大体意思就是：用个bool值表示现在的速度趋势是趋于上升还是下降，然后每次更新数据时，计算一个概率去反转这个方向，如果当前值越接近最大值或者接近最小值，反转的概率就越大，反之，值越靠近中间，反转的概率就越小，然后每次根据这个趋势增加或减少一个随机值。这样，就能大体模拟用水的速度曲线，而且大致上在大周期内往复，正好符合用水高峰的设定。

控制算法

$$\begin{gathered} 当前误差=目标水位-当前水位 \\ 累计误差=累计误差+当前误差\ast \Delta Time \\ 误差变化率=(当前误差-上次误差)/\Delta Time \\ 期望注水速度=当前误差\ast K_p+累计误差\ast K_i+误差变化率\ast K_d \\ 注水速度=Mathf.Clamp(期望注水速度,0,最大注水速度) \end{gathered}$$

// 每1秒读取一次水位传感器
private IEnumerator PIDController()
{
    while (gameObject.activeSelf)
    {
        float err = targetPool - waterLeft;
        totalErr += err * controlTime;
        float de = (err - lastError) / controlTime;
        lastError = err;

        pidText.text = $"Kp={kp:0.0} Ki={ki:0.0} Kd={kd:0.0} Err={err:#00.00} TotalErr:{totalErr:00.00} deltaErr:{de:00.00}";
        
        productionSpeed = Mathf.Clamp(err * kp + totalErr * ki + de * kd, 0f, 200f);
        inSpeedTex.text = productionSpeed.ToString("000");
        yield return new WaitForSeconds(controlTime);
    }
}

更新水位

private void Update()
{
    waterLeft = Mathf.Clamp(waterLeft + (productionSpeed - CurrValue) * Time.deltaTime, 0f, 1000f);
}

效果图

上面的折线图是消耗水的速度数据；下面的折线图是水池水位历史数据，左边的柱状图是当前水位。

调参过程

先将KI、KD置零，首先调整KP，KP其实就是比例因子，起主导作用，逐渐调大KP，使得控制曲线刚刚上下震动时，往下调一点；
然后调KD，KD是抑制因子，表示误差的趋势，用于减弱当前的调整趋势，KD过大会高频振荡，过小起不到抑制KP震动的效果。
最后调KI，KI是增益因子，误差积分而来，用于补充长期以来的误差。
当KP=1.5-2，KI=0.1-0.2，KD=0.1-0.2时，效果很好。基本上控制曲线是一条稳定的直线，无论消耗速度如何变化，水位始终保持在500m³（50%）。