■ホビーロボット部品の製造・販売 
  モータコントローラ、センサ、音声、画像、無線モジュールなど、
■ホビーロボット制作記事のページ (各種マイコン、PCとの接続事例)
■特殊メイク、特殊材料の販売 ※特殊メイクのコーナーはこちらに移りました。
Top(お知らせ) 製品紹介 使い方とサンプルプログラム 通信販売 リンク ロボット掲示板 会社案内
 Easy Robotics for all enthusiastic people!!!  ---HOBBY ROBOT PARTS SHOP ASAKUSAGIKEN---   Since 2003...
スタンプと2軸加速度センサーの接続

初めに

最近は2足歩行ロボットを自作する方が増えてきました。4足以上のロボットとは違い、2足の場合は歩行時に倒れないように制御する必要があります。歩行パターンで倒れないようにする方法もありますが、外部からの予期しない力が加わった時には対処ができません。自立してバランスを取るためには外部から加わる力を測定するセンサーか、自分がどのぐらい傾いているかを測定するセンサーが必要になります。
今回ご紹介するものは2軸の加速度センサーです。センサーを傾けると重力により加速度が加わったことと同じ結果が得られ、加速度を角度に変換することにより傾きを計測することもできます。


加速度センサー

ラジコンヘリコプター又は飛行機用のパーツでジャイロセンサーなるものがあります。ラジコンヘリなどは飛んでいる時に風の影響をうけますが、操縦者には遠くにいるヘリがどちらから風を受けているかわかりません。この時にジャイロセンサーがヘリ自体の動きを検出してRCサーボを補正する信号を生成します。このようにジャイロセンサーは外部から加わる力を検出し、自動で補正をかける役割をしますが、1軸のもので一万円〜、2軸になると3万円〜と高価です。また、出力はRCサーボを補正するための波形が出るのでマイコンなどでセンサー出力を取ろうとする場合ジャイロセンサーの出力波形を知っている必要があります。
ジャイロセンサーの中には角速度センサー素子又は加速度センサー素子が入っています。これに外部回路を追加してRCサーボを補正しますが、市販のセンサー素子は5V駆動のものもありますので出来れば余分な回路は取り除いて安価にセンサーを作りたいものです。


重力と傾斜

加速度センサーを使ってどの方向にどれだけの速さで動いたかを測定することも
出来ます。加速度というのは物体に加わる力によって変わります。地球には重力というものがありますが、重力も物体に加える力です。よってこのセンサーを傾けると重力によりセンサーの加速度検知部分に力が加えられますのでこの力を測定してどのぐらい傾斜しているかを測定することも出来ます。これは2足歩行ロボットの作成に有効なセンサーと言えるでしょう。


2軸加速度センサーを使って加速度と傾斜を測定する
当ページではMemsic2125を使い、必要な抵抗とDIP型ピンを取り付けた「2軸加速度センサー」を販売しています。 ここではこのセンサーとBasicStampIIを使って傾きの測定をする方法を紹介します。



このセンサーから出力される波形は先に述べた波形がそのまま出ますのでStampのPULSIN命令を使ってHighの長さを調べます。PULSIN命令は PULSIN <ポート番号>,<状態>,<結果格納変数名> という記述になります。状態はHighの時間を測定するなら"1"、Lowを測定するなら"0"を記述します。この命令は指定した状態のパルス幅を結果を2μ秒単位で出力しますので、0Gの場合はT1が50%のデューティーとなり、T2が10m秒なのでT1は50%の5m秒となり5m秒を2μ秒で割ると「2500」という数値が変数に格納されます。
実験用に下記のように配線をしました。



赤が+5V、黒がGND、黄色線がXで白がYの出力です。Stamp側は実験をしやすくするためにキャリアボードを使用していますが、9V電池の接続と、PCへの接続部分を自作すればStampと直接接続することが出来ます。PCとの接続については「Stampマイコンの開発方法」を参照願います。

出力パルスの長さを測定するプログラムは次のようになります。

Xin CON 8
Yin CON 9

pulse VAR word
Xvalue VAR word
yvalue VAR word

LOOP:

PULSIN Xin, 1, Xvalue
PULSIN Yin, 1, Yvalue

DEBUG CLS
DEBUG "Xvalue=", SDEC Xvalue,CR
DEBUG "Yvalue=", SDEC Yvalue,CR
PAUSE 100

GOTO LOOP

RH10-5

上記プログラムはLOOP内で100m秒ごとに値を計測するプログラムです。
温度によってT2の時間が変わりますので正確には2500にはなりませんでしたが2490前後の数値が出ています。温度が低いと低く、高いと高い数値になるようです。センサーを動かすと、動かした瞬間にこの数値が変わります。
Memsic2125は1Gでデューティーが12.5%変化しますので加速度(G)を求める式は次のようになります。


