6軸IMU~拡張カルマンフィルタ

概要

6軸IMU(慣性センサ)3軸加速度センサ + 3軸ジャイロセンサ。3軸加速度センサにてセンサローカル座標系(センサ座標系と称す)の3軸における重力加速度gの分量が出力され、観測方程式によりワールド座標系から見たピッチ・ロール角が計算できる。3軸ジャイロセンサからセンサ座標系の3軸まわりの角速度が出力され、状態方程式によりワールド座標系から見たピッチ・ロール角・ヨー角が計算できる。また、誤差(ノイズ)が存在し、そしてドリフトが蓄積していくので、補正の必要がある。補正には、拡張カルマンフィルタ状態空間モデルが利用できる。

3軸加速度センサ

3軸加速度センサのローカル座標のX軸・Y軸・Z軸とも回転する場合

3軸加速度計のX軸・Y軸・Z軸とも回転する
3軸加速度計のX軸・Y軸・Z軸とも回転する

回転行列

センサ座標系⇒ワールド座標系の回転行列R(ℇ)は、オイラー角がXYZ軸まわりの回転順として、
$$R(ℇ) = R_z(ψ)R_y(θ)R_x(ϕ)$$
$$R(ℇ) = \begin{bmatrix}cosψ&-sinψ&0\\sinψ&cosψ&0\\0&0&1\end{bmatrix}\begin{bmatrix}cosθ&0&sinθ\\0&1&0\\-sinθ&0&cosθ\end{bmatrix}\begin{bmatrix}1&0&0\\0&cosϕ&-sinϕ\\0&sinϕ&cosϕ\end{bmatrix}$$

観測方程式

$$A_{world} = \begin{bmatrix}0&0&-g\end{bmatrix}^T , A_{sensor} = \begin{bmatrix}Ax&Ay&Az\end{bmatrix}^T$$
$$A_{world} = R(ℇ)A_{sensor} ⇒ A_{sensor} = R(ℇ)^T A_{world}$$
$$\begin{bmatrix}Ax\\Ay\\Az\end{bmatrix}=\begin{bmatrix}csψcsθ&snψcsθ&-snθ\\csψsnθsnϕ-snψcsϕ&snψsnθsnϕ+csψcsϕ&csθsnϕ\\csψsnθsnϕ+snψsnϕ&snψsnθcsϕ-csψsnϕ&csθcsϕ\end{bmatrix}\begin{bmatrix}0\\0\\-g\end{bmatrix}$$
ただし、cs = cos 、sn = sin
よって、3軸加速度センサの観測方程式は以下の式で表される。また、Ax、Ay、Azは、ヨー角ψと関係ないことに気付き、逆にヨー角ψは、3軸加速度センサで推定できないことが分かる。
$$\begin{bmatrix}Ax\\Ay\\Az\end{bmatrix} = \begin{bmatrix}gsinθ\\-gcosθsinϕ\\-gcosθcosϕ\end{bmatrix}$$

傾斜角度

よって、重力加速度gの分量により3軸加速度計センサの傾斜角度が以下の式で表される。
$$\begin{bmatrix}ϕ\\θ\end{bmatrix} = \begin{bmatrix}tan^{-1} -\frac{Ay}{Az}\\tan ^{-1}\frac{Ax}{\sqrt{Ay^2 + Az^2}}\end{bmatrix}$$
ただし、ϕ、θは、それぞれワールド座標系から見たロール・ピッチ角と呼ばれる。ジンバルロック現象を避けたいので、ϕ ≠ ±90°。
なお、ワールド座標系のヨー角は、3軸加速度センサだけでは推定できないゆえに、3軸ジャイロセンサまたはコンパスセンサが必要になる。しかし、センサ融合手法として活用された拡張カルマンフィルタは、複数のセンサとも推定できる角度しか求めないので、ヨー角は、以下の拡張カルマンフィルタの状態方程式から除外される。ヨー角の求め方について、角速度ωzの時間積分して単独に求める。ヨー角の精度も高めるには、ヨー角を別途観測可能なコンパスセンサを加え、これで9軸IMU = 6軸IMU + 3軸コンパスセンサになる。9軸IMUについては、別途検討してみる。

3軸ジャイロセンサ

coordinate
coordinate

状態方程式

