青はフルスケール2000dps、赤は245dpsで、下のグラフは上を拡大したもの。
実際にはFSキッチリでサチるわけじゃなく、-2^15 - +2^15-1の範囲でサチっている。
結構しっかり振り回したつもりなんだけど、2000dpsのほうはサチらなかった。2300dpsくらいでサチるので、まだだいぶ余裕がある。
拡大した部分を見ると、若干の差が見られる。
FS2000dpsと245dpsの差が下のグラフ。
縦軸が角速度、横軸が差。
250dps時で10dps程度の差なので、4%程度のゲインエラーがありそう。
オフセットは1dpsくらいかな。ちょっと少なすぎる気がするけど、こんなもんか。FS245dpsだとオフセット114LSBで1dpsくらいなので、確かにその程度だ。
***
スペースプローブで姿勢伝搬を計算しようとすると、一瞬でもサチると以降の姿勢が得られなくなるので、大きめのFSを設定する必要がある。
一方で、FSを大きくすると分解能が下がってしまう。おそらくノイズ特性も悪化するはず。
例えば、センサAは500dps、センサBは2000dpsで、センサAがサチった場合はセンサBのみを使用する、みたいな運用もアリだろうが、そうするとセンサAがサチった部分の分解能が悪くなる。またそれ以降の姿勢にも影響が及ぶ。
A/B共に2000dpsにしておけば全期間で同程度の信頼性になるが、誤差の蓄積は多くなるはず。
スペースプローブで一番角速度が大きいのは、ロケットから放り出されるタイミングで、打ち上げ期間中はほぼ角速度は無く、パラシュートによる降下中も、さほど大きな角速度にはならないはず。
と考えると、複数のレンジの組み合わせは有効な気がするけど、どの程度の角速度を想定するか。
現在はセンサ全体のオフセット値を補正に使ってるけど、サチったセンサは除く、という使い方だと、センサ毎のオフセット値を計算してやらなきゃいけないのか。面倒だなぁ。。
***
このセンサ、ローパスフィルタやハイパスフィルタも入っているらしい。ステッピングモーターとかで角加速度を与えれば計測できるだろうけど、モーター使うと電源やら試験場所やら、一気に面倒になるんだよなぁ。
でも、ステッピングモーターだとかなり正確な角速度を与えられるはずなので、センサ毎のゲインエラーを計測したりということもできるようになる。複数の角速度を与えて計測すれば非線形性エラーも補正できるだろうけど、ミッション期間たかだか数十秒のスペースプローブでそこまで必要になるとは思えんし。ミッション期間数時間のスペースバルーンとかだとどんな事やっても精度足りないだろうし。
***
カメラの手ぶれ補正だと、もうすぐ地球の回転が問題になるようなレベルまで進化してるんだそうだ。4.16 millideg/sec (15 deg/hour)がノイズに埋もれないセンサ、ということになる。いったいどんなセンサを使ってるのやら。
***
追記
静止時のエラー。
出力値が小数点以下1桁しか出してないのでガタガタだけど。
100秒後で1.5度程度の誤差。スペースプローブのミッション期間は1分に満たない程度のはずだから、十分なはず。
ただ、245+2000の組み合わせで、かつ静止時なので、オーバーレンジ時(2000のデータだけ使う)の誤差はどれくらいになるか不明。
245と2000は50%/50%で計算してるけど、本来は245%が7割くらいの比率で組み合わせたほうがいいはず。このあたりはノイズフロアを比較して決めるべきなんだけど。
とりあえずジャイロベースでの姿勢推定がある程度の精度で動くっぽいぞ、というのはわかってきたので、次は加速度と地磁気で絶対的な姿勢を推定したい。加速度はまぁどうとでもなるけど、地磁気が結構面倒。オフセットエラーやゲインエラーを求めなきゃいけない。ジャイロベースで推定した姿勢から地磁気を球体に貼り付けて球体の中心と大きさを計算する、みたいな流れでできそうな気もするんだけど。
本来は地磁気センサの値だけでも計算できるっぽいんだけど、数学的な話ばっかりでいまいち理解できない。
***
追記
べた書きでサチった軸を除去するような処理を追加してみた。
手でグリグリ回して戻しただけなので最終的な角度に誤差が残ってる。
紫、水色、オレンジは使用したデータ数を表している。通常はセンサ2個の平均を使っているので2だが、センサがサチった場合はその軸を使わないので、1に落ちる。
内部処理の関係で絶対値での表示だが、ジャイロセンサの生データ。
上の図でデータ数が落ち込んでいる場所は、角速度が245dpsを超えていることがわかる。2000dpsをサチらせるのはかなり大変。手で捻った程度では一瞬しか角速度が発生しないので、やはりステッピングモーターで振り回したい。
サチったと言っても4秒付近でZ軸が大きめに回った以外はあまり大きな値ではないから、これだけで正常に動いていると判断はできなさそうだ。もう少し追加検証が必要。
現在はFS245dpsとFS2000dpsの組み合わせだが、例えば245dpsと500dpsを組み合わせて、245がサチったら2000dpsに再設定し、その状態で500dpsがサチったら2つとも2000dpsに設定し、というような動作モードもアリかもしれない。ただ、定角加速度の場合で、最終的に1500dps程度に達するような使い方ならいいけど、大きな角加速度で一気に800dpsを超えるような動きには対応できない。
例えば、モデルロケットとかに載せて、空力的に角速度が発生する場合には、オートスケールは有効かもしれない。スペースプローブのような、空中に放出される際に大きな角速度が発生する場合には無力。
打ち上げ中は245dps/500dpsで、無重力状態(放出直前)になったら2000dps/2000dpsにし、降下中はオートスケールで計測する、といった、強制的にレンジを広げる機能も作っておけばいいかもしれない。
十分に角速度が低い場合はすべてのセンサが245dpsで計測し、角速度が増えてきたら1つを500dpsに設定し、500dpsに近づいてきたらすべてを500dpsに設定し、というようなモードでもいいかもしれない。そうすれば精度的に有利。レンジをPD制御する、みたいな感じか。
ジャイロで遊んでると一向に加速度センサの方が進まないなぁ。
加速度センサ積分してもつまらないでしょ～という思い込みもある。ジャイロは積分すれば姿勢が得られるけど、加速度を2回積分したからと言って位置は得られない…と思うんだけど。
***
追記
姿勢の計算の処理にかかる時間を計測してみた。
floatで12マイクロ秒、doubleで100マイクロ秒程度だった。
doubleはソフトウェアで処理してるのでさすがに時間がかかるが、floatなら十分短い。doubleでも、更新レートが100Hzなら十分余裕がありそうな感じ。
SDカードへの保存とかも含めると、ちょっと余裕ない感じか。
デバッグ用のUSB CDCやRTOSでどれくらいリソース食ってるかにもよるかな。別にCPU使用率も測らないといけないけど、そっちはちょっと面倒な仕掛けが必要。
doubleの性能はもっと悪いと思ってたが、意外とたいしたことないんだな。
ま、今はジャイロだけしか扱ってないので、今後数倍程度には増えるだろうけど。と考えると、doubleはあんまり余裕ないかも。
168MHzのCortex-M4Fだけど、12マイクロ秒だと2k命令、100マイクロ秒だと17k命令くらい。姿勢の計算が2千命令程度に収まってる? そんなに軽いんだろうか……
この時間にはセンサへのアクセスは含めていない。
センサのアクセス時間は、SPIクロック5.25MHzで100マイクロ秒程度だった。センサが増えれば処理時間も長くかかる。STのセンサはたいていSPI10MHzまで対応してるので、センサアクセス時間が問題になるなら、コアクロックを160MHzにダウンクロックしてSPIを10MHzにすれば、2倍弱の高速化ができる。まぁ、気休め程度だけども。
スペースプローブとかで使う場合、あんまり姿勢計算とかに専念すると、他のミッションがやりづらくなるんだよな。姿勢制御くらいならなんとかなるだろうけど、JPEGカメラとかつけようとすると急に辛くなると思う。STBee F4miniならさほどの大きさじゃないので、複数枚乗せるというのもアリだろうけど。
DCMIとか使うならSTBee F4miniじゃ不可なので、ボード2枚載せとか必須になるし。
0 件のコメント:
コメントを投稿