« 2009年10月 | トップページ | 2009年12月 »

2009年11月30日 (月)

ファームにバグ発見

今年度のつくばチャレンジは終わりましたが、まだ12月24~26日にSI2009の発表が残っているので、開発が間に合わなかった分を少しずつ作っています。

今日は台車を制御しているファームウェアにちょっとした機能を追加しましたが、その際にバグを見つけました。Windows側の時間管理があまり信用できないのでファームからロータリーエンコーダのカウント数を取得する際に、ファーム側にミリ秒単位のタイムスタンプをつけるようにしました。そしてPCでそのタイムスタンプ付きのロータリーエンコーダカウント数を取得させてみたところ、本来ならばタイムスタンプは100ms毎になっているはずなのに、きれいに105ms毎になっていました。

ファームではSH7125のCMT0のコンペアマッチ割り込みで 1ms 単位の時間管理をしているのですが、これが正確に 1ms になっていなかったようです。理由は、CMT0の初期化を行う部分のソースコードを見たらすぐにわかりました。周辺クロックを20MHzで計算してました。

このSH7125のCMT0初期化部分のソースコードは、以前アルファプロジェクトのSTK-7125用に書いたものをそのまま使いまわしたのですが、今回使っているCPUボードは秋月電子のAKI-7125なので、クロック周波数が少し違います。周辺回路のクロックはSTK-7125なら20MHzでしたが、AKI-7125では25MHzになります。なので、コンペアマッチを作るためのカウンタの値が違います。

参考までにCMT0の初期化部分のソースコードをUpします。


/**
	CMT0を 1ms のインターバルタイマとして使用するための初期化
*/
void CMT0_init(void)
{
	// 周辺回路動作周波数がφMHz、プリスケーラ P, のとき、
	// 時間 t を作るのに必要なカウント値 tcnt は
	//		tcnt = (t ×φ×10^6 ÷ P) - 1
	// である。
	// tcnt が出来る限り大きな値になるよう、プリスケーラを
	// 選択すること。
	
	// 周辺回路クロック		20MHz
	// プリスケーラ		8
	// 作りたい時間		10-^3 sec
	unsigned short tcnt = ((20 * 1000)/8) - 1;
	
	CMT0.CMCSR.BIT.CKS  = 0;		// プリスケーラ(0:φ/8)
	CMT0.CMCOR 		= tcnt;		// 1msタイマーSET
	CMT0.CMCSR.BIT.CMF	= 0;		// コンペアマッチフラグクリア
	CMT0.CMCSR.BIT.CMIE = 1;		// コンペアマッチ割り込みを許可
	
	
	INTC.IPRJ.BIT._CMT0 = 8;		// 割り込み優先度
	
	CMT.CMSTR.BIT.STR0  = 1;		// CMCNT0カウントスタート	
	
}

直すべき部分は、すぐわかりますね。

2009年11月25日 (水)

2009年度トライアルコースのWay Point設定を検証する

去年と同様に今年もスタート地点を出た後でロボットが左方向に進み、つくばエキスポセンターのゲートにぶつかってしまいました。去年はWay Pointの設定を Google Earth上で行いました。しかし、Google Earthの航空写真と地図が大きくずれていることに気付き、今年は Google Map上でWay Pointを設定しました。しかし、結果は同じ。

Way Pointは下図のように設定しました。

Challenge_2009_trial_couse_waypoi_3

じゃあGoogle Mapもずれているのか? と思ったので、ゼンリンの電子地図帳を使ってスタート地点の座標をGoogle Mapと比較してみましたが同じ座標です。当然ですね、なにせ、日本のGoogle Mapはゼンリンの地図を使っているのですから。

トライアルでエキスポセンターの入り口を出られなかったロボットの多くが、私のロボットと同じように左に曲がって走行していました。GPS受信機の測位精度の問題ならば、もっと左右にバラついてもよいのではないかと思います。う~ん、謎だ。

2009年11月22日 (日)

つくばチャレンジ2009 終了~

11/20, 21で今年度のチャレンジも終わりました。うちのチーム(私ひとり)の結果は、今年もトライアルを突破できず。去年とまったく同じように、つくばエキスポセンターの入口ゲートに突進していきました。

自律走行のソースコードを何とか書いたのは11/19の夜、宿について翌朝までの間だったので、よく書いたといえばよく書いたのですが、う~ん、まったくいいところ無しでした。

今年の写真も例年通り、Picasaにアップしましたので、雰囲気を味わいたい方は写真を見てください。

大会自体は終わりましたが、12月のSI2009とレポートがまだ残っているので、大会までに未完成だったところを実装して、定量的な評価まで行って今年度のチャレンジを終わる予定です。