センサ座標系のジャイロセンサ角速度のωx、ωy、ωz(*)が互いに直交して、ワールド座標系のオイラー角の微分(**)が必ずしも直交してないので、(*)と(**)の相互関係を求めるには、オイラー角の微分がワールド座標系の軸まわりの分量に変換する必要がある。
$$R(ℇ)\begin{bmatrix}ωx\\ωy\\ωz\end{bmatrix} = \begin{bmatrix}0\\0\\\dot{ψ}\end{bmatrix}+R_z(ψ)\begin{bmatrix}0\\\dot{θ}\\0\end{bmatrix} + R_z(ψ)R_y(θ)\begin{bmatrix}\dot{ϕ}\\0\\0\end{bmatrix}$$
よって、回転行列を利用して、センサ座標系の角速度から、ワールド座標系のオイラー角の微分が求められる。
$$\begin{bmatrix}\dot{ϕ}\\\dot{θ}\\\dot{ψ}\end{bmatrix} = \begin{bmatrix}1&sinϕtanθ&cosϕtanθ\\0&cosϕ&-sinϕ\\0&−sinϕsecθ&cosϕsecθ\end{bmatrix}\begin{bmatrix}ωx\\ωy\\ωz\end{bmatrix}$$
ただし、ϕ、θ、ψはそれぞれワールド座標系から見たロール・ピッチ・ヨー角、ωx、ωy、ωzはセンサ座標系でのx、y、z軸周りの角速度を表す。ワールド座標系からセンサ座標系への回転はヨー・ピッチ・ロールの順とする。

回転角度での状態方程式の表現

ジャイロセンサの角速度からワールド座標系から見たロール・ピッチ角の計算は以下の式にて表される。
$$\begin{bmatrix}ϕ\\θ\end{bmatrix} = \begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}$$
ただし、ヨー角の積分、加算はここで省略する。
*センサ座標系で3軸加速度センサから傾斜角度と3軸ジャイロセンサから回転角度を求めて、静的重み付け(経験値)で加算するセンサ融合の手法がある。出典4.を参照する。

誤差の補正

加速度センサの精度が悪く、とくに運動中の場合、但し加速度センサの誤差が蓄積しない。ジャイロセンサの精度が良く、但しドリフト現象が起きる。ドリフトにより、誤差がじわじわと蓄積していく。それで、補正しないといけない。加速度センサにジャイロセンサの出力値を組み合わせて、静的重み係数(経験値)を付ける相補フィルタに対して、時系列分析から異なるセンサの出力値に信頼性に関わる動的重み係数を付ける、いわゆるカルマンフィルタ(Kalman Filter)の再帰的アルゴリズム(手法)がある。フィルタとは誤差(ノイズ)をフィルタリングする意味合いがある。性能からみれば、論理的にカルマンフィルターが良いといわれる。

カルマンフィルタ

wikipedia: カルマンフィルタ (Kalman Filter、KFと略す) は、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルタの一種である。この解釈では理解できたというわけではなく、以下カルマンフィルター、拡張カルマンフィルタで状態を推定してみる。

状態空間モデル

状態方程式、観測方程式は以下のとおり。
$$\begin{eqnarray*}&& x_t = F_{t-1}(x_{t-1}) + w_{t-1}\\&& y_t = H_t(x_t) + v_t\end{eqnarray*}$$
ただし、F(・)、H(・)は実装に際して、3軸加速度センサ、3軸ジャイロセンサに掲載した状態方程式、観測方程式のとおりで利用する。システム誤差、観測誤差の分布は期待値が0の正規分布(ガウス分布)にする。
$$\begin{eqnarray*}&& w_t \in N(0,Wt)\\&& v_t \in N(0,Vt)\end{eqnarray*}$$

パラメータ初期化

$$\begin{eqnarray*}&& \hat{x}_0 = E(x_0)\\&& \hat{P_0} = E((x_0-\hat{x_0})(x_0-\hat{x_0})^T)\end{eqnarray*}$$

状態値、共分散行列予測ステップ

$$\begin{eqnarray*}&& \bar{x_t} = F_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + W_{t-1}\end{eqnarray*}$$

カルマンゲイン、観測値、共分散行列更新ステップ

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + W_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$

拡張カルマンフィルタ

拡張カルマンフィルタ(Extended Kalman Filter、EKFと略す)は、非線形フィルタリングである。前述した状態方程式、観測方程式より、以下の状態空間モデルのf(⋅)が非線形であり、6軸IMU(慣性センサ)の角度計算に拡張カルマンフィルタが適用される。非線形であるf(⋅)の1次微分を線形化とし、カルマンフィルタのアルゴリズムが適用可能となる。しかし、f(⋅)の微分ではf(⋅)の一部しか表現できず、この線形化処理(1次微分)に誤差を大きく招く場合がある。
$$\begin{eqnarray*}&& \hat{F}_t =\frac{\partial f_t(x)}{\partial x}|_{x=\hat{x}_{t-1}}\end{eqnarray*}$$

