加速度センサーから姿勢角算出

みなさん，こんにちは．
おかしょです．

今回はロボットの姿勢を求めたいと思います．
当ブログでは様々なロボットを製作し，自律させることを目的としています．

ロボットを自律させるにはどのようにすればいいのでしょうか．そもそも自律とは何なのでしょうか．
倒立振子ロボットの場合は，倒立状態を維持することが自律への第一歩だと考えています．
そして最終的には，目的の位置を指定してこちらからは何も操作をせずに倒立状態を維持したまま移動させることができれば，自律していると言えると考えています．

それでは，第一歩の倒立状態を維持するにはどうすれば良いでしょう．
ロボットの自立状態を維持するには，ロボットに現在の姿勢が倒立状態からどのくらいずれているかを知らせる必要があります．
つまり，自立させるためにはロボットの姿勢角を求める必要があります．

以前，LSM9DS1というセンサーを使ってデータを取得する記事を書きました．
このセンサーは加速度やジャイロ，地磁気を取得できます．このセンサーのすべての機能を使えば，ロボットの姿勢角をかなり正確に知ることができます．
今回はそのうちの一つの加速度を使って姿勢角を求めていきます．

この記事を読むと以下のようなことがわかる・できるようになります．

加速度から姿勢角を求める方法
加速度センサーの使用方法

この記事を読む前に
加速度センサーとは
姿勢角の表し方
姿勢角の求め方
姿勢角を実際に求める
まとめ
続けて読む
1. 共有:
2. 関連

この記事を読む前に

この記事ではLSM9DS1というセンサーを使用します．

センサーからデータの取得がまだできていない方は以下の記事を先に読んでおくことをおすすめします．

加速度センサーとは

まずは加速度センサーの説明から始めます．

地球上では常に重力加速度がかかっています．数値では約$9.8m/s^{2}$です．
この数値を基準として1Gと表記されることが多いです．
センサーのデータシートを見ても，データをGに変換する方法が記載されていることが一般的です．
加速度センサーは静止している状態では，この重力加速度を観測します．3軸加速度センサーは各軸方向の加速度を観測します．
例えば，z軸を下向きにセンサーを設置すると，センサーから出力されるデータはz軸のみ1Gを示し，他のxやy軸方向の値は0となります．実際はノイズがあるので完全に0とはなりませんが，それに近い値となります．

つまり，加速度センサーは各軸方向にどれだけ重力加速度がかかるのかを観測できます．

姿勢角の表し方

姿勢角の求め方の解説をする前に，姿勢角をどのようにして表すのかを示します．

例えば$\phi=30$度とすると左のようになります．

姿勢角の求め方

車のように2次元で移動する物体の姿勢角は簡単に求めることが可能です．
しかし，飛行する物体の姿勢角を求めるのは少し難易度が上がります．
倒立振子ロボットの場合は，飛行する物体のように3次元の姿勢角は必要ないのですが，ゆくゆくはマルチコプターも製作していきたいと考えているので，3次元の姿勢角を求めていきます．

1軸のみ回転

しかし，いきなり3次元の姿勢角を求めるのは困難なので，簡単な例を使って姿勢角を求めてみます．
まずはy軸方向のみ回転した場合を考えます．

上の図において黒い矢印Gは重力加速度を表しています．
加速度センサーでは，これとは反対向きの加速度を観測します．

重力加速度の向きを加速度センサーで検出する向きに合わせると左の図のようになります．

これを回転後の各軸方向に分解します．

実際に加速度センサーで取得できる値は上の図の$G_{x}$と$G_{z}$になります．
この二つの値から傾き$\theta$を求めます．

ここで，重力加速度Gと回転後の各軸に分解された重力加速度の関係を見ていきます．
すると，それぞれ以下のような式で表すことができます．

$$ G_{x}=G\cdot \sin \theta $$

$$ G_{z}=G\cdot \cos \theta $$

$$ G = \sqrt{G_{x}^{2}+ G_{z}^{2}} $$

