6軸センサを用いた姿勢角の推定
制御システム工学科2年 井土拓海
はじめに
 現在玉乗りロボットを製作中
 制御するのに精度の高い姿勢角が必要
 様々な手法で姿勢角を計算&比較した
玉乗りロボット
制御回路
 6軸センサであるMPU6050を使用
回転行列について
 今回はロールピッチヨーで表現する
 x軸周りの回転 ロール 𝜙
 y軸周りの回転 ピッチ 𝜃
 z軸周りの回転 ヨー 𝜓
 ヨー・ピッチ・ロールの順に回転したものをロールピッチ
ヨー回転したものと定義する
 𝑅 ...
加速度→姿勢角
 並進運動はなく、回転運動のみを仮定
 重力がどの方向を向いているかから計算
 ヨーは求められない
 玉乗りロボットでは使用しない
 地磁気センサを使うと計算できる
ジャイロセンサ→姿勢角
 並進運動してても問題ない
 センサに取り付けたxyz軸周りの角速度が得られる
 絶対座標系におけるロールピッチヨーに変換して積分
問題点
 加速度センサ
 並進運動が入るとずれる
 ジャイロセンサ
 センサの出力にオフセットが大きい
 オフセットが温度で大きく変化する
 積分誤差が発生する
利点
 加速度センサ
 回転運動のみならいい値が得られる
 積分しなくても現在の値のみから計算できる
 ジャイロセンサ
 並進運動に強い
センサフュージョン
 複数のセンサを組み合わせて計測すること
 うまく組み合わせるとそれぞれの利点をいかせる
 例えば
 カルマンフィルタ
 未来を予測&予測値と観測値の差をフィードバック
 相補フィルタ
 センサから得られる値を...
線形カルマンフィルタ
 状態方程式、観測方程式が線形な場合に使える
 システムノイズ・観測ノイズが正規分布に従う(白色雑
音)と仮定
 白色雑音に隠れた真の値を推定する
状態方程式 𝑥 𝑘+1 = 𝐴 𝑘 𝑥 𝑘 + 𝐵 𝑘 𝑢 𝑘 + ...
拡張カルマンフィルタ
 状態方程式・観測方程式が非線形でも微分可能なら使
えるように拡張
 非線形モデルを現在の状態の近傍で逐次線形近似
 線形近似する際にヤコビ行列を用いる
 状態方程式 𝑥 𝑘+1 = 𝑓 𝑥 𝑘, 𝑢 𝑘 + 𝑤 ...
相補フィルタ
 複数のセンサの値を重み付け
 𝑥 𝑠 = 𝐹𝑖 𝑠 𝑥𝑖(𝑠) , ( 𝐹𝑖 𝑠 = 1 , ∀𝑠)
 ジャイロ・加速度から姿勢角を求める場合は、重み付け