状態空間モデル

$$\begin{eqnarray*}&& x_t = \hat{F}_{t-1}(x_{t-1}) + w_{t-1}\\&& y_t = H_t(x_t) + v_t\end{eqnarray*}$$

パラメータ初期化

$$\begin{eqnarray*}&& \hat{x}_0 = E(x_0)\\&& \hat{P_0} = E((x_0-\hat{x_0})(x_0-\hat{x_0})^T)\end{eqnarray*}$$

状態値、共分散行列予測ステップ

$$\begin{eqnarray*}&& \bar{x_t} = \hat{F}_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = \hat{F}_{t-1}\hat{P}_{t-1}\hat{F}_{t-1}^T + W_{t-1}\end{eqnarray*}$$

カルマンゲイン、観測値、共分散行列更新ステップ

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + W_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$

6軸IMUへの実装

coming soon…

出典

1.wikipedia: カルマンフィルタ、オイラー角
2.Analog Device AN-1057、アプリケーション・ノート、加速度センサーによる傾きの検出、著者:Christopher J. Fisher
3.マルチボディダイナミクスの基礎―3次元運動方程式の立て方、著者:田島 洋
4.センサ融合を加速度センサやジャイロスコープに適用、著者:Bonnie Baker
5.基礎からわかる時系列分析 Rで実践するカルマンフィルタ・MCMC・粒子フィルタ、著者:萩原淳一郎 等

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

ROS・Unity・ロボット・ドローン姿勢制御に関わるクォータニオン

オイラー角による回転行列の表現

coordinate
coordinate

物体を座標系とともにx軸、y´軸、z´軸まわりの順にそれぞれ角度ϕ、θ、ψだけ回転させたときに、物体の位置の変換を表す回転行列は、

euler_angle
euler_angle

と表する。 3つの角度ϕ、θ、ψ をオイラー角と呼ぶ。

しかし、以下のイラストのとおり、オイラー角による姿勢制御の弱点(Gimbal lock)があり、例えば西方向のY軸を90度傾けると、X軸とZ軸が同軸となってしまい、姿勢制御が困難となる。

ジンバルロック
ジンバルロック

このジンバルロックを解消するのがクォータニオンの出番だ。

クォータニオンによる回転の表現

ROSではクォータニオンのq=(x,y,z,w)を虚数形式で

q=ix+jy+kzq=ix+jy+kz+W

と表する。原点を通す回転軸を表す単位ベクトルa=(ax,ay,az)で、この回転軸から物体軸の回転角度がθの場合は、クォータニオンは

(x,y,z,w)=(ax*sin(θ/2), ay*sin(θ/2), az*sin(θ/2),cos(θ/2))

となる。

以下の例では、平坦地面にて辺長1mの正方形に沿って自律移動ロボット(クローラ)に走行させて、動作を確認する。

※ROSでは、ロボットの位置&姿勢の表現について、ロボットの中心(センタ・ポイント)または、ロボットにある他のポイントの位置はx,y,z(m)、姿勢はそのポイントのクォータニオンにて表する。

from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
...
locations['square_vertex_1'] = pose = Pose(Point(1.0,0.0,0.0), Quaternion(0.0,0.0,0.0,1.0))
locations['square_vertex_2'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_3'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
locations['square_vertex_4'] = pose = Pose(Point(0.0,1.0,0.0), Quaternion(0.0,0.0,0.707,0.707))
...

クォータニオンからオイラー角へ変換する

geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;    
tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));    
double yaw, pitch, roll;    
mat.getEulerYPR(yaw, pitch, roll);

オイラー角からクォータニオンへ変換する

tf::Quaternion q;
q.setRPY(Out_X, Out_Y, Out_Z);
sensor_msgs::Imu imu_data;
imu_data.orientation.x=q[0];
imu_data.orientation.y=q[1];
imu_data.orientation.z=q[2];
imu_data.orientation.w=q[3];

以上

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

3DR Pixhawk Mini フライトコントローラー キット

3DR Pixhawk Mini フライトコントローラー キット
3DR Pixhawk Mini フライトコントローラー キット

概要