このように三角関数によって求められます．
上と真ん中の式を工夫することによって姿勢角の算出が行えます．

上の式を真ん中の式で割ってみます．

$$ \frac{G_{x}}{G_{z}} = \frac{\sin \theta }{\cos \theta } $$

ここで，三角関数の公式より

$$ \frac{\sin \theta }{\cos \theta }=\tan \theta $$

であるから，上式は

\begin{eqnarray} \tan \theta &=& \frac{G_{x}}{G_{z}} \\ \theta &=& tan^{-1} \frac{G_{x}}{G_{z}}\\ \end{eqnarray}

このようにして姿勢角を求めることができます．
倒立振子を直立状態で維持するだけの場合は，この式で求められる姿勢角だけでも可能です．

2軸回転

次に二つの軸が回転した場合の姿勢角の求め方を解説します．

2つ以上の軸を回転させることを考える時は，回転させる軸の順番を定義しておかなければなりません．

回転する軸の順番

なぜなら，回転する軸の順番によって姿勢角が異なるからです．
例えば，回転する軸がx軸とy軸であったとします．まずはx軸→y軸の順番に回転させる時を考えます．

最初の姿勢はこのようになっています．

これをx軸周りに$\phi=90^{\circ}$回転させます．

次にy軸周りに$\theta=90^{\circ}$回転させます．

最終的にこのような姿勢になりました．

次にy軸→x軸の順番に回転させた場合を考えます．

先程と同様に，初期状態からy軸周りに$\theta=90^{\circ}$回転させます．

続いて，x軸周りに$\phi=90^{\circ}$回転させます．

こちらの場合は最終的にこのような姿勢になりました．

同じ角度回転させても，回転の順番を変えることによって姿勢が全く違ったものになることは上の図を見れば明らかです．そのため，姿勢角を求めるためにこの順番は明確に決めておかなければなりません．

この順番は人それぞれ，好みで決めてもかまいませんが，私が専門としている航空工学では3-2-1系と言って，z軸→y軸→x軸の順で回転させるのが一般的となっています．なので，このブログでもこの順番で定義したいと思います．

姿勢角の求め方

前置きが長くなりましたが，本題に入ります．
y軸→x軸の順に回転させた時，加速度センサーから得られるデータによって姿勢角はどのようにして求めることができるのでしょうか．

先程示した例にもあるように，姿勢角というのは初期状態から各軸周りにどれだけ回転したのかを表します．
加速度センサーから得られるデータは，回転した後の各軸方向の加速度になります．
従って，加速度センサーのデータを姿勢角に変換するには，先程のy軸→x軸とは反対方向に変換して求めます．

言葉では理解しづらいと思うので，図で表してみます．
例えば，ロボットの座標軸が以下のような姿勢であったとします．

この座標系の姿勢を，元の初期状態に戻るように回転させていきます．

この姿勢はy軸→x軸の順に回転させてできた姿勢なので，元に戻すにはx軸から回転させます．

x軸に$-\phi$回転させると，以下のような姿勢になります．

次にy軸周りに$-\theta$回転させます．

初期状態に戻すことができました．
以上のことから，加速度センサーから出力された値を使って，最初にx軸周りの姿勢角，次にy軸周りの姿勢角を求めればいいことがわかります．

姿勢角を求める計算

ここからは，具体的にどのような計算をすればいいのかを解説します．

まず，x軸周りの姿勢角を求めます．
これは，先程の1軸のみ回転させた場合と同様にして求めることができます．

$$ \phi = tan^{-1} \frac{-G_{y}}{G_{z}} $$

次にy軸周りの姿勢角を求めます．
これも1軸のみ回転した場合の姿勢角の求め方を応用することで求めることができます．

この図の$G_{z}^{‘}$というのは先ほどの図のGと一致します．
$G_{x}^{‘}$は$G_{x}$と一致します．
つまり

