立命館守山高校 Sci-tech部 加速度センサで傾斜を測ろう!

スポンサーサイト

上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。

加速度センサで傾斜を測ろう!

どうも、卒業して今や大学生活を送っているkuroです。
なんというか更新されなさそうなので
上記の広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書く事で広告が消せます。
という表示が出る前に更新しようかと思います。

ということで、今回は微妙に暇な講義中に考えた加速度センサで角度を求める方法についてです。
逆三角関数という概念を知って感動の余り勢いで作りました。
むしろ今まで知らなかったのかとか言わないで…
関数電卓一昨日買ったんです。

で、早速本題ですが
今回は、3軸加速度センサがあったとしてもXYの2軸、地面と平行な軸を使います。

概要

アークサイン、アークコサイン、アークタンジェントのテーブルを作成する。asin()関数を使っても構わない。
傾きの方向(XY軸の加速度ベクトルの向き)をアークタンジェントで求める。
加速度の大きさ(ベクトル)を三平方の定理で求める。
加速度からアークサインでZ方向の傾きを求める。

といった感じになっています。
サインとかコサインとかベクトルとか加速度とかルートとかが意味不明だとちょっと難しいかも知れません。
ただ、一度関数に実装してしまえば誰でも使えるので、わからなかったらおとなのひとにきいてみよう!(責任転嫁
一応今回は、概念だけなので関数は作りません。ていうか作りたいけど手元にArduinoも加速度センサも無いので作っても動作確認できません。


ではまずは、逆三角関数の求め方です。
Arduinoのリファレンスにちょこっと書かれてますが、ArduinoはAVR LibcにリンクされているのでAVR Libcの関数は全て使えます。AVR Libcの関数一覧を見ればわかりますが、とても豊富です。
ということで、math.hのasin()なども普通に使えます。
ただ、math.hがどういう風に計算を行っているかは知りませんし、もし浮動小数点演算をばりばり使っているなら、プログラムの実行速度に影響を与える可能性があります。気になるなら一度測ってみる必要がありますね。

もうひとつの方法は、値を1万倍した逆三角関数テーブルを作るという方法です。
これだと、確実に浮動小数点演算は必要無いですが、代わりに303個の値を収納する必要があるのでRAMを食います。
intに収納すると、606バイトほど使うことになります。
ArduinoUNOのメモリサイズは2kbですので、かなり宜しくない状況だと思います。
1個おきか2個おきの値にすれば、容量は半分か1/3で済みますが、それでも大きいですよね。
そして何より、Arctanは無限に続くので、作りにくい。

どっちが良いか分かりませんが、お手軽なのはasin(),acos(),atan()を使うことだと思います…


ここから本題
accel2.jpg
加速度センサの状態はがこんな感じになっているとします。
黒矢印が重力加速度、赤がX軸、青がY軸で、白がX軸とY軸から求められる重力加速度のベクトルです。
重力加速度は静止していれば1Gなので、白矢印が求まればアークサインから角度が求まります。

XY軸の加速度ベクトルの向きを求める
まず、白矢印が、X軸から何度の位置にあるかを計算します。
これはアークタンジェントを使えば簡単に求まります。
θ_xy=tan^(-1)(y/x)
これだけだとどの象限にあるのか分からないので、XY軸の正負から判断します。
その場合角度は全てX軸からになるので、例えば第二象限では180からθを引くみたいな処理が必要です。
後、

ベクトルの大きさを求める
こっちは更に簡単。
ピタゴラスの定理、若しくは三平方の定理を使うだけです。
α=(x^2+y^2)^(1/2)

これで加速度ベクトルの大きさが求まりました。


角度を求める
アークサインで角度を求めます。
θ_z=sin^(-1)(α/1)
以上です。


これで、傾いている向きと角度がわかりました。
ちなみに、角度の方は
θ=sin^(-1)(x^2+y^2)^(1/2)
みたいな感じで一発で求めた方が早いですね。


そういえば、数式書くのが面倒だったので^とか使ってますが、分かりますよね。
^は上付き文字です。
1/2乗してるのはルートです。




さて、webで文章を書くと無駄に文章が長くなるのは自分の悪い癖ですが、上の長ったらしい説明わかりましたでしょうか?
高校数学+αぐらいの知識があれば理解できると思います。何せ数学の成績が3.0ぎりぎりだった自分が言うんですから間違いない。
実装するとなると20行ほどで書けると思います。


float x = 0.3;
float y = 0.5;
float xyangle, zangle;

xyangle = atan(y/x);
if(x>=0){
  if(y>=0){
  }else{
    xyangle = 360 - xyangle;
  }
}else{
  if(y>=0){
    xyangle = 180 - xyangle;
  }else{
    xyangle = 180 + xyangle;
  }
  
zangle = asin(sqrt((x*x+y*y)));



ちなみに上のソースはコンパイルは通りましたが、実機で動かしてないので、あくまで参考程度に考えてください。
ていうか間違ってる気がするのと、0度や90度の場合にどっちの象限に入るかの判定が超適当です。

そういえば、アークタンジェントは90度という答えを返すことは出来ないので(90度の極限にならできますが)、90度丁度が欲しいときは、例えばX軸が0でY軸が正なら90度、といった例外処理を入れた方が良いかもしれません。
それと、上のソースコードは浮動小数点演算使いまくりなのでマイコンには重いかも知れません。
実際に使う前に、処理にかかる時間を測定してみることをオススメします。もし重ければ、例えばアークサインを1万倍して整数にしてから計算に使うといった方法もあります。




おわかりいただけただろうか?

コメントの投稿

非公開コメント

プロフィール

立命館守山高校 Sci-tech部

Author:立命館守山高校 Sci-tech部
立命館守山高校、Sci-tech(サイテック)部のブログです。
研究の途中結果や、日記、連絡事項などを載せていきます。

主な成績
ロボカップジュニアレスキューセカンダリ2010
世界大会5位

やってること
バイオディーゼル燃料を使ったバイオカートの制作
AR(拡張現実)の研究
クルーレスソーラーボート大会への出場
ロボカップジュニアへの出場
レールガン制作
など

リンク
過去ログ
最新記事
最新コメント
アクセスカウンタ
合計閲覧者数

現在の閲覧者数
カテゴリ
検索フォーム
アクセス解析
    ブロとも申請フォーム

    この人とブロともになる

    QRコード
    QR
    上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。