2009年11月12日 (木)

もっと正確に処理周期を管理するために

先日の日記でWindowsのプログラムの制御周期がばらつきすぎると書きましたが、改善するための手段を考えてみました。

昔からある定石では、正確に時間を刻める外部のデバイスから割り込みをかけるという手段があります。この手段として使えそうなデバイスが2つあって、ひとつはTop-URG、もうひとつは台車のCPUです。Top-URGのほうは1スキャンあたり25msなので、Top-URGの出力メッセージを受信するタイミングでうまいこと時間を刻めるのではないかと思います。台車のCPUはファームを自分で書いていますからUART出力のタイミングは好きなようにできます。Windows側はSerialPortで受信したタイミングでデータを取り込めば、(所詮はイベント処理なのでシリアル通信受信イベントが後回しにされる惧れはありますが)多少はましになるのではないかと目論んでいます。

ということで、GPSのGPRMCセンテンスから移動軌跡(Course)を取り出す処理は既に追加したので、台車のファームに少し手を入れて、今夜中に再度データ収集を行って、カルマンフィルタのチューニングに片をつけようと思います。そしたら、明日と明後日でway point巡回走行を作れるだけ作って、11/15の試走会でなんとか自律走行できるはず。厳しいな。

2009年11月11日 (水)

チューニング、チューニング、チューニング

モンテカルロ法でカルマンフィルタのチューニングを行うツールを作り、PCのパワーに任せて力づくでチューニングしています。しかし、まずいことがいろいろわかってきました。

まず、UTM座標系におけるロボットの角度を東を0[rad]と定義し、反時計まわりを正としたのですが、ジャイロもデジタルコンパスも出力は時計回りが正というのに気付かずかなり時間を消費しました。

つづいて、いくらチューニングをしてもきれいな軌跡を描いてくれないので、なんでだろうといろいろ探っていたら、走行開始時のロボットの進行方向がデータ収集をしたときと違う向きになっていることに気付きました。なんでだろうと思ってセンサの生データを見てみると、すでにセンサデータで方向が41°くらいずれています。もしや、デジタルコンパスの付近を通る配線に大きな電流が流れて磁気がみだれたのか、と思いましたがそれはちょっと納得できません。他にはコース上の走行開始位置が地磁気の乱れている場所なのではないかという疑問がわいたので、雨の中コンパスを持ち、傘をさして調べてきました。場所は、私が住んでいるアパートの駐車場です。ん~、車のフロントバンパ付近はすごく強い磁石があるようですね。セルモータとかダイナモとかなのでしょうか。持って行ったコンパスがとんでもない向きになったので、これが原因だと思います。

走行開始時に向きが違っているデータを使ってチューニングをするのはかなり無理があります。その向きを修正するのにGPSのNMEAセンテンスからAzimathを取得するようプログラムを書いたつもりだったのに、これも抜けていました。

こんなレベルなので、まだ自律走行できないんですよ。ああ、時間が無い。

2009年11月 9日 (月)

カルマンフィルタに不連続なセンサデータを与えてはいけない

11/7(土)の試走会参加を見送り、11/8(日)のTANTOスキークラブの秋のパーティも欠席して、ロボットの自己位置推定のためのカルマンフィルタチューニングに励んでおります。

チューニングの対象はプロセスノイズの共分散なわけですが、チューニングの前にカルマンフィルタのモデルを正しく作れているか、計測ノイズの共分散が妥当な値になっているか、カルマンフィルタに与えるセンサデータを(生のセンサデータから)正しく生成できているか確認するのに手間取りました。

なかでも一番苦戦したのがロボットの進行方向です。デジタルコンパスの値は0~360度(磁北を0度とする)なのですが、最初はロボットが磁北を向いたときに359.9度と0.0度の間で値が不連続になることに気付かず、カルマンフィルタの出力がでたらめになってしまいました。よく考えてみれば、カルマンフィルタは状態が連続的に変化することを前提としているので、値が突然360度も飛んでしまえばイノベーション(計測値に対する観測と予測の差)が大きくなってしまい、計測値の尤もらしさが急激に損なわれます。