\begin{eqnarray}
G_{z}^{‘}&=& \sqrt{G_{y}^{2}+G_{z}^{2}} \\
G_{x}^{‘}&=& G_{x}
\end{eqnarray}

ということになります．
以上のことから姿勢角$\theta$は

\begin{eqnarray} \theta&=&tan^{-1} \frac{G_{x}^{‘}}{G_{z}^{‘}}\\ &=& tan^{-1} \frac{G_{x}}{\sqrt{G_{y}^{2}+G_{z}^{2}}}\\ \end{eqnarray}

と求められます．

3軸回転

それでは，最後に3軸全てを回転させた姿勢角を算出してみましょう．

2つの軸を回転させた時の姿勢角を求めることができたので，最後に3つの軸を回転させた姿勢角を求めたいのですが，加速度センサーでは残念ながらできません．

理由は簡単です．回転しても加速度が変化しないからです．
先程，回転の順番を3-2-1系で定義しました．最初の回転方向はz軸です．
センサーのz軸を上向きに設置していた場合，z軸周りに回転させてもセンサーから出力される値は変化しません．
そのため，計算で求めるのは不可能です．

このようなときは，他のセンサーを新たに搭載する必要があります．一般的なものは地磁気センサーやジャイロセンサーが挙げられます．
ロボットの方位角も使用したい場合はそれらのセンサーが必要になることを頭に入れておく必要があります．

方位角の算出について知りたい方はこちらの記事を参照してください．

姿勢角を実際に求める

使用したもの

姿勢角を求める際に必要となったものは，以下のものになります．

Arduino Uno
USBケーブル（書き込み用）
ブレッドボード
ジャンパー線
XBee×2（シリアル通信用）
XBeeピッチ変換基板（3.3V電圧レギュレータ実装）
XBee USBインターフェースボード
USBケーブル（通信用）
電池（9V）
LSM9DS1（9軸センサー）

センサーを動かす際にコードが邪魔になるので，XBeeで無線化を行っています．XBeeがなくても動作の確認はできるので問題はありません．

回路図

今回の動作確認のために使用した回路を以下に示します．

実際に配線するとこのようになります．

加速度センサーから姿勢角を求めるプログラム

今回，加速度センサーとして使用したLSM9DS1のデータを取得し，姿勢角に変換するプログラムを以下に示します．

#include <Wire.h>
#include <math.h>

#define AG_DEVICE_ADDRESS 0x6B
#define M_DEVICE_ADDRESS  0x1E
#define AG_WHO_AM_I       0x0F
#define CTRL_REG1_G       0x10
#define CTRL_REG2_G       0x11
#define CTRL_REG3_G       0x12
#define ORIENT_CFG_G      0x13
#define OUT_T             0x15
#define STATUS_REG_T      0x17
#define OUT_G             0x18
#define CTRL_REG4         0x1E
#define CTRL_REG5_XL      0x1F
#define CTRL_REG6_XL      0x20
#define CTRL_REG7_XL      0x21
#define STATUS_REG_AG     0x27
#define OUT_A             0x28
#define M_WHO_AM_I        0x0F
#define CTRL_REG1_M       0x20
#define CTRL_REG2_M       0x21
#define CTRL_REG3_M       0x22
#define CTRL_REG4_M       0x23
#define CTRL_REG5_M       0x25
#define STATUS_REG_M      0x27
#define OUT_M             0x28

#define PRINT_RATE        40    // PRINT_RATE Hzで出力する

#define ACCELERATION_SENSITYVITY  0.000244  // scale 2g=0.061, 4g=0.122, 8g=0.244, 16g=0.732 mg/LSB
#define MAGNETIC_SENSITYVITY      0.00043   // scale 4=0.14, 8=0.29, 12=0.43, 16=0.58 mgauss/LSB
#define ANGULAR_SENSITYVITY       0.00875   // scale 245=8.75, 500=17.5, 2000=70 mdps/LSB
#define TEMPERATURE_SENSITYVITY   16        // LSB/℃