この式を使って上記プログラムで出た数値を加速度の単位Gに変換するには次のようになります。単位をGにすると小数点以下の数値になりStampでは小数点の計算はあまり得意ではないのでmG(ミリジー)に直しています。

mG=((xvalue / 5) −500)× 8

Xvalueが2500の時に2500/5=500となり、500を引いていますのでmGは0になります。上記プログラムのDEBUG命令(PCに結果を表示する命令)を

DEBUG "Xvalue=", SDEC ((Xvalue/5)-500)*8,CR
DEBUG "Yvalue=", SDEC ((Yvalue/5)-500)*8,CR

よって、このようにするとmG単位での結果が表示されます。 今、実験をしている温度環境で2500と出るべきところが2490と出ていましたので補正のために値10を足した結果が次のようになります。この補正値は環境によって変える必要がありますが、たいていは1%以下の誤差なので精密な制御を行う必要の無い場合には補正は不要です。
'補正して結果に10を足したところ
DEBUG "X(mG)=", SDEC ((Xvalue+10/5)-500)*8,CR
DEBUG "Y(mG)=", SDEC ((Yvalue+10/5)-500)*8,CR

RH10-7

次に傾斜の測定を考えます。
重力とセンサーから出力される加速度の値は比例関係ではなく、下記のような関係になります


傾斜(°) X出力(G) Y出力(G)
90 1.000 0.000
85 0.996 0.087
80 0.985 0.174
70 0.940 0.342
60 0.866 0.500
45 0.707 0.707
30 0.500 0.866
20 0.342 0.940
10 0.174 0.985
0.087 0.996
0.000 1.000


出力から傾斜確度を求めるにはアークサイン関数が必要になりますが、Stampの場合にはサインとコサイン関数しかありません。しかし、アメリカのTracyAllen博士が作ったアークサインを求めるコードがありますのでこれを使用して作成したプログラムが次のようになります。

Xin CON 8
Yin CON 9
HiPulse CON 1
pulse VAR word
xmG VAR word
ymG VAR word
disp VAR Byte
angle VAR Byte
xTilt VAR Word
yTilt VAR Word

LOOP:
GOSUB Read_G_Force
DEBUG CLS
DEBUG "X(mG)=", SDEC xmG,CR
DEBUG "Y(mG)=", SDEC ymG,CR
GOSUB Read_Tilt
DEBUG "X_Tilt(do)=", SDEC xTilt,CR
DEBUG "Y_Tilt(do)=", SDEC yTilt,CR
PAUSE 100
GOTO LOOP

Read_G_Force:
PULSIN Xin, Hipulse, pulse xmG = ((pulse / 5) - 500) * 8
PULSIN Yin, Hipulse, pulse ymG = ((pulse / 5) - 500) * 8
RETURN

Read_Tilt:
GOSUB Read_G_Force
disp = ABS xmG / 10 MAX 100
GOSUB Arcsine
xTilt = angle * (-2 * xmG.bit15 +1)
disp = ABS ymG / 10 MAX 100
GOSUB Arcsine
yTilt = angle * (-2 * ymG.bit15 +1)
RETURN

Arcsine:
GOSUB Arccosine
angle = 90 - angle
RETURN

Arccosine:
disp = disp */ 983 / 3 angle = 63 - (disp / 2)
LOOP2:
IF (COS angle <= disp) THEN EXIT
angle = angle + 1
GOTO LOOP2
EXIT:
angle = angle */ 360 RETURN


Y軸方向に45°傾けたとき。Yが重力を表し、Tiltが角度を表します。重力が720mGつまり0.720Gで傾斜角度が45°に表示されました。


Y軸を垂直にしたところです。傾斜角度が90°とでました。


今度は反対方向に傾けます。角度の数値はマイナスになります。


垂直にすると、Gが−1.040Gとほぼ1Gになりました。角度も−90°とでます。

写真ではY軸の傾きしか表していませんが、X軸も同様の結果になりました。計算結果を1°単位(Gでは8mG単位にまるめている)でまるめていますので、温度によるパルス幅の変化については無視できるものになっています。

Stampではないマイコンを使用している方は当ページの「ロボット作成入門」にStampBASICのコマンド解説がありますのでそちらを参照にしながら上記プログラムを解析してください。基本的には、入力されたパルス幅を測定する機能があるマイコンであればこのセンサーを使って2軸の傾きを計測することが出来ます。



2003年4月12日
(C)Copylight 2003. 有限会社浅草ギ研 | 通信販売の法規(訪問販売法第8条)に基づく通信販売業者の表示