ジャイロセンサーの繰り返し誤差の補正について

マイコン初心者です。
SH7144のA/Dコンバーターにジャイロセンサーを接続し、ジャイロの出力を積分して、角度を求めようとしました。下記のコードで、積分値は出せたのですが、ジャイロを一度傾けて元に戻しても初期値に戻らず、オフセットが残ってしまいます。積分前の出力を見ると、CW側にジャイロを回すと、CW側に値が振れた後にCCW側へ少し値が振れているようなので、角速度のデータから来る誤差じゃないかと自分では仮説を立ててみたものの、その補正のところでつまづいてしまいました。
質問はCだけの問題ではないかもしれませんが、組み込みでジャイロセンサーを扱った経験のある方がいましたら、ご教授をお願いいたします。
ジャイロセンサーは村田製作所製圧電振動ジャイロ、ジャイロスターを使用し、カタログに載っているハイパスフィルタとローパスフィルタを通して、SH7144のA/Dコンバーターへ接続しています。
・ジャイロスターのカタログ
ttp://www.murata.co.jp/catalog/s42.pdf
角速度はコンペアマッチタイマを使用し、ノイズ対策のためにCMT0で角速度の平均値を計算しています。CMT0は0.8msecごとにで割り込みが発生するように設定し、main()ではタイマのカウンタを開始するコードのみを書いています。下記にCMT0割り込みのコードを示します。
void int_cmi0(void)
{
unsigned static int t_ave = 0;
static short int adc0_buffer[3] = {0, 0, 0};
unsigned short int i;

unsigned static short int count0 = 1;

adc0_buffer[0] = adc0_buffer[0] + AD0.ADDR0.BYTE;
adc0_buffer[1] = adc0_buffer[1] + AD0.ADDR1.BYTE;
adc0_buffer[2] = adc0_buffer[2] + AD0.ADDR2.BYTE;

t_ave++;
if( t_ave > 100)　//AD0のADDR0～2の100回平均を求めています
{

for(i = 0; i < 3; i ++)
{
data_adc0[i] = (adc0_buffer[i] / t_ave) - 109;　//"109"は静止時の出力電圧を示しています

if(-3 <= data_adc0[i] && data_adc0[i] <= 3) //平均しても振れてしまう値の分をカットしています
{
data_adc0[i] = 0;
}

adc0_buffer[i] = 0;

if(count0 > 89)　//電源投入時にdata_adc0[]の値が安定しないため、最初の10個のデータを捨てています。
{
count0 = 100;
}
else
{
count0++;
data_adc0[i] = 0;
}
}
t_ave = 0;
}
CMT0.CMCSR.BIT.CMF = 0;//コンペアマッチフラグをクリア

}
角度は、CMT1を使用し、8msecの間隔で割り込みを発生させ計算しています。最終的な計算結果は、SH7144のSCIを使用して、PCのハイパーターミナルに表示しています。
void int_cmi1(void)
{
static short int adc0_buffer_s[3] = {0, 0, 0};　//角速度の初期値を入れるバッファです
　unsigned static short int count1 = 1;
　short int i;

for(i = 0; i < 3; i ++)
{
data_adc0i[i] =data_adc0i[i] + ((adc0_buffer_s[i] + data_adc0[i]) / 2);　//積分を行う部分です。最終的に制御用のゲインにしようと考えているので、時間項8msecは明記していないです。
adc0_buffer_s[i] = data_adc0[i];

if(count1 > 89)　//角速度と同様最初の10個のデータは0にしています
{
count1 = 100;
}
else
{
count1++;
data_adc0i[i] = 0;
}
}
CMT1.CMCSR.BIT.CMF = 0;//コンペアマッチフラグをクリア

}
main()では、各機能のイニシャライズと各CMTのカウンタ動作を開始するコードのみを書いています。ハイパーターミナルで動作が見れているので、各機能は動作していると思います。
void main(void)
{
initIO();　//各機能のイニシャライズです。
initINT();
initSCI();
initADC();
initCMT();
CMT.CMSTR.BIT.STR0 = 1;//CMT0カウント開始
CMT.CMSTR.BIT.STR1 = 1;//CMT1カウント開始
while(1);
}

No.4 ベストアンサー 回答者： Interest 回答日時： 2008/12/23 22:01

ANo.1～3=Interestです。

ちょっと考えてみたのですが、手順を踏んで問題を切り分けて考えたほうがいいかなぁと。