uint8_t ODR_G = 3;        // Output data rate: 1=14.9, 2=59.5, 3=119, 4=238, 5=476, 6=952　Hz
uint8_t FS_G = 0;         // Full scale: 0=245, 1=500, 2=no, 3=2000 dps
uint8_t BW_G = 0;         // cutoff frequency: table 47
uint8_t LP_mode = 0;      // low power mode: 0=disable, 1=enable
uint8_t HP_EN = 1;        // high pass filter: 0=disable, 1=enable
uint8_t HPCF_G = 1;       // high pass cutoff frequency: table 52
uint8_t Zen_G = 1;        // enable gyro Z:  0=disable, 1=enable
uint8_t Yen_G = 1;        // enable gyro Y:  0=disable, 1=enable
uint8_t Xen_G = 1;        // enable gyro X:  0=disable, 1=enable
uint8_t LIR_XL1 = 1;      // latch interrupt: 0=no request, 1=request
uint8_t FourD_XL1 = 0;
uint8_t SignX_G = 0;      // x axis sign: 0=positive, 1=negative
uint8_t SignY_G = 0;      // y axis sign: 0=positive, 1=negative
uint8_t SignZ_G = 0;      // z axis sign: 0=positive, 1=negative
uint8_t Orient = 0;
uint8_t DEC_ = 0;
uint8_t Zen_XL = 1;       // enable accel Z:  0=disable, 1=enable
uint8_t Yen_XL = 1;       // enable accel Y:  0=disable, 1=enable
uint8_t Xen_XL = 1;       // enable accel X:  0=disable, 1=enable
uint8_t ODR_XL = 3;       // Output date rate: 1=10, 2=50, 3=119, 4=238, 5=476, 6=952 Hz
uint8_t FS_XL = 3;        // Full scale: 0=2, 1=16, 2=4, 3=8 g
uint8_t BW_SCAL_ODR = 1;  // 0=band width determined by ODR, 1=determined by BW_XL
uint8_t BW_XL = 0;        // band width: 0=408, 1=211, 2=105, 3=50 Hz
uint8_t HR = 0;           // high resolution mode: 0=disable, 1=enable
uint8_t DCF = 0;          // low pass cutoff frequency: 0=ODR/50, 2=ODR/9,　1=ODR/100, 3=ODR/400
uint8_t FDS = 0;
uint8_t HPIS = 0;  
uint8_t TEMP_COMP = 0;    // temperature compensation: 0=disable, 1=enable
uint8_t OM = 0;           // operation mode: 0=continuous-conversion, 1=single-conversion,　2=power-down
uint8_t DO = 7;           // Output data rate: 0=0.625, 1=1.25, 2=2.5, 3=5, 4=10, 5=20, 6=40, 7=80 Hz
uint8_t FAST_ODR = 0;
uint8_t ST = 0;
uint8_t FS = 2;           // full scale: 0=4, 1=8, 2=12, 3=16 gauss
uint8_t REBOOT = 0;
uint8_t SOFT_RST = 0;
uint8_t I2C_DISABLE = 0;  // I2c interface: 0=enable, 1=disable
uint8_t LP = 0;           // low power mode: 0=disable, 1=enable
uint8_t SIM = 0;          // SPI interface
uint8_t MD = 0;           // operation mode: 0=continuous-conversion,　1=single-conversion,　2=power-down
uint8_t OMZ = 3;          // z axis operation mode: 0=low power, 1=medium, 2=high, 3=ultra-high performance
uint8_t BLE = 0;
uint8_t FAST_READ = 0;
uint8_t BDU = 0;

// 時間変数
unsigned long InitTime, RunTime, PrintTime, sample;
uint8_t PrintRate = 1000/PRINT_RATE;

int16_t AccX, AccY, AccZ;
float Ax, Ay, Az;
int16_t GyrX, GyrY, GyrZ;
float Gx, Gy, Gz;
int16_t MagX, MagY, MagZ;
float Mx, My, Mz;
int16_t Temp;
float Te;

double phi, theta, psi;


