Octaveによる慣性計測の計算


  1. 基本的な慣性計測
    1. データの読み込みとスケーリング
    2. 必要なデータ範囲の切り出し(もし必要なら)
    3. ジャイロのゼロ点補正
    4. 角速度を数値積分して姿勢角(4元数)を求める
    5. 加速度の座標変換
    6. ダウンサンプルとフィルタリング
    7. 加速度の数値積分による位置推定
    8. 画面表示とファイルへの保存
    9. Gnuplotによるグラフ描画
  2. GPSとの複合計測(カルマンフィルタ)


基本的な慣性計測



慣性計測とは,慣性センサ(加速度センサ,ジャイロスコープ)の信号を基に,位置を計測する手法である.

ここでは,MEMSの加速度センサ・ジャイロスコープの信号を用いて,慣性計測をしてみる.



ハードウェア,装置のファームウェアはここ



得られた結果,時刻と加速度,角速度のデータは下のような形式でファイルに書かれているとする.

test3.dat


時刻(10us単位), X加速度,Y加速度,Z加速度,温度,X角速度,Y角速度,Z角速度
2020981,-513,1012,16724,4252,-352,215,-217
2260231,-512,820,16724,4212,-32,13,0
2260281,-512,820,16724,4212,-64,-253,236
2260331,-512,848,16688,4132,-320,-23,222
2260381,-512,848,16688,4132,-352,-54,226

    :

ちなみにこのデータは,慣性センサ(MPU9250,InvenSense)を用いた結果である.
時刻のデータは10us単位で書かれているので,この数値に10e-6を掛けると秒になる.
加速度,温度,角速度のデータは,センサのデジタル出力値がそのまま書かれている.
なので,適当なスケールファクタを掛けて,実単位に直す必要がある.
スケールファクタはデータシートに書いてあるので,それを使用する.

まずは,必要なユーティリティ関数(MyUtils, 4元数の計算用)をOctaveのカレントディレクトリ下にコピーする.


