PID制御を用いて推進力を求める [Unity]

PID制御を用いて推進力を求めるスクリプトを実装します。

実装

まず、PID制御で用いる比例ゲイン、積分ゲイン、微分ゲインをまとめて保持する構造体を定義します。

Gains.cs

using System;

/// <summary>
/// PID制御で用いるゲインを保持する構造体
/// </summary>
[Serializable]
public struct Gains
{
    /// <summary>
    /// 比例ゲイン
    /// </summary>
    public float P;
    /// <summary>
    /// 積分ゲイン
    /// </summary>
    public float I;
    /// <summary>
    /// 微分ゲイン
    /// </summary>
    public float D;


    public Gains(float p, float i, float d)
    {
        P = p;
        I = i;
        D = d;
    }
}

このGains構造体を用いてPID制御の計算を行うクラスを実装します。

PidVector3Controller.cs

using UnityEngine;

public class PidVector3Controller
{
    public Gains Gains;

    private Vector3 intError;
    private Vector3 prevError;

    public PidVector3Controller()
    {
        Gains = new Gains();
    }

    public PidVector3Controller(float p, float i, float d)
    {
        Gains = new Gains(p, i, d);
    }

    public PidVector3Controller(Gains gains)
    {
        Gains = gains;
    }

    /// <summary>
    /// PID制御を用いて現在の座標から特定の座標へ移動するために必要な推進力を計算する
    /// </summary>
    /// <param name="currentPos">現在の座標</param>
    /// <param name="desiredPos">目標座標</param>
    /// <param name="deltaTime">前回呼び出しからの経過時間(Time.deltaTimeなど)</param>
    /// <returns>計算した推進力</returns>
    public Vector3 ComputeRequiredForce(Vector3 currentPos, Vector3 desiredPos, float deltaTime)
    {
        // 差分
        var error = desiredPos - currentPos;
        // 積分
        intError += error * deltaTime;
        // 微分
        var diffError = (error - prevError) / deltaTime;

        prevError = error;

        var requiredForce =
            Gains.P * error +
            Gains.I * intError +
            Gains.D * diffError;
        return requiredForce;
    }
}

使い方

こんな感じのスクリプトを用意してPidVector3Controllerを用いて計算することで、対象にPID制御を用いて接近することができます。
注意点として、Gainsで保持するパラメータは設計値なので、実験して適切な値を設定する必要があります。

PidSphere.cs

using UnityEngine;

[DisallowMultipleComponent]
[RequireComponent(typeof(Rigidbody))]
public class PidSphere : MonoBehaviour
{
    [SerializeField] private Gains gains;
    [SerializeField] private Transform target;

    private Rigidbody rb;
    private PidVector3Controller pidController;

    private void Awake()
    {
        TryGetComponent(out rb);
    }

    private void Start()
    {
        pidController = new PidVector3Controller(gains);
    }

    private void FixedUpdate()
    {
        // ※Time.deltaTimeはFixedUpdateで呼ぶとTime.fixedDeltaTimeと同値を返す
        var force = pidController.ComputeRequiredForce(transform.position, target.position, Time.deltaTime);
        rb.AddForce(force);
    }
}