uint8_t I2Cread1byte(uint8_t DeviceAddress, uint8_t SlaveAddress)
{
  Wire.beginTransmission(DeviceAddress); // 通信を開始する
  Wire.write(SlaveAddress);             // レジスターアドレスを送信
  Wire.endTransmission(false);  // リスタートメッセージをリクエストの後に送信し，接続を続ける
  Wire.requestFrom(DeviceAddress, (uint8_t) 1);  // 1 byte分のデータを要求する
  return Wire.read();         // データを返す
}

uint8_t I2CreadMULTIbyte(uint8_t DeviceAddress, uint8_t SlaveAddress, uint8_t num, uint8_t * data)
{
  Wire.beginTransmission(DeviceAddress);
  Wire.write(SlaveAddress|0x80);
  Wire.endTransmission(false);
  Wire.requestFrom(DeviceAddress, num);
  for (uint8_t i=0; i<num; i++)
  {
    data[i] = Wire.read();
  }
  return num;
}

void I2Cwrite1byte(uint8_t DeviceAddress, uint8_t SlaveAddress, uint8_t data)
{
  Wire.beginTransmission(DeviceAddress); // 通信を開始する
  Wire.write(SlaveAddress);             // レジスターアドレスを送信
  Wire.write(data);             // レジスターアドレスを送信
  Wire.endTransmission();       // 通信を終了する
}

bool LSM9DS1setting()
{
  uint8_t who_ag, who_m;

  uint8_t RegisterData = 0;

  RegisterData=ODR_G<<5;
  RegisterData|=FS_G<<3;
  RegisterData|=BW_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG1_G, RegisterData);
  RegisterData=0;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG2_G, RegisterData);
  RegisterData=LP_mode<<7;
  RegisterData|=HP_EN<<6;
  RegisterData|=HPCF_G;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG3_G, RegisterData);
  RegisterData=0;
  RegisterData=Zen_G<<5;
  RegisterData|=Yen_G<<4;
  RegisterData|=Xen_G<<3;
  RegisterData|=LIR_XL1<<2;
  RegisterData|=FourD_XL1;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG4, RegisterData);
  RegisterData=0;
  RegisterData=SignX_G<<5;
  RegisterData|=SignY_G<<4;
  RegisterData|=SignZ_G<<3;
  RegisterData|=Orient;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, ORIENT_CFG_G, RegisterData);
  RegisterData=0;
  RegisterData=DEC_<<6;
  RegisterData|=Zen_XL<<5;
  RegisterData|=Yen_XL<<4;
  RegisterData|=Xen_XL<<3;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG5_XL, RegisterData);
  RegisterData=0;
  RegisterData=ODR_XL<<5;
  RegisterData|=FS_XL<<3;
  RegisterData|=BW_SCAL_ODR<<2;
  RegisterData|=BW_XL;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG6_XL, RegisterData);
  RegisterData=0;
  RegisterData=HR<<7;
  RegisterData|=DCF<<5;
  RegisterData|=FDS<<2;
  RegisterData|=HPIS;
  I2Cwrite1byte(AG_DEVICE_ADDRESS, CTRL_REG7_XL, RegisterData);
  RegisterData=0;
  RegisterData=TEMP_COMP<<7;
  RegisterData|=OM<<5;
  RegisterData|=DO<<2;
  RegisterData|=FAST_ODR<<1;
  RegisterData|=ST;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG1_M, RegisterData);
  RegisterData=0;
  RegisterData=FS<<5;
  RegisterData|=REBOOT<<3;
  RegisterData|=SOFT_RST<<2;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG2_M, RegisterData);
  RegisterData=0;
  RegisterData=I2C_DISABLE<<7;
  RegisterData|=LP<<5;
  RegisterData|=SIM<<2;
  RegisterData|=MD;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG3_M, RegisterData);
  RegisterData=0;
  RegisterData=OMZ<<2;
  RegisterData|=BLE<<1;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG4_M, RegisterData);
  RegisterData=0;
  RegisterData=FAST_READ<<7;
  RegisterData|=BLE<<6;
  I2Cwrite1byte(M_DEVICE_ADDRESS, CTRL_REG5_M, RegisterData);


  who_ag = I2Cread1byte(AG_DEVICE_ADDRESS, AG_WHO_AM_I);
  who_m = I2Cread1byte(M_DEVICE_ADDRESS, M_WHO_AM_I);
  if (who_ag==0x68&&who_m==0x3D)
  {
    return true;
  }
  else
  {
    return false;
  }
}

