DATE: --/--/--(--)   CATEGORY: スポンサー広告
スポンサーサイト
上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
page top
DATE: 2018/09/11(火)   CATEGORY: Nucleoマウス
Nucleo-32boardを使ったクラシックマウスの開発その21 ~壁制御~


今回は壁制御のお話しです。
壁制御はマウスの安定感に直結します。
壁制御ができればある程度の迷路は走れます。

壁制御の概要はMice Wikiをご覧ください。

壁制御については初代ステッパマウスの「うむ夫。」
のときから苦しんできました。
初年度の記事はこちら。

うむ夫。探索編(3) ~壁制御編~

今見ると現在実装している壁制御とは異なる部分もありますが、
基本は上に書いてある通りです。

それでは今回は過去の自分と変わった点を中心に書いていきます。
探索で使っている壁制御についてのポイントです。
→最短走行時などの難しい話は知りません


・壁センサ値の取り方(差分取得)

壁センサの値は制御周期毎に取得します。

これはよく聞く方法でLEDを点灯させているときと
消灯しているときの値の差分を取ります。

こうすると外乱の影響を減らすことができます。

これについてはDMAの割り込み関数を利用します。
→DMAの扱い方については以前の記事を参照

Nucleo-32boardを使ったクラシックマウスの開発その16 ~ADC~


実装の流れとしては、

①回目の割り込み
→LED消灯時の値を取る
→左右方向のLEDを点灯
②回目の割り込み
→左右方向の値を取る
→左右方向のLEDを消灯
→前方向のLEDを点灯
③回目の割り込み
→前方向の値を取る
→前方向のLEDを消灯
→とった値でセンサ値を計算

です。
イメージはできたのでこんな感じで実装しました。


void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc) {
getWallSensorValue(&g_ADCCount);
}

void getWallSensorValue(uint8_t *count) {
volatile uint16_t i;
switch (*count) {
case OFFVALUE:

HAL_ADC_Stop_DMA(&hadc1);
g_offWallSensorValue.frontLeft = g_ADCBuffer[FRONTLEFT];
g_offWallSensorValue.frontRight = g_ADCBuffer[FRONTRIGHT];
g_offWallSensorValue.left = g_ADCBuffer[LEFT];
g_offWallSensorValue.right = g_ADCBuffer[RIGHT];

*count = SIDEVALUE;
if (g_sensorLEDFlag == 1) {
sensorSideLED(HIGH);
for (i = 0; i < SENSORLEDWAITCOUNT; i++) {
}
}
HAL_ADC_Start_DMA(&hadc1, g_ADCBuffer, ADLENGTH);

break;

case SIDEVALUE:

sensorSideLED(LOW);
HAL_ADC_Stop_DMA(&hadc1);
g_onWallSensorValue.left = g_ADCBuffer[LEFT];
g_onWallSensorValue.right = g_ADCBuffer[RIGHT];

*count = FRONTVALUE;
if (g_sensorLEDFlag == 1) {
sensorFrontLED(HIGH);
for (i = 0; i < SENSORLEDWAITCOUNT; i++) {
}
}
HAL_ADC_Start_DMA(&hadc1, g_ADCBuffer, ADLENGTH);

break;

case FRONTVALUE:

sensorFrontLED(LOW);
HAL_ADC_Stop_DMA(&hadc1);
g_onWallSensorValue.frontLeft = g_ADCBuffer[FRONTLEFT];
g_onWallSensorValue.frontRight = g_ADCBuffer[FRONTRIGHT];

*count = END;

g_nowWallSensorValue.frontLeft = g_onWallSensorValue.frontLeft
- g_offWallSensorValue.frontLeft;
g_nowWallSensorValue.frontRight = g_onWallSensorValue.frontRight
- g_offWallSensorValue.frontRight;
g_nowWallSensorValue.left = g_onWallSensorValue.left
- g_offWallSensorValue.left;
g_nowWallSensorValue.right = g_onWallSensorValue.right
- g_offWallSensorValue.right;

break;

default:
break;
}
}



迷路を走らせて壁がなくなるときのセンサ値のログをとりました。
→ぼちぼちですかね?

壁生値


・壁センサ値の取り方(移動平均)

壁センサの値は回路によって、
ノイズが発生してしまうことがあります。
自分も過去に苦労しました。

その対策として壁センサの値を移動平均をとって使用しています。
移動平均をとることにより、細かな値の変動を抑えることができます。


移動平均を使う上で重要になってくるのはサンプル数です。
移動平均のサンプル数は機体毎に決めればいいと思います。

