Unityでのネタ的な投稿です。

地面の黒ラインに沿って走行するロボット（ライントレーサー）をUnity上で再現してみました。

黒ラインはカメラ画像から認識するようにします。
今回はカメラを照度センサに見立て、画像全体の輝度からロボットをラインに沿って走行するようにフィードバック制御します。

ロボットの実装

自律移動型ロボットの中でも基本的な構造となる独立二輪型を採用しました。

左右後部に独立して回転する車輪が2つ、筐体を三点接地で支えるためのキャスタからなる構造です。
前進・後進、旋回、その場旋回などの動きが可能です。

車輪はHingeJointで筐体にジョイント接続します。

ここで、HingeJointのUserMotorにチェックを入れます。
こうすることで、ジョイントはモータとして機能するようになります。

キャスタはFixedJointで筐体に固定させました。
したがって、走行すると地面と擦れます（笑）
地面との摩擦さえなければ走行には差し支えないので大丈夫でしょう。

照度センサの実装

地面の明暗を計測するための照度センサを実装します。
照度センサはCameraを用いて実装します。

カメラ画像の照度を取得するためには、画像のピクセルデータを取得する必要があり、TargetTextureにRenderTextureを指定します。
そして、このRenderTextureのピクセルデータから画像全体の輝度を計算するスクリプトを実装し、アタッチします。

IlluminanceSensor.cs

using UnityEngine;
using System.Collections;

/// <summary>
/// 照度センサ
/// </summary>
public class IlluminanceSensor : MonoBehaviour {
    /// <summary>
    /// センシング対象カメラ
    /// </summary>
    [SerializeField]
    private Camera dispCamera;

    /// <summary>
    /// 照度センサ値
    /// </summary>
    public float illuminance;

    /// <summary>
    /// センシング対象カメラのRenderTexture
    /// </summary>
    private Texture2D targetTexture;

    /// <summary>
    /// 照度センサの値を計測する
    /// </summary>
    /// <returns></returns>
    private IEnumerator Start() {
        var tex = dispCamera.targetTexture;
        targetTexture = new Texture2D(tex.width, tex.height, TextureFormat.ARGB32, false);

        while ( true ) {
            yield return new WaitForEndOfFrame();

            // RenderTextureキャプチャ
            RenderTexture.active = dispCamera.targetTexture;
            targetTexture.ReadPixels(new Rect(0, 0, tex.width, tex.height), 0, 0);
            targetTexture.Apply();

            // 照度を取得する
            illuminance = GetLightValue(targetTexture);
        }
    }

    /// <summary>
    /// 画像全体の照度計算
    /// </summary>
    /// <param name="tex"></param>
    /// <returns></returns>
    private float GetLightValue(Texture2D tex) {
        var cols = tex.GetPixels();

        // 平均色計算
        var avg = new Color(0, 0, 0);
        foreach ( var col in cols ) {
            avg += col;
        }
        avg /= cols.Length;

        // 照度計算
        return avg.grayscale;
    }
}

これで照度センサの完成です。

フィードバック制御の実装

ライントレースを行うには、照度センサの値を用いてラインのずれが0となる方向に旋回しながら走行する必要があります。
これは、ラインのずれ計算→旋回量計算→旋回量反映のループ処理を実装することで実現できます。
（今回は説明簡略化のため伝達関数やブロック図云々の話をすっ飛ばします・・・）

ラインのエッジをずれ0の位置とし、右にずれるほど正、左にずれるほど負の値となるようにずれを定義します。

　ラインのずれ = 閾値 – 照度センサ値

そして、ラインのずれを制御偏差(error)としてPID制御を行います。

　制御量 = KP * error + KI * errorの積分値 + KD * errorの微分値
　　KP：比例ゲイン
　　KI：積分ゲイン
　　KD：微分ゲイン

最後に、制御量を左右車輪の目標速度に加減して終了です。

　左車輪の目標速度 = 前進目標速度 – 制御量
　右車輪の目標速度 = 前進目標速度 + 制御量

上記の処理を行うフィードバック制御のスクリプトを実装し、アタッチします。

RobotControler.cs

using UnityEngine;
using System.Collections;

/// <summary>
/// ロボット制御
/// </summary>
public class RobotControler : MonoBehaviour {
    /// <summary>
    /// 左車輪
    /// </summary>
    [SerializeField]
    private HingeJoint wheelLeft;
    /// <summary>
    /// 右車輪
    /// </summary>
    [SerializeField]
    private HingeJoint wheelRight;
    /// <summary>
    /// 照度センサ
    /// </summary>
    [SerializeField]
    private IlluminanceSensor illumSensor;

    /// <summary>
    /// 目標速度
    /// </summary>
    [SerializeField]
    private float speed = 100;

    /// <summary>
    /// 照度センサ閾値
    /// </summary>
    [SerializeField]
    private float threshold = 0.5f;