void GetAccel()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&1)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_A, 6, data)==6)
    {
      AccX = ((data[1]<<8)|data[0]);
      AccY = ((data[3]<<8)|data[2]);
      AccZ = ((data[5]<<8)|data[4]);
    }
  }
}

void GetGyro()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&2)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_G, 6, data)==6)
    {
      GyrX = (data[1]<<8)|data[0];
      GyrY = (data[3]<<8)|data[2];
      GyrZ = (data[5]<<8)|data[4];
    }
  }
}

void GetTemp()
{
  uint8_t data[6];
  if(I2Cread1byte(AG_DEVICE_ADDRESS, STATUS_REG_AG)&4)  // データの更新確認
  {
    if(I2CreadMULTIbyte(AG_DEVICE_ADDRESS, OUT_T, 2, data)==2)
    {
      Temp = (data[1]<<8)|data[0];
    }
  }
}

void GetMag()
{
  uint8_t data[6];
  if(I2Cread1byte(M_DEVICE_ADDRESS, STATUS_REG_M)&8)  // データの更新確認
  {
    if(I2CreadMULTIbyte(M_DEVICE_ADDRESS, OUT_M, 6, data)==6)
    {
      MagX = (data[1]<<8)|data[0];
      MagY = (data[3]<<8)|data[2];
      MagZ = (data[5]<<8)|data[4];
    }
  }
}

void setup() {
  Serial.begin(9600); // シリアル通信のレートを設定
  Wire.begin();         // I2C通信の初期化

  if(LSM9DS1setting())
  {
    Serial.println("LSM9DS1 setting success");
  }
  else
  {
    Serial.println("LSM9DS1 setting failure");
  }

  InitTime = millis();
  sample = 0;
}

void loop() {
  RunTime = millis()-InitTime;
  GetAccel();
  Ax = AccX*ACCELERATION_SENSITYVITY;
  Ay = AccY*ACCELERATION_SENSITYVITY;
  Az = AccZ*ACCELERATION_SENSITYVITY;
  phi = atan2(-Ay,Az);
  theta = atan2(Ax,sqrt(pow(Ay,2)+pow(Az,2)));
  if((millis()-PrintTime)>PrintRate)
  {
    Serial.print("sample,");
    Serial.print(sample);
    Serial.print(",phi,");
    Serial.print((int)(phi*180/PI));
    Serial.print(",theta,");
    Serial.println((int)(theta*180/PI));
    PrintTime = millis();
    sample++;
  }
}

これの動作結果はこちらで紹介しています．

まとめ

この記事では加速度センサーを使用して，姿勢角を算出する方法を解説しました．
一つ一つの説明に図を交えて説明していたので，かなり長くなってしまいました．回りくどい部分もあったと思いますが，理解できたでしょうか．

続けて読む

この記事では加速度センサーを使用しましたが，角速度センサーでも姿勢角の算出は可能です．

以下の記事では角速度センサーを使った姿勢角の算出方法を解説しているので参考にしてください．

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．

それでは最後まで読んでいただきありがとうございました．

この記事を読む前に

加速度センサーとは

姿勢角の表し方

姿勢角の求め方

1軸のみ回転

2軸回転

回転する軸の順番

姿勢角の求め方

姿勢角を求める計算

3軸回転

姿勢角を実際に求める

使用したもの

回路図

加速度センサーから姿勢角を求めるプログラム

まとめ

続けて読む

共有:

関連

コメント