それではこまるので、こんな感じで値の範囲を -2nπ~2nπに拡張してみました。

            // a は東向きを 0 とする。(コンパスの出力は磁北が 0[deg])
            // a の範囲を 0~360 [deg] から -PI~PI [rad] にする。
            double temp_deg_a = currentRawData.compass + 90.0;
            if (temp_deg_a > 180)
                temp_deg_a -= 360;
            else if (temp_deg_a < -180)
                temp_deg_a += 360;

            data.a = GeoProjection.deg2rad(temp_deg_a);

            // a が不連続になるとカルマンフィルタに使用できないので、
            // a の範囲を -2nPI ~ 2nPI に拡張する。
            double last_a_deg = lastRawData.compass + 90.0;
            if (last_a_deg > 180)
                last_a_deg -= 360;
            else if (last_a_deg < -180)
                last_a_deg += 360;

            double last_a = GeoProjection.deg2rad(last_a_deg);

            if ((last_a - data.a) > (Math.PI * 1.8))
                n++;
            else if ((last_a - data.a) < (-Math.PI * 1.8))
                n--;

            data.a += 2.0 * n * Math.PI;

これで長方形コースを6周したデータから進行方向 a を求めると、a = 40.11 [rad]くらいになりました。 [deg]に換算すると 4.011 / 3.14 * 180 = 2299.29 [deg] であり、 これを360で割ると 6.3 くらいになるので、カルマンフィルタに与えるセンサデータとして妥当なものになったと思います。

ちなみに、カルマンに食わせるときは地磁気偏角も入れてますよ。

2009年11月 7日 (土)

バッテリの持ち具合

来る11/20, 21のつくばチャレンジに向けたロボット開発は進捗がかなり危機的状況ですが、あきらめずに粘っています。今日はカルマンフィルタのチューニングのために外でロボットを走らせていて、バッテリが意外にもつことがわかりました。

うちのロボットはマイコンおよびセンサ用に7.2V×1本と、モータ駆動とロボットに搭載する小型PCとレーザーレンジセンサ用に7.2V×3本直列というバッテリ構成にしています。前者は元々数百ミリアンペア程度の電流しか必要ないので、2200mAhのバッテリを使えば電源ONのまま3時間くらい放置しても問題ありませんし、心配していません。後者はPCと走行用モータの両方とも消費電力が大きいのでちょっと心配していました。

今日は2200mAhのバッテリを十分充電しないまま5分間×2回程度走行したのですが、フル充電で21.4Vのバッテリが15V台まで落ちてきてもPCに12Vの電源を供給したまま走行することができました。ステアリングを制御するRCサーボモータのトルクが細くなってきて、さらに走行速度が落ちてきているのがはっきりわかりましたが、まだ走ってた。

ニッケル水素電池の放電終止電圧は1セルあたり1.0Vなので、3本直列(=18セル)で15Vまで下がっていたら1セルあたり0.83Vということで、放電終止電圧を下回っています。こういう使い方をすると、きっと電池の寿命が縮むんでしょうね。

2009年11月 5日 (木)

カルマンフィルタのチューニング

ロボットの自己位置推定を行うためのカルマンフィルタを実装して、11/3はフィルタのチューニングに必要なデータ収集を行いました。そして今日はそのデータの解析を行いました。

まず、ジャイロ(STL Japan 司21)は静止状態で5分間×4回計測した結果をみるとADコンバータの1~2ビット程度のバラつきが出る程度でバイアスドリフトを起こしていません。20分もあれば1km走行するには十分なので、その間バイアスドリフトを心配する必要が無いことがわかりました。ちなみに、ジャイロの計測ノイズの分散は、角速度を[rad/s]で表した場合で 1.81×10^-5(10の-5乗の意味)程度でした。

続いてGPS単体測位で得られた緯度経度をUTM座標系に直してXY座標(単位はm)で分散を計算してみたところ、静止状態で5分間×4回を通算してX座標の分散が0.5078、Y座標の分散は0.7406でした。もう少し直感的に分かりやすく表現すると、(Y座標の標準偏差が0.86なので)半径0.86mの円内に68%入るくらいの精度ですね。2σ(すなわち95%)の精度まで考えるなら、半径 1.72mの円になります。直径に直せば3.44mなので、SBASなしの単体測位だったのにSBASを使ったのではないかと思うくらい(単体測位にしては)バラつきが少なかったようです。

一方、エンコーダについてはプログラム設計のまずいところが発覚してしまい、プログラムを修正してデータを取り直さないとまずい状態だとわかりました。具体的には、センサデータのログをみると100ms周期で計測しているつもりのタイムスタンプが115msを中心にして76~154msの幅でサンプリング周期がばらついていることがわかりました。サンプリング周期がばらつくと走行速度の算出に大きな影響がでるだけでなく、速度を積分して得られるロボットの位置にも大きな影響が出ます。まずいです、これは。うかつでした。Windows上で .NET Frameworkを使っているので、タイマの精度が当てにならないということをすっかり忘れていました。

こんな調子なので、今週末までに自律走行は無理そうです。どうやて間に合わせよう?

« 2009年10月 | トップページ | 2009年12月 »

最近の写真