壁距離センサと前壁を使って車体位置補正 – HM-StarterKit初級者 Part 6 –

こんにちわ、青木です。

前回の物理的な車体位置補正をすることで完走率があがります。ただし、欠点として探索経路にほとんど袋小路がない迷路では、補正することができません。ここでは、その問題点を解決すべく、袋小路ではなく、前壁がある時に壁距離センサの値を使った車体位置補正について紹介します。

前壁での車体位置補正の考え方
補正の方法と補正の範囲
プログラミング
実際に補正を確認した動画
お知らせ

前壁での車体位置補正の考え方

前壁での車体位置補正では、前センサの値を使って、目標の位置に対してどのぐらい差分があるか把握し、その差分をソフトウェア上で管理している位置に対して補正する方法です。簡単にいうと、前壁の距離をセンサで測定し、目標としていたセンサ値より小さかったら予定していた距離より少し余分に進み、目標としていたセンサ値より大きかったら予定していた距離を少し短くするという考え方です。しかし、HM-StarterKit初級者Part4にあるように迷路の場所によって明るさが違うため、壁との距離を正確に把握することはできません。

ロボットは理論値で計算した通りの距離や位置になることはほとんどないので、どこかで補正をすることがあります。しかし、その区画に対して正確に位置や距離を補正するのは大変なので、もっと緩やかな目線で、スタートからゴールまでの経路でスタックしないで完走ができるような少し曖昧な補正をすることにします。これなら前壁との距離を正確に知る必要はなく、目標としている前壁との距離に対して遠い、近いといった大雑把な情報で良いことになります。

補正の方法と補正の範囲

具体的にどのような補正をするかというと、予め1区画の走行が終わる位置(壁の有無を取得する位置)での前壁センサの値を取得しておきます(下図の位置)。この値を”前壁の閾値1″とします。

1区画の走行が終わる付近で、前壁があることがわかった時、先程取得した前壁の閾値1を超えたらその時点で今の走行を終了し、次のステップに進みます。逆に、センサの値が小さい時は、少し様子を伺いながら進み、予定した距離より5mm多く進んだ時点で今の走行を終了します。5mmにした理由は、1区画=90mmの5%が4.5mmなので、このぐらいの値なら、間違って補正しても走行にあまり影響がでない距離だと判断したからです。また、迷路の場所によって壁との距離が同じでも前壁センサの値は、違う値が返ってきますが、反射物との距離が近くなるとセンサの値が急激に値が大きくなります。前壁の閾値1を超えそうな位置にある場合は、1mm程度の誤差範囲になるのではないかと思います。しかし、前壁から遠いと位置では、少し遠いだけでも前壁センサの値が急激に小さくなり誤差が大きくなるので補正距離を大きくするほど危険度が増します。

プログラミング

run.cファイルにあるstraight関数に補正する処理を追加します。straight関数は、走行終了時の速度が0かそれ以外で処理を切り替えてます。今回、探索で90mm進むところに補正を入れるので、終了速度が0ではないところに追加します。参考までに、走行終了時の速度が0の時は、袋小路や超信地旋回に入る前の、減速する時に使用しています。1つ目のwhileは探索の時、tar_speedとend_speedが同じなので、((len_target-10) – len_mouse) > 0が成立している間は、1つ目のwhile文でループしていることになります。予定の走行距離-10mmになったら次のwhile文に移行します。2つ目のwhile文に補正する処理を追加します。68行目に+5があります。これは、前壁があって、前壁の閾値1に足りない時、余分に進んでも良い距離になります。+5は先程の補正量5mmの距離です。4×4の迷路では+10でも問題なく動作しました。69行目は、前壁がない時予定した距離になったらwhileループを抜ける文になります。前壁がない時この処理がないと、68行目で追加した距離まで進むことになります。例えば18区画直線の時、18区画x補正量5=90mm余分に進んでしまい1区画座標を間違えてしまいます。72行目は、前壁センサの値が前壁の閾値1を超え、さらに予定して距離-5mm以下になったらwhileループを抜けるようにしています。RIGHT_90とLEFT_90は、前壁の閾値1の値であり、parameter.hに#defineで定義してあります。

//減速処理を始めるべき位置まで加速、定速区間を続行
while( ((len_target-10) - len_mouse) > 1000.0*((float)(tar_speed * tar_speed) - (float)(end_speed * end_speed))/(float)(2.0*accel));

//減速処理開始
accel = -acc;					//減速するために加速度を負の値にする	
while(len_mouse < (len_target+5)){		//停止したい距離の少し手前まで継続
    if((sen_fr.is_wall==false) && (sen_fl.is_wall==false) && (len_mouse > len_target)){//前壁なし
        break;
    }
    if( (sen_fr.value > RIGHT_90) && (sen_fl.value > LEFT_90) && ( len_mouse > (len_target -5)) ) {//目標の位置の5mm手前
        break;
    }
//一定速度まで減速したら最低駆動トルクで走行
    if(tar_speed <= end_speed){	//目標速度が最低速度になったら、加速度を0にする
        accel = 0;
      //tar_speed = end_speed;
    }
}