を周波数に応じてやるのが良い
 加速度:低周波特性に優れる(LP...
今回行った実験
 MPU6050で計測&SDmicroに保存
 PCで計測値を色々な手法で計算
 ロール角について実験
 使用した計算方法
1. 加速度センサの値のみ
2. 加速度センサの値 + LPF
3. ジャイロセンサの値を積分
...
実験結果①
 ロール回転→x方向並進→y方向並進→ピッチ回転
 赤:加速度のみ 青:加速度+LPF 緑:ジャイロを積分
実験結果②
 赤:線形KF 緑:拡張KF 青:相補フィルタ
まとめ
 加速度+ジャイロの6軸から姿勢角を計算する場合には、
拡張カルマンフィルタ・相補フィルタが良い応答を示す。
 簡略化した相補フィルタがとても実装が楽なので、とりあ
えずで良ければオススメ。
 相補フィルタの重み付けを周波数に応じ...
Upcoming SlideShare
Loading in...5
×

6軸センサを用いた姿勢角の推定

13,720

Published on

3軸加速度センサ&3軸ジャイロセンサをつかって水平に対する傾きを推定する方法を色々比較しまとめたものです。
現在製作中の「玉乗りロボット」に搭載予定

Published in: Technology
0 Comments
17 Likes
Statistics
Notes
  • Be the first to comment

No Downloads
Views
Total Views
13,720
On Slideshare
0
From Embeds
0
Number of Embeds
4
Actions
Shares
0
Downloads
50
Comments
0
Likes
17
Embeds 0
No embeds

No notes for slide

6軸センサを用いた姿勢角の推定

  1. 1. 6軸センサを用いた姿勢角の推定 制御システム工学科2年 井土拓海
  2. 2. はじめに  現在玉乗りロボットを製作中  制御するのに精度の高い姿勢角が必要  様々な手法で姿勢角を計算&比較した
  3. 3. 玉乗りロボット
  4. 4. 制御回路  6軸センサであるMPU6050を使用
  5. 5. 回転行列について  今回はロールピッチヨーで表現する  x軸周りの回転 ロール 𝜙  y軸周りの回転 ピッチ 𝜃  z軸周りの回転 ヨー 𝜓  ヨー・ピッチ・ロールの順に回転したものをロールピッチ ヨー回転したものと定義する  𝑅 = 𝑅 𝑧 𝜓 𝑅 𝑦 𝜃 𝑅 𝑥(𝜙)
  6. 6. 加速度→姿勢角  並進運動はなく、回転運動のみを仮定  重力がどの方向を向いているかから計算  ヨーは求められない  玉乗りロボットでは使用しない  地磁気センサを使うと計算できる
  7. 7. ジャイロセンサ→姿勢角  並進運動してても問題ない  センサに取り付けたxyz軸周りの角速度が得られる  絶対座標系におけるロールピッチヨーに変換して積分
  8. 8. 問題点  加速度センサ  並進運動が入るとずれる  ジャイロセンサ  センサの出力にオフセットが大きい  オフセットが温度で大きく変化する  積分誤差が発生する
  9. 9. 利点  加速度センサ  回転運動のみならいい値が得られる  積分しなくても現在の値のみから計算できる  ジャイロセンサ  並進運動に強い
  10. 10. センサフュージョン  複数のセンサを組み合わせて計測すること  うまく組み合わせるとそれぞれの利点をいかせる  例えば  カルマンフィルタ  未来を予測&予測値と観測値の差をフィードバック  相補フィルタ  センサから得られる値を重み付けして足す  パーティクルフィルタ  モンテカルロ法を用いて、確率的にもっともらしい値を計算する
  11. 11. 線形カルマンフィルタ  状態方程式、観測方程式が線形な場合に使える  システムノイズ・観測ノイズが正規分布に従う(白色雑 音)と仮定  白色雑音に隠れた真の値を推定する 状態方程式 𝑥 𝑘+1 = 𝐴 𝑘 𝑥 𝑘 + 𝐵 𝑘 𝑢 𝑘 + 𝑤 𝑘 観測方程式 𝑧 𝑘 = 𝐻 𝑘 𝑥 𝑘 + 𝑣 𝑘  𝑥 𝑘: 状態, 𝑢 𝑘: 入力, 𝑧 𝑘: 観測値  𝑤 𝑘: システムノイズ,𝑣 𝑘: 観測ノイズ
  12. 12. 拡張カルマンフィルタ  状態方程式・観測方程式が非線形でも微分可能なら使 えるように拡張  非線形モデルを現在の状態の近傍で逐次線形近似  線形近似する際にヤコビ行列を用いる  状態方程式 𝑥 𝑘+1 = 𝑓 𝑥 𝑘, 𝑢 𝑘 + 𝑤 𝑘  観測方程式 𝑧 𝑘 = ℎ 𝑥 𝑘 + 𝑣 𝑘  近似1 𝑥 𝑘+1 = 𝑓 𝑥 𝑘, 𝑢 𝑘 + 𝐴 𝑘 𝑥 𝑘 − 𝑥 𝑘 + 𝑤 𝑘  近似2 𝑧 𝑘 = ℎ 𝑥 𝑘 + 𝐻 𝑘 𝑥 𝑘 − 𝑥 𝑘 + 𝑣 𝑘  𝑥 𝑘: 推定値 事後推定 𝐴 𝑘 = 𝜕𝑓 𝜕𝑥 , 𝐻 𝑘 = 𝜕ℎ 𝜕𝑥
  13. 13. 相補フィルタ  複数のセンサの値を重み付け  𝑥 𝑠 = 𝐹𝑖 𝑠 𝑥𝑖(𝑠) , ( 𝐹𝑖 𝑠 = 1 , ∀𝑠)  ジャイロ・加速度から姿勢角を求める場合は、重み付け を周波数に応じてやるのが良い  加速度:低周波特性に優れる(LPFをかける)  ジャイロ:高周波特性に優れる(HPFをかける)  今回はかなり適当な実装をした  𝑥 𝑘 + 1 = 0.99 𝑥 𝑘 + 𝜔𝑑𝑡 + 0.01𝑥 𝑎(𝑘)  𝜔はジャイロから計算した角速度  𝑥 𝑎 𝑘 は加速度から計算した姿勢角
  14. 14. 今回行った実験  MPU6050で計測&SDmicroに保存  PCで計測値を色々な手法で計算  ロール角について実験  使用した計算方法 1. 加速度センサの値のみ 2. 加速度センサの値 + LPF 3. ジャイロセンサの値を積分 4. 線形カルマンフィルタ 5. 拡張カルマンフィルタ 6. 相補フィルタ(かなり簡単な)
  15. 15. 実験結果①  ロール回転→x方向並進→y方向並進→ピッチ回転  赤:加速度のみ 青:加速度+LPF 緑:ジャイロを積分
  16. 16. 実験結果②  赤:線形KF 緑:拡張KF 青:相補フィルタ
  17. 17. まとめ  加速度+ジャイロの6軸から姿勢角を計算する場合には、 拡張カルマンフィルタ・相補フィルタが良い応答を示す。  簡略化した相補フィルタがとても実装が楽なので、とりあ えずで良ければオススメ。  相補フィルタの重み付けを周波数に応じて変化させると より良い結果が得られそう。(要調査)
  1. A particular slide catching your eye?

    Clipping is a handy way to collect important slides you want to go back to later.

×