    /// <summary>
    /// 比例ゲイン
    /// </summary>
    [SerializeField]
    private float kp = 1;
    /// <summary>
    /// 積分ゲイン
    /// </summary>
    [SerializeField]
    private float ki = 1;
    /// <summary>
    /// 微分ゲイン
    /// </summary>
    [SerializeField]
    private float kd = 1;

    /// <summary>
    /// 制御偏差の積分値
    /// </summary>
    private float errorInt = 0;

    /// <summary>
    /// 1フレーム前の制御偏差
    /// </summary>
    private float errorPrev = 0;

    /// <summary>
    /// 照度センサ値に基づくフィードバック制御
    /// </summary>
    private void Update() {
        // 制御偏差計算
        var error = threshold - illumSensor.illuminance;

        // 制御偏差の微積分値計算
        errorInt = (error + errorPrev) * Time.deltaTime / 2;
        var errorDiff = (error - errorPrev) / Time.deltaTime;

        // 制御量計算(PID制御)
        var output = kp * error + ki * errorInt + kd * errorDiff;

        // 車輪目標速度設定
        var motor = wheelLeft.motor;
        motor.targetVelocity = speed - output;
        wheelLeft.motor = motor;

        motor = wheelRight.motor;
        motor.targetVelocity = speed + output;
        wheelRight.motor = motor;

        errorPrev = error;
    }
}

実際に動かしてみる

走行速度・フィードバック制御のゲインは以下のように設定しました。
（何故かPIが大きい気がしますが、まあいいか・・・）
　speed = 500
　KP = 1500
　KI = 500000
　KD = 500

コースは以下としました。

以下、実際に走らせた動画です。

黒ラインの左エッジに沿って走行しています。
急カーブでブレてコースアウトしそうになってますが、そこそこの速度でライントレースしてくれるようになりました。

照度センサを増強してみた

照度センサ1個でもライントレースはできますが、環境光に左右されずにライントレースさせるために2個にしたパターンも試してみました。

以下、2個の照度センサでフィードバック制御を行うように改良したスクリプトです。

RobotControler2.cs

using UnityEngine;
using System.Collections;

/// <summary>
/// ロボット制御(2個の照度センサ使用版)
/// </summary>
public class RobotControler2 : MonoBehaviour {
    /// <summary>
    /// 左車輪
    /// </summary>
    [SerializeField]
    private HingeJoint wheelLeft;
    /// <summary>
    /// 右車輪
    /// </summary>
    [SerializeField]
    private HingeJoint wheelRight;
    /// <summary>
    /// 照度センサ(左)
    /// </summary>
    [SerializeField]
    private IlluminanceSensor illumSensorLeft;
    /// <summary>
    /// 照度センサ(右)
    /// </summary>
    [SerializeField]
    private IlluminanceSensor illumSensorRight;

    /// <summary>
    /// 目標速度
    /// </summary>
    [SerializeField]
    private float speed = 100;

    /// <summary>
    /// 比例ゲイン
    /// </summary>
    [SerializeField]
    private float kp = 1;
    /// <summary>
    /// 積分ゲイン
    /// </summary>
    [SerializeField]
    private float ki = 1;
    /// <summary>
    /// 微分ゲイン
    /// </summary>
    [SerializeField]
    private float kd = 1;

    /// <summary>
    /// 制御偏差の積分値
    /// </summary>
    private float errorInt = 0;

    /// <summary>
    /// 1フレーム前の制御偏差
    /// </summary>
    private float errorPrev = 0;

    /// <summary>
    /// 照度センサ値に基づくフィードバック制御
    /// </summary>
    private void Update() {
        // 制御偏差計算
        var error = illumSensorRight.illuminance - illumSensorLeft.illuminance;

        // 制御偏差の微積分値計算
        errorInt = (error + errorPrev) * Time.deltaTime / 2;
        var errorDiff = (error - errorPrev) / Time.deltaTime;

        // 制御量計算(PID制御)
        var output = kp * error + ki * errorInt + kd * errorDiff;

        // 車輪目標速度設定
        var motor = wheelLeft.motor;
        motor.targetVelocity = speed - output;
        wheelLeft.motor = motor;

        motor = wheelRight.motor;
        motor.targetVelocity = speed + output;
        wheelRight.motor = motor;

        errorPrev = error;
    }
}

2個の照度センサは左右前方に設置します。
両者は距離を少し空けます。

ラインのずれの計算は両者の照度の差分で求まります。
　ラインのずれ = 右の照度センサ – 左の照度センサ

以降のフィードバック制御の処理は照度センサ1個の時と変わりません。

走行速度・ゲインは以下の通り。

　speed = 1000
　KP = 3000
　KI = 500000
　KD = 1000

照度センサ1個の時よりも安定して高速でライントレースしてくれるようになりました。

おわりに

実物のライントレーサーを模倣した原理でライントレースを行っているので、フィードバック制御の学習の題材としての活用が期待できるかもしれません。
ただし、あくまでシミュレータなので参考程度にとどめておくべきかと思います。