3 ms移動平均

しかし、サンプル数を多くとりすぎると
遅延時間の影響も大きくなってしまいます。
各センサ値のログを取ってみて決めましょう。

移動平均による遅延の影響

探索で用いる場合には、3~5個がいいと思います。
→多くても10。それでも改善しないなら回路を見直すべき


・変化量の取り方

変化量は吸い込まれ対策として重要です。
この変化量の取り方にも注意が必要です。

単純に思いつく変化量の取り方は、
今の値-過去の値
で求める方法です。
これを1 msでとった場合がこちらです。

変化量

ひどいです。
正直言って使い物になりません。
一番変化量を検知したい160~220 msの部分すら怪しいです。
これでは、ノイズの影響なのか壁が無くなっている影響なのか、
それとも機体が壁から離れようとしている影響なのか、
どういった原因で変化量が生じているのか判断が難しいです。
→吸い込まれ対策として壁制御を切る判断が難しい

きちんと吸い込まれ対策をするには変化量をうまく取り出さなければいけません。

この変化量をうまく取り出す方法の一つとして、
前に説明した移動平均が役に立ちます。

下に示すのは壁センサ値を移動平均を3 msで用いた場合の差分データです。

移動平均後の変化量

ノイズの影響が減っていることがわかります。
壁センサ値の移動移均をとることは、変化量のノイズを減らす効果もあることがわかります。

これは移動平均の性質から考察できます。
移動平均データ同士の差分をとった場合に出てくる値は、

\[\begin{align}\overline{val_0}-\overline{val_1}&=\frac{val_0+…+val_{n-1}}{n}-\frac{val_1+…+val_n}{n}\\ &= \frac{val_0-val_n}{n}\end{align}\]
です。
結局、移動平均同士の差分を計算すると
より長い時間間隔で差分をとっている
ことになります。
このことから、
変化量をとる場合には
長い時間間隔で差分をとると区別がつきやすい
ということがわかりますね。

この性質を踏まえて、
遅延時間が気にならない程度に時間間隔をあけて差分をとり、
自分で扱いやすいと思えるよう変化量を算出しました。

→処理内容を書くと長くなるので割愛
→1/msに換算したら7 ms分くらいの変化量な気がしました(真偽は不明)

なんだかんだで求めた変化量

とてもわかりやすくなりました。
センサ値との対応を見てみます。

壁センサ値と変化量の対応

壁が無くなっていく160 ms付近から変化量が発生し、
柱を通り過ぎた後に変化量が急激に大きくなっていることがわかります。
これなら、微妙なノイズと壁の切れ目との区別がつけられます。

最後にどれだけ改善したかを示すために、
生値だけのものを示しておきます。

比較用生値


・制御量の取り方

以前は制御量の算出には比例項のみしか入れていませんでしたが、
微分項も加えてPD制御にしてみました。
こちらを参考にさせてもらいました。

[rogy Advent Calendar 2015] マイクロマウスと制御理論 (1)壁トレース制御 -Tokoro's Tech-Note


・制御量の入れ方

制御量の入れ方です。
前回に加えてMice Wikiに従うと、

\[v_R=v+\omega \frac{W}{2}+(制御量)\]
\[v_L=v-\omega \frac{W}{2}-(制御量)\]

となります。
ステッパなら別にこれでもいいのですが、
壁制御の性質を考えてみると
これって左右に速度差を発生させる点では角速度の目標値を設定するのと同じですね。
なので、角速度項と壁制御項を合わせて以下のようにしました。
→DCマウスで学んだ

\[v_R=v+\omega '\frac{W}{2}\]
\[v_L=v-\omega '\frac{W}{2}\]
\[\omega '=\omega_{tar}+\sum{wall_i}\]

ここで合えて総和として表しているのは、
今回触れた探索用の壁制御の他にも、
櫛対策で入れる壁制御や
斜め走行時での壁制御での制御量もω'に加えているからです。

ω'内の各項については、使わないものは0としておけば大丈夫です。
例)
直進動作→ω&斜め制御=0、探索用壁制御&櫛制御ON,
旋回動作→ω=目標値、壁制御=0



こんな具合で壁制御を導入してみました。
過去記事でもありましたが、
実際に壁制御を運用するにあたり調整しなければいけないパラメータがあります。
そのへんは調整方法の方に書こうかなと思います。
→疲れた


スポンサーサイト

コメントの投稿

 管理者にだけ表示を許可する
Copyright © うむ夫の歩み. all rights reserved.
上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。