かみやんの技術者ブログ

主にプログラムの話です

オドメトリログの可視化

前回のエントリで、モータドライバの過電流保護でガックンガックンと停止する問題だが、とりあえず急加速、急停止をしないようにマイコンのプログラムを改良したら、うまくいった。これはあっさり。

次に、オドメトリログの可視化をしたいなと書いたが、実装してみた。
左右のロータリーエンコーダのカウンタ値がマイコンからPCへ送られるようになっていて、それをファイルへ落としている。カウンタ値からXY座標への変換を行い、それを描画する。

最初に実装したロジックのソースが下記。

//左右のカウントからXY位置、方向へ変換
public void ConvertToPosition()
{
    int bakL = 0, bakR = 0;
    double bakDir = Math.PI/2.0;//右にX軸、上にY軸、スタートは上向きとする
    double bakX = 0.0, bakY = 0.0;
    foreach (Odometry od in List) {
        int vL = od.Left - bakL;
        int vR = od.Right - bakR;
        double theta = (vR - vL) * MM_PER_COUNT / ENCODER_DISTANCE;
        od.Direction = bakDir + theta;
        double front = (vL + vR) / 2 * MM_PER_COUNT;
        od.X = bakX + front * Math.Cos(od.Direction);
        od.Y = bakY + front * Math.Sin(od.Direction);
        bakL = od.Left;
        bakR = od.Right;
        bakX = od.X;
        bakY = od.Y;
        bakDir = od.Direction;
    }
}

併進成分は、左右輪の進んだ距離の平均とした。回転成分は、左右輪の進んだ距離の差÷左右輪の距離(単位はラジアン)とした。
これで実装して、部屋の中を机を避けるようにS字に走ってみたら

こんな感じ。これは、全然あってない。。曲がる角度が多めに判断されている。併進成分の計算が手抜き(旋回中の併進成分を直線で扱っている)過ぎたかなと思って、厳密な計算にしてみたのが下記、

//左右のカウントからXY位置、方向へ変換
public void ConvertToPosition()
{
    int bakL = 0, bakR = 0;
    double bakDir = Math.PI/2.0;//右にX軸、上にY軸、スタートは上向きとする
    double bakX = 0.0, bakY = 0.0;
    foreach (Odometry od in List) {
        int vL = od.Left - bakL;
        int vR = od.Right - bakR;
        double theta = (vR - vL) * MM_PER_COUNT / ENCODER_DISTANCE;
        od.Direction = bakDir + theta;
        if (Math.Abs(theta) > 0.0001) {//旋回しているとき
            double r = Math.Min(vL, vR) * MM_PER_COUNT / theta + ENCODER_DISTANCE / 2.0 * Math.Sign(vR - vL);//旋回半径
            double dx = -r * (1.0 - Math.Cos(theta));
            double dy = Math.Abs(r * Math.Sin(theta));
            double dir = bakDir - Math.PI / 2.0;
            double dx2 = dx * Math.Cos(dir) - dy * Math.Sin(dir);
            double dy2 = dx * Math.Sin(dir) + dy * Math.Cos(dir);
            od.X = bakX + dx2;
            od.Y = bakY + dy2;
        } else {//直進のとき
            double front = (vL + vR) / 2 * MM_PER_COUNT;
            od.X = bakX + front * Math.Cos(od.Direction);
            od.Y = bakY + front * Math.Sin(od.Direction);
        }
        bakL = od.Left;
        bakR = od.Right;
        bakX = od.X;
        bakY = od.Y;
        bakDir = od.Direction;
    }
}

これは、左右輪の進んだ距離が違うとき(回転成分があるとき)の、併進成分を旋回半径と旋回角から弧の距離になるように計算している。
結果、

全然変わらなかった。
なので計算は間違っていないと思われる。
次に、直線でも誤差がでているのか計測してみた。ロボットに1800mmマニュアル操作で走らせてみて、オドメトリログでの認識距離をみてみたところ、1469mmとなった。80%ぐらい実際より少なく認識されている。
ということで、強引に、オドメトリログからXY変換のときに80%で割ってみたところ、

こんな感じ。うむ、走った経路としては、こんな感じのところを走った。

う〜む、この80%の誤差が何たるか。よく考えないといけない。。