Octaveでは以下のように処理していく ( IMU.m


データの読み込みとスケーリング


clear;

pkg load signal;  #signalパッケージをロードする(フィルタリングに使用)

addpath("./MyUtils");  #コピーしたユーティリティ関数にアクセスできるようにパスを通す

din = dlmread("test3.dat", ",");   #データファイルを読み込む(カンマ区切り)

t_scale = 10e-6;  #時間のスケール値

SF_ACC = 16384;  #加速度のスケールファクタ   (digit/g) 
SF_GYRO = 131;    #ジャイロのスケールファクタ  (digit/dps)  

acc_scale = 1.0/SF_ACC;
gyro_scale = 1.0/SF_GYRO;
temp_scale = 1.0/333.87;

#読み込んだデータをスケーリングして,実単位の量に直す.
#また, 時間,加速度,温度,ジャイロそれぞれの変数に分離する.
tt_in=din(:,1)*t_scale;
acc_in=din(:,2:4) * acc_scale * 9.80665;   # Unit : m2/s
temp_in=din(:,5) * temp_scale + 21;  
gyro_in=din(:,6:8) * gyro_scale * pi/180.0;   #  rad/s

in_nums = size(din,1);  #読み込んだデータ点数



必要なデータ範囲の切り出し(もし必要なら)

 
plot(tt_in, acc_in)

等してみて,おかしなデータがある場合は,これを除去する.
特に開始直後におかしなデータが入ることがある.
(通信器に残っていた過去のデータが送信されたりする. ソフトウェアの改良で除去できるけど,面倒くさいので,いまはここで除去する)

num_start = 100;  #切り出すデータ範囲の開始データ番号
num_end   = in_nums; #切り出すデータ範囲の終了データ番号

tt = tt_in(num_start:num_end);  #各データを切り出して,新たな変数に保存する.
acc_meas = acc_in(num_start:num_end, :);
temp_meas = temp_in(num_start:num_end);
gyro_meas = gyro_in(num_start:num_end, :);

nums = size(tt,1);  #切り出し後のデータ点数



ジャイロのゼロ点補正


ジャイロは,静止時にも非零の出力を持っている(バイアス).
これを除去するために,慣性計測の初期1秒程度は装置を静止させておき,バイアスを計測する.
このバイアスを全データから差し引くことで,ゼロ点を合わせる.


dt=(tt(nums)-tt(1))/(nums-1)   #計測データの時間間隔(の平均値)
fs=1.0/dt;    #サンプリングレート(の平均値)

dtt=diff(tt);   #時刻データの数値差分を計算する

t_init = 1.0;  #ゼロ点を計測するために,装置を静止させておいた時間(秒)を設定

gyro_zero=mean( gyro_meas(1:floor(t_init*fs),:)); #上の時間分のデータを平均する.これがジャイロのバイアス値になる.

gyro_meas = gyro_meas - ones(nums,1)*gyro_zero; #バイアス値を全データから差し引く



角速度を数値積分して姿勢角(4元数)を求める

姿勢を表すにはオイラー角等様々な方法があるが,ここでは,数値計算上扱いやすい4元数を用いて姿勢を表す.


q0=[1, 0, 0, 0];   #単位クオータニオン

d_th(2:nums,:) = gyro_meas(1:nums-1,:) .* dtt;   #ジャイロの加速度とサンプリング間隔から,1サンプル区間における角度増分(X,Y,Z軸周り)を計算する

#この角度増分から, クオータニオンの変化分を計算する
ddq0(2:nums,1) = cos( d_th(2:nums,1)*0.5 ) .* cos( d_th(2:nums,2)*0.5 ) .* cos( d_th(2:nums,3)*0.5  ); #クオータニオンの第0成分
ddq1(2:nums,1) = sin( d_th(2:nums,1)*0.5 ) .* cos( d_th(2:nums,2)*0.5 ) .* cos( d_th(2:nums,3)*0.5  );  #クオータニオンの第1成分
ddq2(2:nums,1) = cos( d_th(2:nums,1)*0.5 ) .* sin( d_th(2:nums,2)*0.5 ) .* cos( d_th(2:nums,3)*0.5  );  #クオータニオンの第2成分
ddq3(2:nums,1) = cos( d_th(2:nums,1)*0.5 ) .* cos( d_th(2:nums,2)*0.5 ) .* sin( d_th(2:nums,3)*0.5  );  #クオータニオンの第3成分

ddq(2:nums,:) = [ddq0(2:nums,1), ddq1(2:nums,1), ddq2(2:nums,1), ddq3(2:nums,1)];  #求まった,1サンプル区間でのクオータニオンの微小変分 (時刻"2"〜最後まで)
ddq(1,:) = [0, 0, 0, 0]; 

q_imu(1,:) = q0;  #とりあえず初期姿勢は単位クオータニオンとする.
for i=2:nums
    q_imu(i,:) = q_mul( q_imu(i-1,:), ddq(i,:));       #時刻 "2”〜"最後" まで, 姿勢(クオータニオン)を計算する(正確には座標変換のための4元数) .これが角速度を数値積分して角度を求めることに相当.
end



加速度の座標変換

加速度センサで求められる加速度値は,加速度センサの座標(≠固定座標)の値である.
慣性計測で知りたいのは,地球(もしくは空間)に固定された座標である.
そこで,上で求めた姿勢(4元数)を使って加速度を座標変換して,固定座標での加速度値を得る.

次に,重力を差し引く.
ジャイロのゼロ点と同様に,初期の静止期間の加速度値の計測値を重力として用いる.


a_imu_0 = rot_vec_q( acc_meas, q_imu);   #姿勢(q_imu)を使って,加速度ベクトル(acc_meas)を座標変換する.

#初期重力方向の取得
a_grav_0=mean( a_imu_0(1:floor(t_init*fs),:));  #重力を取得する. 初期静止期間( t_init)の加速度値を重力とする.

a_imu = a_imu_0 - ones(nums,1)*a_grav_0;  #重力を差し引く.



plot(tt, acc_meas)
plot(tt, a_imu_0)
plot(tt, a_imu)

等してみると,演算結果を視覚的に見ることができる.


ダウンサンプルとフィルタリング


ジャイロのバイアス変動,スケールファクタの誤差により,求まった姿勢角は誤差を持つことになる.
姿勢角に誤差があると,重力の差し引き時に "(重力)x(姿勢誤差のsin, cos)" 分だけ,本来存在しないはずの加速度が生じてしまう.
仮に,0.1°誤差があったとすると,0.017 m2/s の加速度が生じることになる.
この加速度が一定であると仮定すると,10秒後には0.85[m]もの変位を生じることになる.

よって,このような姿勢誤差の影響を減らすために,フィルタリングを行う.
バイアス変動によるドリフトは一般的に非常にゆっくり起こるため,高域通過フィルタ(HPF)で極低周波成分をカットすれば,バイアス変動の影響を除去できる.
(ただし,このフィルタのカットオフ周波数よりも低い信号(=非常にゆっくりした動き)も除去されてしまう. なので,カットオフ周波数は適切に選ぶ必要がある.)

フィルタリングにはデジタルフィルタを用いる.
非常にゆっくりした成分のみを除去したいが,このような極端なフィルタは構成が難しい.
( 例えば,サンプリング周波数 fs = 10kHz で, fc = 0.1 Hzのカットオフ周波数を実現するためには,規格化周波数として, fc/fs = 0.00001 という極端に小さい値を設定する必要がある)

そこで,まずダウンサンプルでデータ点数を減らした後,”そこそこの”フィルタを適用する.
例えばダウンサンプル比を ds = 100にすれば, 規格化周波数は fc/fs*ds = 0.001にできるので,フィルタが実現しやすい.



ds_ratio = 20;    # Down sampling ratio

f_cut = 0.3;    # Filter Cutoff freq [Hz], この周波数よりもゆっくりした成分を除去する.
fn_cut = f_cut / (fs/ds_ratio); # Normalized Freq;

#Downsampling
tt_ds = downsample(tt, ds_ratio);                   # ダウンサンプルを実行する (時刻に対して)
a_imu_ds = downsample(a_imu, ds_ratio);  # ダウンサンプルを実行する (加速度に対して)

#デジタルフィルタの定義,様々なフィルタが設定できる. 詳しくはhelpやマニュアル等を参照
#filt_b=fir1(4096, fn_cut, "high"); filt_a=1;      #F IRフィルタ
#[filt_b, filt_a]=cheby1(4, 2, fn_cut, 'high');     #IIR (チェビチェフフィルタ)
#[filt_b, filt_a]=cheby2(4, 30, 0.0001, 'high');  #IIR( 逆チェビチェフフィルタ)
[filt_b, filt_a]=butter(4, fn_cut, 'high');               #IIF(バタワース)

#設計したフィルターの特性を見るときは,下をコメントアウト.
#もし,あまりにも規格化周波数が小さすぎれば,所望の特性が得られないので,その場合は,ダウンサンプル比を変えたりしてみる.

# freqz(filt, 1, 1000, "half", 1/(tt(2)-tt(1))) 

a_filt_0 = filter(filt_b, filt_a, a_imu_ds);   #フィルタ−を信号に適用する
a_filt = a_filt_0;



ちなみに,装置を回転させた場合には,ジャイロのスケールファクタが不正確だと,やはり姿勢角に誤差が生じる.
この場合は,姿勢角の誤差が持つ周波数成分は,必ずしも極低周波ではないので,この方法では除去できない.


であるので,ジャイロのスケールファクタの安定性,直線性はとても重要な性質である.


加速度の数値積分による位置推定

あとは,加速度を2回積分すれば,位置が求まる.
ためしに,フィルタを使った場合( x_filt )と使わない場合 ( x_imu ) を計算してみる.


#フィルタを使わない場合のIMU計算
v_imu = cumtrapz(tt_ds, a_imu_ds, 1);
x_imu = cumtrapz(tt_ds, v_imu, 1);

#フィルタを使った場合のIMU計算
v_filt = cumtrapz(tt_ds, a_filt, 1);
x_filt = cumtrapz(tt_ds, v_filt, 1);




画面表示とファイルへの保存


figure 1; clf;  
plot(x_imu(:,1), x_imu(:,2) , "-*");  #フィルタ−を使わない場合の慣性計測の結果(X-Y)
title "IMU result (w/o filter)";

figure 2; clf;
plot(x_filt(:,1), x_filt(:,2), "-*");    #フィルタ−を使った場合の慣性計測の結果(X-Y)
title "IMU result (with HPF)";

figure 3; clf;
plot3(x_filt(:,1), x_filt(:,2), x_filt(:,3), "-@"); axis equal;   #3次元プロット
xlabel "X"; ylabel "Y"; zlabel "Z";
view([-1,-1,1]);

dlmwrite("NoFilt-IMU.dat", [tt_ds,x_imu], ",");   #データの書き出し
dlmwrite("Filtered-IMU.dat", [tt_ds,x_filt], ",");



Gnuplotによるグラフ描画

ここも参照


gnuplot で以下を実行

set xrange [-0.4:0.4]
set yrange [-0.4:0.4]
set zrange [-0.4:0.4]
set xlabel "X"
set ylabel "Y"
set zlabel "Z"

set datafile separator ","
splot "Filtered-IMU.dat" u 2:3:4 w lp



プロットしたものを画像として保存するなら,

set terminal postscript eps enhanced color size 3.0,3.0
set output "IMU-result.eps"
replot


等でできる(EPSの場合).
png, jpeg等もできる.


set terminal jpeg size 800,800
set output "IMU-result.jpg"
replot
   
        
 


アニメーションさせることもできる.
やり方は,このページを参照




GPSとの複合計測(カルマンフィルタ)


Comming Soon(?)