年越しテーマとなってしまいましたが、ドローンのシステム基礎の続きです。前回までプロポから、ロール目標速度(Φ target rate)、ピッチ目標速度(θ target rate)、ヨー目標速度(ψ target rate)を設定するようにしていました。
しかし冷静に考えると、プロポでヨーの速度(Z軸回転:ψ)を設定するのはなんとなく分かりますが、ロール速度(X軸回転:Φ)、ピッチ速度(Y軸回転:θ)をプロポで設定するのは至難の業です。
プロポからはロール(Φ)、ピッチ(θ)の目標角度を設定するのが現実的です。ということで、システム図をロール(Φ)、ピッチ(θ)の目標角度を設定するように変更してみました。
基本的な構成は二段階PID制御システムになります。目標角度(target angle)と測定角度(measured angle)から、目標角速度(target rate)をPID制御を使って算出し、それを後段のPID制御ブロックに渡してPWM値を求めます。
ここで、測定角度(measured angle)を算出には、ジャイロセンサーと加速度センサーを用いたセンサーフュージョンを使います。
センサーフュージョンは下記のような算出式と、DCM(Direction Cosign Matrix)もしくはQuaternion、Kalman Filter などのアルゴリズムで構成されています。
センサーフュージョンは、最近は各センサーメーカーから提供されていることが多いので、利用するにあたって詳細まで知る必要はありません。
これで基本的な理論背景はだいたい整ったかな。あとは実践しながら考えていきたいと思います。
p(^ー^)q
しかし冷静に考えると、プロポでヨーの速度(Z軸回転:ψ)を設定するのはなんとなく分かりますが、ロール速度(X軸回転:Φ)、ピッチ速度(Y軸回転:θ)をプロポで設定するのは至難の業です。
プロポからはロール(Φ)、ピッチ(θ)の目標角度を設定するのが現実的です。ということで、システム図をロール(Φ)、ピッチ(θ)の目標角度を設定するように変更してみました。
基本的な構成は二段階PID制御システムになります。目標角度(target angle)と測定角度(measured angle)から、目標角速度(target rate)をPID制御を使って算出し、それを後段のPID制御ブロックに渡してPWM値を求めます。
ここで、測定角度(measured angle)を算出には、ジャイロセンサーと加速度センサーを用いたセンサーフュージョンを使います。
センサーフュージョンは下記のような算出式と、DCM(Direction Cosign Matrix)もしくはQuaternion、Kalman Filter などのアルゴリズムで構成されています。
// preΦg、preθg、preψg は前回の測定角度
// Δt はジャイロセンサーの測定間隔
Φa = arctan(-ay/-az)
θa = arctan(ax/√(ay*ay + az*az))
Φg = preΦg + (Φ measured rate) * Δt
θg = preθg + (θ measured rate) * Δt
ψg = preψg + (ψ measured rate) * Δt
センサーフュージョンは、最近は各センサーメーカーから提供されていることが多いので、利用するにあたって詳細まで知る必要はありません。
これで基本的な理論背景はだいたい整ったかな。あとは実践しながら考えていきたいと思います。
p(^ー^)q
SODIAL(R) GY-521 MPU-60503軸ジャイロ+3軸加速度センサ
- 出版社/メーカー: SODIAL(R)
- メディア: Personal Computers
ArduPilot -UAV Controller w/ ATMega328 [並行輸入品]
- 出版社/メーカー: SHANHAI
- メディア: エレクトロニクス
Building Your Own Drones: A Beginners' Guide to Drones, UAVs, and ROVs
- 作者: John Baichtal
- 出版社/メーカー: Que Publishing
- 発売日: 2015/09/14
- メディア: ペーパーバック