ジャイロセンサを用いた角度の算出について
現在二足歩行ロボットを製作しているのですが、ボディ剛性が低いのとモーターを増やしすぎた(RC用のサーボを26個使用しています)ためか、直立状態でも姿勢が安定しないことから、姿勢の安定化のためのフィードバック用にジャイロセンサを利用しようとしたのですが、多々の問題から上手く角度が算出できずに困っています。ジャイロセンサを扱った経験のある方がいらっしゃいましたら御指導願います。
マナーが悪くて恐縮なのですが、同様の質問を下記のURLでしています。
http://oshiete1.goo.ne.jp/qa4561017.html
上記のURLでいただいたアドバイスを元に、現在はサーボ1個をジャイロを動かすために使用し、マイコンのA/Dで値を取得してexcelで計算という作業を繰り返しています。
開発環境は下記の様になります。
・ジャイロセンサ
→村田製作所製圧電振動ジャイロENC-03R(秋月電子製圧電振動ジャイロキットをそのまま使用し、オペアンプの出力をマイコンのA/Dに直結)
印加電圧:3.38V(マイコンに印加している電圧をそのままセンサに分岐)
静止時出力:約1.41V(たまたまかもしれませんが手元にある2個のジャイロキットは大体これ位出力しています)
・マイコン
→SH7144F(秋月電子製SH7144Fマイコンボード(12.5MHz))
クロック設定PΦ=25MHzシングルチップモード
・コンパイラ
→GCC
・現在行っていること
(1)SH7144のA/Dを10bit、PΦ/8で連続スキャンモードにし、CMTを8msec(センサの応答20msecの2.5倍でサンプリング)で割り込みさせてA/Dの結果をUARTでPCに出力。
(2)UART割り込みでサーボを90°回転。このときサーボ回転開始時と停止時に"S"と"E"をPCに出力し、サーボの回転時間を計算できるようにしておく。
(3)SH7144からのUART出力をPCのハイパーターミナルでテキストキャプチャして結果をexcelに入力。
(4)excelにてA/Dの結果→電圧→角速度→角度を計算。同時にサーボの回転時間と角度から平均角速度を算出して比較。
現在抱えている問題
(1)角速度計算結果について
今回使用しているジャイロキットの場合、角速度を求める式は下記の式であっているでしょうか?キットの回路は村田製作所から出ている増幅回路例と同じです。
ENC-03Rのカタログ
http://www.murata.co.jp/catalog/s42.pdf
角速度=(A/D出力-静止時出力)*(3.38/1024)/(感度*ローパスフィルタの増幅度)
※静止時出力=静止時のA/D出力をあらかじめメモして代入
感度=0.00067[V/deg/sec]
ローパスフィルタの増幅度=10[倍](カタログの回路図より直流増幅度)
参考URL
http://okwave.jp/qa3523603.html?ans_count_asc=0
http://www.hokutodenshi.co.jp/PUPPYSupportPage/soft/status/status.html
(2)センサーからの出力について
・他の人のホームページを見た感じのセンサの出力
/\
/ \
↑センサ出力 / \
静止電圧___/ \
\/ ̄ ̄ ̄
→時間
参考URL
http://tkstechnology.web.fc2.com/robot/system/enc_03.html
・私の持っているジャイロキットの出力
/\
/ \
↑センサ出力 / \
静止電圧___/ \
\ / ̄ ̄ ̄
\ /
\ /
\/
→時間
上記のような出力のせいか、積分の式が間違っているのか、または別の問題か積算していくうちに角度の計算結果が0付近に戻ってしまいます
。
(3)積分について
数値積分の表記は下記のようであっているでしょうか。
C風表記
kakudo = kakudo + (kakusokudo_s + kakusokudo) * (t / 2);
kakusokudo_s = kakusokudo;
(上記をタイマで繰り返す)
※kakudo:角度(初期値0)[deg]
kakusokudo_s:一回前のサンプリング時の角速度(初期値0)[deg/sec]
kakusokudo:サンプリングした角速度(初期値0)[deg/sec]
t:サンプリングタイム[sec]
excel風表記
角度 = 1セル前の角度 + (1セル前の角速度 + 角速度) * (サンプリングタイム / 2)
(2)の問題で、時間がたつにつれて計算結果が0付近になってしまうのも問題なのですが、サンプリングタイムに0.008[sec]を代入しても、角度の値が出てこなくて困っています。
お礼
改めて回答ありがとうございます。 ただ、残念ながらセンサのスペックシートにはそのような項目はありませんでした。