3DR Pixhawk Mini Autopilot & Micro M8N GPS コンパス付き & PM06 PDBボードのフライトコントローラーキットです。3DR Pixhawk MiniオートパイロットはPixhawkの次世代進化バージョンです。 元のPixhawkのサイズの約1/3で、より強力なプロセッサとセンサーを備えています。PPX4オープンハードウェアプロジェクトに基づいており、PX4フライトスタック用に最適化されています。

主な仕様

メイン・プロセッサ

・STM32F427 Rev 3

IOプロセッサ

・STM32F103

センサ

・Accel / Gyro / Mag:MPU9250
・Accel / Gyro:ICM20608
・気圧計:MS5611

電圧定格

・パワーモジュール出力:4.1~5.5V
・最大入力電圧:45V(10S LiPo)
・最大電流検出:90A
・USB電源入力:4.1~5.5V
・サーボレール入力:0~10V

サイズ・重量

・38x43x12mm
・15.8g

GPSモジュール

・GNSS受信機:ublox Neo-M8N
・コンパスHMC5983
・重量:22.4g
・サイズ:37x37x12mm

インタフェース

・1×UARTシリアルポート
・Spektrum DSM / DSM2 / DSM-X?衛星対応
・フタバSBUS?対応
・PPM合計信号入力
・I2C
・CAN
・ADC
・内蔵マイクロUSBポート

開封動画

販売サイト

3DR Pixhawk Mini フライトコントローラー キット

以上です。

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

低コスト超軽量Benewake LED ライダーDE-LIDAR TF01

Benewake LED DE-LIDAR TF01主な特長

・高感度
・検出範囲:最大10m(@ 90%反射)
・検出周波数:500Hz
・耐環境光:100kLux環境光以下屋外でも動作
・形状:コンパクトサイズ51mm x 36mm x 48mm、軽量<50g
・精度:cmレベル(1%相対誤差)
・ドローン、ロボットに最適

紹介ビデオ

ロボット翔-電子部品ストアより購入可能

DE-LIDAR TF01 単方向ライダー

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください

199gぴったりのドローン ZEROTECH DOBBY ドビー販売中!

ドローン専門メーカーのZEROTECH・ゼロテック社製ドローン DOBBYは、Snapdragon 801クアッドコア2.3GHzのチップ、2GBのRAM、16GBのSSD、Adreno330 GPUとHexagon DSPを搭載。顔の識別、FHDビデオ撮影、スマホアプリによりドロニーも可能。電子3軸ぶれ補正技術(EIS)を採用。技適マークありの製品は9月中以降、日本市場に出荷。プロペラ展開サイズ135mm x 147mm x 36.8mm、プロペラ折りたたみサイズ135mm x 67mm x 36.8mm、規制のない、重さ199g(バッテリー含む)。プロ撮影向けフライトパス、屋外においてGPS+GLONASSダブル衛星ナビゲーション、屋内において光ストリーム、ソナで精確に測位可能、爽快に体験できる。内蔵バッテリーで9分の飛行が楽しめる。スマホアプリ、音声で制御可能。わずか5分間でマスターが可能に。ハイスペックのZEROTECH DOBBYに乞うご期待ください。

折り畳み式、ユニークなデザイン、可愛さが漂う

 折り畳み式、ユニークなデザイン、可愛さが漂うドローン DOBBY
ドローン ZEROTECH DOBBY

ポケットに入れる、スマホサイズ、ハイスペックが凝縮したドローン DOBBY

ポケットドローン DOBBY
ポケット ドローン ZEROTECH DOBBY

発売状況について

海外のサイトより日本の技適マークなしバージョンのDOBBY ドローンが既に発売されているようですが(海外からお届け)、ZEROTECH社セールス担当者のお話しによりますと、技適マークがありのDOBBY ドローンは9月中以降、日本市場に出荷する見通しです。

動画:箱から製品本体まで工夫されたデザイン

技適取得済み日本仕様、型番

ZEROTECH DOBBY STANDARD :D100BS

国内の取り扱い店から買い求める!

9月中以降の発売に向けて、日本国内では、
ZEROTECH DOBBY ドローン 技適マーク付き | ロボット翔-電子部品ストア
で技適マーク、PSEマークがつく日本正式代理店商品を販売しています。ご安心にDOBBY ドローンを飛ばしましょう!

以上

研究開発・検証試作に提案するロボット翔・電子部品ストアにロボット・ドローン関連部品が品揃えています。どうぞご利用ください