(1) サーボモータを動かさない状態でジャイロの出力から正しく姿勢が求まることを確認する。
(2) 回路的にノイズを抑制できないのであれば、(1)にノイズ除去／平滑化の仕組みを盛り込む。

> ノイズの周波数の件ですが、手元にスペアナがない+本格的な周波数解析も初めてなので、
> これもまた自信がないのですが、サンプリング周波数1.25kHz(0.8msec)でSH2のA/Dを使って
> サンプリングしてscilabで、離散フーリエをかけてみました。

がんばりましたね～。うちのデジタルオシロにはFFTが付いているのでそんなものすぐに測れるだろうと思っていましたが、無ければ無いなりに工夫しましたね。驚きました。scilabというのも初めて知りました。

さて、±15mVのノイズですが、ムラタのジャイロスターの感度が0.67mV/deg/sec ということからすると、5deg/secぐらい狂ってしまう大きなノイズなのですね。ふと私の手持ちのジャイロの感度がどれくらいか調べてみたところ、
　IDG-300の場合、　　　2.0 mV/deg/sec
　HS-EG3(司21)の場合、25.0 mV/deg/sec
と、ジャイロスターと比較するとノイズに対してかなり寛容だということがわかりました。（反面、ダイナミックレンジが狭いのですが。）結構違うんですね。

参考
http://strawberry-linux.com/catalog/items?code=1 …
http://www.mecharoboshop.com/Products/STLJapan/t …

この回答への補足

返答が遅くなりすいませんでした。
返答が遅くなったのは結果が出てないせいです。(泣)
頂いたアドバイスを元にデジタルGNDとアナログGNDを電源付近で1点接続にしてみました。結果、26個のサーボ駆動で±15mV位のノイズ→±10mV位になりました。
元々デジタル用とアナログ用の2個のバッテリーを積んでいるので、デジタルGNDとアナログGNDが明確に分かれていたことから1点接続は比較的簡単にできたのですが、
"太く""短く" → 基板を自前のCNCフライスで切削加工しているのでベタグランドがうまく作れない。　+　センスの問題？で現時点でこれ以上は難しいです。
現在行っている内容も含めて、Cの問題とは少し異なってると思いましたので、マナーが悪くて恐縮ですが、別のカテゴリで、質問を立てました。この質問もしばらく締め切らずにいようと思いますが、差し支えなければ参照願います。
http://oshiete1.goo.ne.jp/qa4584822.html

上記の質問とは別に移動平均のコードを書いてみたので下記に示します。結果は多分あっていると思います。コードは20回平均ですが、40回平均くらい取ると、サーボがプルプルしても差し支えないレベルまで値は安定しますが、実用的にはちょっと疑問かもしれません。

unsigned short int adc0_buffer[20][3];　//移動平均用バッファ"[3]"は3軸用の為です。グローバルで宣言し、main()であらかじめ20個分の初期値を代入しています
unsigned int buffer_sum[3];　//平均値計算時の合計値用バッファです
int data_adc0i[3];　　//計算結果を入れる変数です。

void int_cmi0(void)
{
short int i, j = 0;

for(i = 0; i < 20; i++)　//20回の合計を求めます
{
for(j = 0; j < 3; j++)
{
buffer_sum[j] = buffer_sum[j] + adc0_buffer[i][j];
}
}
for(i = 0; i < 3; i++)　　//平均を求めます
{
data_adc0[i] = buffer_sum[i] / 20;
}
for(i = 0; i < 19; i++)　//adc0_buffer内の値を1個ずらします
{
for(j = 0; j < 3; j++)
{
adc0_buffer[i][j] = adc0_buffer[i + 1][j];
}
}
buffer_sum[0] = 0;　　//合計値を初期化します
buffer_sum[1] = 0;
buffer_sum[2] = 0;
adc0_buffer[19][0] = AD0.ADDR0.WORD;　　//配列の最後に新しい値を代入します
adc0_buffer[19][1] = AD0.ADDR1.WORD;
adc0_buffer[19][2] = AD0.ADDR2.WORD;
for(i = 0; i < 3; i++)
{
adc0_buffer[19][i] = adc0_buffer[19][i] >> 6;
}
CMT0.CMCSR.BIT.CMF = 0;//コンペアマッチフラグをクリア
}

コード面で進展がありましたら、御報告いたしますので御指導のほどよろしくお願いいたします。
参考URL
http://www.hokutodenshi.co.jp/PUPPYSupportPage/s …
http://tkstechnology.web.fc2.com/robot/system/en …
http://okwave.jp/qa3523603.html?ans_count_asc=0

補足日時：2008/12/26 17:50

