鈴鹿高専アドベントカレンダー12日目の記事です。

データ検品用アルゴリズムの中でも、非常にベーシックな「スミルノフ・グラブス検定」を使ってデータ検品をし、それを基に自己位置推定するプログラムを書きました。

与太話をすると、このプログラムは鈴鹿高専の2023年度高専ロボコンで作ったプログラムですが、自動制御がなくなったため結局このプログラムは使いませんでした。その供養もかねてこの記事を書かせていただきます。あと多分外部に公開したところでなにも痛いことはないので、公開します。（実際あまり大したことのないプログラムですので、大目に見てもらえると助かります）

スミルノフ・グラブス検定とは

さらに精緻化した検定手法に「スミルノフ･グラブス検定」という手法があり、これは平均値から最も離れた観測値を選び、外れ値検定を行い、外れ値が見つかったら、これを除外して検定をやり直すということを続けていき、最終的には外れ値の無いデータセットを作ることも目標としています（ただし、スミルノフ･グラブス検定も正規分布を前提とした方法なので、正規分布からかけ離れた分布のデータに利用すると、観測値の殆どが外れ値になってしまうこともあるので注意してください）

記事を見てもらった方が早いと思うのですが、まずデータの平均値を求め、各データからその平均値を引いたものを不変分布で割って、各データのt値を求め、有意点が書いてある表（スミルノフ・グラブス検定の統計数値表があります。調べてみてください）などから、n%外れているものの判定をし、廃棄判断する、というものです。
といってもあまり統計の知識に自信がないため、ちゃんとした知識をお持ちの方は間違ってるところがあったらご指摘ください。

使用した自己位置推定器

使用した自己位置推定器は以下のようなものです。

分かりにくい図です

ロボットy軸平行な側の側面に4つレーザセンサが付いていて、ロボットのx軸平行な中心軸からそれぞれa(m)、b(m)、c(m)、d(m)離れています。
計算するときは見方を変えて、ロボの中心軸をY軸と置いて、レーザセンサが付けられている面に平行な方はx軸と置き、その交点Oを置きます。この図で言うと縦の線がY軸、横線がX軸、ロボの中心軸の矢印付近の交点が原点Oです。
緑の線はレーザの光線の向きです。

実際のプログラム

mbed OS 5を使って書いています。
以下の関数を書いて現在地の推定を行いました。このままで動くかどうかはわかりません（部内情報が漏れないように少々手を加えましたので、Typoがあったら動かないかもしれません）が、ご了承ください。プログラムの雰囲気だけでも...

/**
 **********************************************************************************************************************************
 * @fn float GetPosi
 * @brief 位置補正用センサで角度を取得する。補助情報としてJyroで取得した角度も参考にする
 * @param float _nowPosition[0] 現在地X
 * @param float _nowPosition[1] 現在地Y（情報がないので0にしておく）
 * @param float _nowPosition[2] 現在地Angle
 * @param float Data[4] 位置補正用センサのデータ
 *    _____________→
 *   |             0
 *   |      X      |
 *   |      ↑      1
 *   |       →Y    2
 *   |             |
 *   |_____________3
 *                 →
 * こうらしい (2023-09-28情報)
 * @attention Smirnov-Grubbsの棄却検定を用いて外れ値の判定を行っている。
 ************************************************************************************************************************************
 */