この回答へのお礼

現在補足の方に示した質問の方をメインに話が進んでいるので、一度こちらの質問の方は締め切りたいと思います。
貴重なアドバイスありがとうございました。
また、何かアドバイスをいただけるようでしたらよろしくお願いいたします。
http://oshiete1.goo.ne.jp/qa4584822.html​

お礼日時：2009/01/08 13:08

No.3 回答者： Interest 回答日時： 2008/12/17 22:14

ちょっと私の勘違いがあったかもしれませんので確認させてください。

> ジャイロ静止時に約±15mVのノイズがのっています。

これは、GNDが動いているという意味ではなくて、ジャイロの出力にノイズがのっているということでしょうか？　ノイズの周波数もわかったら教えてください。（周波数からノイズがどこから出ているかわかるかも？）

> 何かよい方法がありましたら、教えていただけると幸いです。

まずはハードウェアのほうでノイズの発生源を突き止めて、対策を取りましょう。「グランドは、太く短く、一点接地」が基本です。

次に、ADC値から角速度を求める計算式を入れましょう。
（角速度）＝( (ADC値）- （静止時のADC値) ) ×（3.3 / 1024) / (感度）
です。

ハイゲインのサーボを使っていて負荷の大きい関節がプルプルしちゃうことによるノイズ問題は、すでに行っている振動の周期より早いサンプリングレートでAD変換して平均をとるという方法でよいと思います。ただし、平均をとるのは100回ずつではなくて、もっと回数を減らして移動平均をとったほうが効率が良いでしょう。

屋外を走行するロボットは路面の凹凸による振動を受けるので他人事ではありません。私もこれからジャイロの特性試験をするつもりなので、もしかしたらここにそのソースコード（SH2用）をアップするかも。かも、ですよ。

この回答への補足

毎度ご丁寧な回答ありがとうございます。
正直昨日の時点では半ばあきらめかけていたのですが、おかげさまでやる気が出ました。
今日はノイズの解析でほとんど終わってしまったのですが、今日やったことをご報告させていただきます。

＞これは、GNDが動いているという意味ではなくて、ジャイロの出力にノイズがのっているということでしょうか？　ノイズの周波数もわかったら教えてください。（周波数からノイズがどこから出ているかわかるかも？）

これは、多分GNDじゃないかなという感じです。
といいますのも、手持ちのオシロスコープでは±15mVというレベルになるとオシロスコープ自身のホワイトノイズに埋もれて見えないのです。その為、SH2のA/Dをデジタルグランドにつないで、サーボを動かした時のA/Dの結果と、動かしていないときの結果を比較しました。回路のA/D比較用の基準電圧は回路を駆動している電源と一緒なので自信はないのですが、サーボを動かしたとき最大で±15mV位値が動いていました。
ノイズの周波数の件ですが、手元にスペアナがない+本格的な周波数解析も初めてなので、これもまた自信がないのですが、サンプリング周波数1.25kHz(0.8msec)でSH2のA/Dを使ってサンプリングしてscilabで、離散フーリエをかけてみました。
なので調べた範囲は600Hz位までだと思うのですが、サーボ無しの結果は、全体的にブロードしていて、どこかの周波数にノイズが立っているという感じではありませんでした。エンベロッブで見ると300Hz位のところに山があるように見えます。
サーボ有りで測定した結果ですが、やはり全体的にブロードしていてエンベロッブで見ると300Hz位のところに山があるように見えます。ジャイロの応答が50Hzなので、50Hz近傍を見ると10Hzと35Hzあたりに比較的ノイズが立っているように見えますが、これに対する対応となるとちょっとどうしようか悩みます。
回路に関しては、スケジュールの関係で結果は来週末位にご報告ということになりそうで、少し間が空いてしまうのですが、よろしくお願いいたします。

補足日時：2008/12/18 19:02

No.2 回答者： Interest 回答日時： 2008/12/17 21:35

ANo.1=Interestです。

アプリケーションは二足歩行ロボットの姿勢制御ということで了解しました。

まず、ノイズの話から。

> ひとつはモーターの制御信号用のGNDとマイコン制御用のGNDを完全に
> 切り離していない(基盤のサイズの関係でフォトカプラ等がのせられな
> かった)ためか、ジャイロ静止時に約±15mVのノイズがのっています

モータのノイズ、という話はよく耳にしますね。制御信号のGNDとパワー系のGNDは、バッテリのところで一点接地していますか？　私は会社の先輩から「グランドは、太く短く、一点接地」と習いました。（語呂がいいですね）

二足歩行ロボットだと、膝を曲げた状態で膝のサーボに最大トルクがかかると思いますから、その状態で最大電流がどれだけ必要だと見積もっているか、それに対して用意したバッテリは十分な電流を供給できるか、ついでに見ておいてください。

> A/Dを8bitで取得していたのは、このノイズを10bitでA/D変換すると
> 最大で(30mV*1024)*3.3V=9.3(バイナリで1001)となるので、
> A/D下2桁は使っても仕方かないんじゃないかと考えたためです。

計算がおかしいですよ。
(30mV / 3.3V)*1024=9.3 -> 10くらいじゃないですか？
確かに、ADCの下位2ビットはノイズレベルであるとして切り捨てることもありますから、2bitを切り捨てることが間違っているわけではありません。（しかし、平均をとるならより情報量が多い方がいいですよね。）

> ジャイロをロボットに搭載した状態で直立させると、
> 約±105mVまでノイズが大きくなってしまっています。

これは信号線のGNDに乗ってしまうノイズのレベルですよね？　であれば、ソフトウェア的にごまかすよりハードウェア（電源周り）の対策を優先すべきです。

次にジャイロの角速度ですが、

> 自分の解釈では"ジャイロの角速度=ジャイロ移動時の出力-ジャイロ静止時の出力"と思いましたので、

これは良いのですが、肝心のADCで読んだ値を角速度に変換する式が質問文中のソースコードには出てきていません。計算式は次式になるかと思います。
（角速度）＝( (ADC値）- （静止時のADC値) ) ×（3.3 / 1024) / (感度）
ただし、この式をそのままC言語のソースに落とすと計算精度の問題がありますし、浮動小数点の演算を行うとバカみたいに遅くなるので、全体を1000倍して mV単位で計算する、計算の順序を入れ替える、などの工夫が必要です。

ついでに書くと、

> if(count0 > 89)　//電源投入時にdata_adc0[]の値が安定しないため、最初の10個のデータを捨てています。

という処理も、ここに入れるべきではないと思います。私なら、CPUが立ちあがってから初期化処理が始まる前に、電源が安定するまでの待ち時間を入れます。今回の例でいえば、InitIO();の前に必要な時間だけ（数十ミリ秒？）の待ち時間を入れると、 int_cmi0(void)の中に変な処理が入らずすっきりしますよね。

カルマンフィルタは難しいので忘れてください。
つづく。

No.1 回答者： Interest 回答日時： 2008/12/17 01:20

こんにちは。

私は屋外走行用の自律移動ロボットを作っており、その関係でジャイロセンサを扱っています。経験的には、角速度を積分して角度を出すのは、時間が経つにつれてだんだんずれていきます。

maa_teruさんは、どういう用途でジャイロを使うのでしょうか？
読めば読むほどソースコードが怪しく見えてきたのですが・・・(^-^;A

> adc0_buffer[0] = adc0_buffer[0] + AD0.ADDR0.BYTE;
> adc0_buffer[1] = adc0_buffer[1] + AD0.ADDR1.BYTE;
> adc0_buffer[2] = adc0_buffer[2] + AD0.ADDR2.BYTE;

SH2のADCの分解能は10bitなので、ここですでに2bit分データの精度が落ちているかも。

> ノイズ対策のためにCMT0で角速度の平均値を計算しています。

すでにデータシート通りにLPFが入っているのに、何のノイズなのですか？
平均をとることとLPFを入れることは、同じことですよね。ジャイロセンサを机の上に置いた状態でオシロスコープで出力を表示させて、ホワイトノイズが観測できるかどうか確認してみてください。（オシロがなければAD変換した値を生のままSCI通信でPCに送って、Excelで表示するとか。）

> "109"は静止時の出力電圧を示しています

109という値がどうやって出てきたのか、計算の根拠を説明してください。
SH7144の電源電圧3.3Vで、ADCの出力を8bit扱いしたとして、ジャイロ静止時の出力電圧Vdc=1.35Vなら、1.35/(3.3/256) = 104.7 -> 105 くらいじゃないかと思います。

> if(-3 <= data_adc0[i] && data_adc0[i] <= 3)
> //平均しても振れてしまう値の分をカットしています

ADCの出力が data_adc0[i] に入っているのだと思いますが、負の値をとることがあるのでしょうか？　-3 と 3、この値も何の値かよくわかりません。ADCの出力を8bit扱いするなら、0～255までしか値をとりませんよね？

CMT0で0.8ms周期でADCの値の100回平均を求めるには合計80msかかるのに、
CMT1で8ms周期で積分を行うというのもなにか変なような気がします。
（100回平均とること自体が無駄だと思いますが。）

参考までに、航空機等で使用されるIMU(Inertial Measurment Unit：慣性計測装置)ではカルマンフィルタを用いて精度の向上を図っています。が、計算量がSH2の内部で処理するには膨大ですので、私はセンサデータを生のままPCに送って、PC側でカルマンフィルタをかける方法をとっています。

この回答への補足

御指導ありがとうございます。
御指摘の内容について、再度確認した内容を補足として記載させていただきます。

＞すでにデータシート通りにLPFが入っているのに、何のノイズなのですか？

最初に、もともとジャイロセンサーの用途について、現在二足歩行ロボットを製作しているのですが、ボディ剛性が低いのとモーターを増やしすぎた(RC用のサーボを26個使用しています)ためか、直立状態でも姿勢が安定しないことから、姿勢の安定化のためのフィードバック用にジャイロセンサを利用しようとしたことが始まりです。
ノイズの話に戻りますが、今回の問題では2種類のノイズがジャイロにのっていると思っています。
ひとつはモーターの制御信号用のGNDとマイコン制御用のGNDを完全に切り離していない(基盤のサイズの関係でフォトカプラ等がのせられなかった)ためか、ジャイロ静止時に約±15mVのノイズがのっています。A/Dを8bitで取得していたのは、このノイズを10bitでA/D変換すると最大で(30mV*1024)*3.3V=9.3(バイナリで1001)となるので、A/D下2桁は使っても仕方かないんじゃないかと考えたためです。ご指摘の通りここで2bit分の誤差が生じます。
もうひとつのノイズですが、多軸でRCサーボを使用しているため、負荷のかかっているサーボが小刻みに振動してしまい、ジャイロセンサがこの振動を拾ってしまうことからボディの振動がノイズとなって現れてしまいます。ジャイロをロボットに搭載した状態で直立させると、約±105mVまでノイズが大きくなってしまっています。角速度を求めるために100回平均をとったり、コードに"if(-3 <= data_adc0[i] && data_adc0[i] <= 3) "などと書いているのは、これらのA/D変換後の値の振れを押さえ込む苦肉の策でした。

＞109という値がどうやって出てきたのか、計算の根拠を説明してください。

これに関しては、計算の根拠と言うよりも実測で静止時の出力が"109"だったためこの値を用いました。御指摘の通りカタログからの計算上は105になると思うのですが、ジャイロ静止時の出力が実測で約1.41Vを出力していたので、1.41/(3.3/256)=109.4となり、A/Dの出力とも合っているので、"109"になってしまったという感じです。

＞ADCの出力が data_adc0[i] に入っているのだと思いますが、負の値をとることがあるのでしょうか？　-3 と 3、この値も何の値かよくわかりません。ADCの出力を8bit扱いするなら、0～255までしか値をとりませんよね？

御指摘の通りA/Dの値は0～255までしかとらないのですが、自分の解釈では"ジャイロの角速度=ジャイロ移動時の出力-ジャイロ静止時の出力"と思いましたので、計算結果ではCCW側にジャイロが回った際は負の値となると思います。しかし、"if(-3 <= data_adc0[i] && data_adc0[i] <= 3)"のコードは角速度の計算結果を-3～3の間、ごっそり破棄しているので、大きな誤差を生じる結果となってしまったと思います。試しに破棄する値の範囲を-1～1にしたところ繰り返し誤差が2/3くらいになりました。

＞CMT0で0.8ms周期でADCの値の100回平均を求めるには合計80msかかるのに、
CMT1で8ms周期で積分を行うというのもなにか変なような気がします。

これは、ご指摘の通りまったくの間違いだったと思います。もともとジャイロの応答20msecに対し2.5倍でサンプリング周波数を定め、10回平均を取るためにCMT0の周期を0.8msecにしたのですが10回平均では値が安定せず闇雲に100回平均にしたまま、CMT1の周期を見逃していたという感じです。

結論として、現在の問題は角速度を求める点で値の変動にとらわれるあまりに、値を丸めすぎている(大雑把に切り捨てすぎている)ところにあり、1からコードを組みなおさないといけないと感じました。ただ今回書いたコードでも自分の中では精一杯だったので、現段階ではまだコードは思いついていない状況です。教えていただいたカルマンフィルタについて少し調べてみたのですが、理解には時間が掛かりそうです。何かよい方法がありましたら、教えていただけると幸いです。

補足日時：2008/12/17 14:55