void GetPosi(float _nowPosition[3], float Data[4], float jyroAngle)
{
    const float LENGTH[6] = {b-a, c-a, d-a, c-b, d-b, d-c};  // 各センサ同士の距離
    float angle[6];
    float angleSum;
    float angleAverage;
    float angleStandardDeviation;               // 角度のデータの標準偏差
    const float angleSmirnovGrubbs = 1.729f;    // スミルノフ・グラブスにおける要素数6の10%の外れ値
    float trustAngle[6];
    float trustpreAngle[6];
    int trustAngleDataNum = 0;
    float trustAngleDataSum = 0;

    const float DISTANCE_TO_CENTER[4] = {a, b, c, d};    //中心との距離、中心を原点Oとおいて左側が距離の減少方向、右側が距離の増加方向
    float distance[4];
    float distanceSum;
    float distanceAverage;
    float distanceStandardDeviation;                    // 距離のデータの標準偏差
    const float distanceSmirnoGrubbs = 1.425f;          // スミルノフ・グラブスにおける要素数4の10%の外れ値
    float trustDistance[4];
    float trustpreDistance[4];
    int trustDistDataNum = 0;
    float trustDistDataSum = 0;

    _nowPosition[3] = {0};     //初期化しとく
    /**
     * 
     * Angleの処理
     * 
     * 
    */
    angle[0] = atanf((Data[0] - Data[1]) / LENGTH[0]);
    angle[1] = atanf((Data[0] - Data[2]) / LENGTH[1]);
    angle[2] = atanf((Data[0] - Data[3]) / LENGTH[2]);
    angle[3] = atanf((Data[1] - Data[2]) / LENGTH[3]);
    angle[4] = atanf((Data[1] - Data[3]) / LENGTH[4]);
    angle[5] = atanf((Data[2] - Data[3]) / LENGTH[5]);

    //pc.printf("angle1 %f %f %f %f %f %f\r\n", angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]);

    for(int i = 0; i < 6; i++){
        angleSum += angle[i];
    }
    
    angleAverage = angleSum / 6.0f;
    angleStandardDeviation = calcStandardDeviation(angle, 6);
    //pc.printf("ave:%f\r\n", angleAverage);

    for(int i = 0; i < 6; i++)
    {
        float testStatistic = fabs(angle[i] - angleAverage) / angleStandardDeviation;
        if(testStatistic > angleSmirnovGrubbs)
        {
            // ジャイロの値を信用することとする
            trustAngle[i] = 0.0f;
        }
        else 
        {
            trustAngle[i] = angle[i];
            trustAngleDataNum++;
        }
    }
    //pc.printf("trustangle %f %f %f %f %f %f\r\n", trustAngle[0], trustAngle[1], trustAngle[2], trustAngle[3], trustAngle[4], trustAngle[5]);
    for(int i = 0; i < 6; i++)
    {
        trustAngleDataSum += trustAngle[i];
    }
    //pc.printf("trustnum %d\r\n", trustAngleDataNum);

    _nowPosition[2] = trustAngleDataSum / (float)trustAngleDataNum;

    for(int i = 0; i < 4; i++)
    {
        distance[i] = ROBOT_RADIUS + Data[i] * cosf(_nowPosition[2]) + DISTANCE_TO_CENTER[i] * sinf(_nowPosition[2]);
    }
    for(int i = 0; i < 4; i++){
        distanceSum += distance[i];
    }
    //pc.printf("dist1 %f %f %f %f\r\n", distance[0], distance[1], distance[2], distance[3]);     //このprintfは消すな！！！

    distanceAverage = distanceSum / 4.0f;
    distanceStandardDeviation = calcStandardDeviation(distance, 4);

    for(int i = 0; i < 4; i++)
    {
        float testStatistic = fabs(distance[i] - distanceAverage) / distanceStandardDeviation;
        if(testStatistic > distanceSmirnoGrubbs)
        {
            // 信頼しない値として0を代入する
            trustDistance[i] = 0.0f;
        }
        else 
        {
            trustDistance[i] = distance[i];
            trustDistDataNum++;
        }
    }
    //pc.printf("trustnum %d trustdist %f %f %f %f\r\n", trustDistDataNum, trustDistance[0], trustDistance[1], trustDistance[2], trustDistance[3]);

    for(int i = 0; i < 4; i++)
    {
        trustDistDataSum += trustDistance[i];
    }
    //pc.printf("sum %f\r\n",trustDistDataSum);
    _nowPosition[0] = trustDistDataSum / (float)trustDistDataNum;
    _nowPosition[1] = 0.0f;

}

ここでは壁との角度と壁までの距離を取得するプログラムを書いています。

85行目から90行目で、各センサが取得した壁までの距離のデータからarctan（アークタンジェント）をとって角度に直し、そのデータを検品しています。

94行目から96行目でそれの和を求めて、98行目でその平均を求めています。また、99行目はcalcStandardDeviation()という関数を使っていますが、これは標準偏差を求める関数です。調べれば簡単に標準偏差のアルゴリズムは出てきて簡単に作れると思うので、ここでは省略します。angleStandardDeviation変数に標準偏差を格納します。

102行目からの

  for(int i = 0; i < 6; i++)
    {
        float testStatistic = fabs(angle[i] - angleAverage) / angleStandardDeviation;
        if(testStatistic > angleSmirnovGrubbs)
        {
            // ジャイロの値を信用することとする
            trustAngle[i] = 0.0f;
        }
        else 
        {
            trustAngle[i] = angle[i];
            trustAngleDataNum++;
        }
    }

はt値を求める計算です。各データから平均値を引いて、偏差を割っています。求まったt値が有意点より高ければジャイロの値を用いて計算し、有意点に収まっているなら信用できる値として扱います。

125行目からは距離を求める計算ですが、流れは角度を求めるときのアルゴリズムの流れと同じです。

実際に動かしてみた結果

写真に残してないのですが、結構いい感じにデータ検品されていました。レーザが障害物が当たっているところの値ははじかれていました。

最後に

センサの値というのは外乱などによってすぐに変化します。また、センサの特性や物によってはすぐノイズが入ったりチラチラと値が変わっていきます。
これらはセンサの値段や性能、またそもそものセンサの特性によって大きく変わります。
これらの外乱やノイズを抑えるためにはフィルタリング技術、データ検品技術が重要になってきます。（ロボティクスでよく使われているようなものは拡張カルマンフィルタやローパス、ハイパスフィルタなどがあげられます）
いかに信用できる値を得られるか、ということに注力するために、今回はデータ検品という技術を用いて信用できる値を得ました。
ぜひご参考までにお送りいたします。

また上のプログラム、または説明に関してミスがあった場合、訂正のコメントをよろしくお願いします。