注：この記事の一部の写真は横線で配置されています。表示するのが不便な場合は、記事の最後にある元のテキストを読むをクリックして、Webバージョンにジャンプできます。
1IMU姿勢計算
IMU、または慣性測定ユニットには、通常、3軸ジャイロスコープと3軸加速度計が含まれます。前回の記事MPU6050姿勢計算方法1-DMPは、MPU6050 IMUを簡単に紹介し、内部DMP処理ユニットを介して姿勢計算のクォータニオン結果を直接取得します。この記事では、オイラー角と回転行列を使用して、ソフトウェア計算によってジャイロスコープと加速度計の元のデータの姿勢を解決します。2つの姿勢は補完されてマージされ、最終的にIMUのリアルタイムの姿勢は次のようになります。得られた。
この記事で姿勢計算用に選択した回転シーケンスはZYXです。つまり、IMU座標系は最初に測地座標系と一致し、次に独自のZ、Y、X軸を中心に回転します。ここでは、最初にの名前をカスタマイズします。各回転と記号：
-
IMUのZ軸を中心に回転します：方位角ヨー、回転角y
-
IMUのY軸を中心に回転します：ピッチ角、回転角p
- IMUのX軸を中心に回転します：ロール角列、回転角r
3回転の概略図は次のとおりです。
また、ロール、ピッチ、ヨーの実際の意味は次のとおりです。
2回転行列
回転行列の知識については、3D回転行列の導出とニーモニックおよび3D回転行列の導出とニーモニック-補足を参照してください。ここでは、この記事で使用する必要のある3つの回転行列のみをリストしています。これらの3つに注意してください。回転行列は、座標変換のための回転行列です。
3オイラー角回転
オイラー角回転の知識については、最初にオイラー角回転を参照してください。ここで説明する必要があるのは、この記事で使用する必要のある軸がZYX順に回転することです。
4姿勢角を計算する加速度計
加速度計は、感じる加速度を測定します。静止しているときは、その動きを加速しません。しかし、相対運動の理論によれば、重力加速度の影響により、感じる加速度は重力加速度の反対です。データはまっすぐです。加速度計の英語の略語はaccであり、最初の文字aは加速度計データを表すために以下で使用されます。
加速度は、静止時の重力加速度を使用して、姿勢を計算します。
-
加速度計が水平に配置されている場合、つまりZ軸が直立している場合、Z軸は1gの値（gは重力加速度）を読み取ることができ、X軸とY軸は両方向で0を読み取ることができます。 （0、0、g）として記録できます。
- 加速度計が特定の姿勢で回転すると、重力加速度によって3つの加速度軸に対応するコンポーネントが生成されます。その本質は、新しい加速度計自体の座標系の測地座標系での（0、0、g）の座標です。加速度計によって読み取られる3つの値は、（0、0、g）ベクトルの新しい座標です。
姿勢の回転は、ZYXシーケンスで3回転を使用し、上記の説明は次のように表すことができます。
この方程式を解くと、ロール角とピッチ角が得られます（Zを中心に回転するとき、重力加速度は一定であるため、加速度計はヨー角を計算できません）
3回転プロセスの分解プロセスは次のとおりです。
5ジャイロは姿勢角を計算します
ジャイロスコープで3軸を中心に測定した角速度。したがって、角速度を積分することで角度を求めることができます。ジャイロスコープの英語の略語はジャイロであり、最初の文字gはジャイロスコープのデータを表すために以下で使用されます。
下の図に示すように、n番目の瞬間のIMUの姿勢角はr、p、yです。これは、IMU座標系が初期位置からZの周りの角度y、Yの周りの角度pだけ回転することを意味します。そして、Xの周りの角度rを取得するには、最終的な姿勢について、次の瞬間の姿勢（n + 1）をこの時点で計算する必要があります。時間n + 1での姿勢角がr +Δr、p +Δp、y +Δyであり、この姿勢も3回転したとします。時間n + 1での姿勢を計算するには、時間nでの姿勢に基づいて、対応する姿勢角度の変化を追加するだけです。姿勢角の変化量は、角速度と採用期間で積分できます。
ここで、赤いボックス内のdr / dtの一定角速度は、実際には姿勢の更新に使用される仮想角速度です。姿勢の更新は、測地座標系と、n番目の状態でジャイロスコープによって読み取られる角速度に基づいています。は独自の座標系です。参考までに、読み取ったジャイロジャイロスコープのデータは、n +1番目の姿勢の計算と更新に使用する前に変換する必要があります。
dr / dtの一定角速度を理解する方法は？次の図を見るか、3回転に分解してください。
最初にdy / dtを見てください。これは、3回転中のZ軸周りのヨー角の角速度です。3回転は最初にZ軸周りを回転します。Z軸方向の単位ベクトルは[0 01として表すことができます。 ] T、Tはベクトル転置を表すため、[0 0 dy / dt] Tは、図の状態①の座標におけるZの周りの角速度を表します。その後、座標系はYとXを中心に2回転するため、[0 0 dy / dt] Tの角速度も2回転後の最終座標系（状態③）の座標になります。2回の変換を行います。図中の[Gx_Zgy_Z gz_Z] Tは、最終姿勢での3回転時のZ軸周りのヨー角の角速度の等価回転角速度を表します。これは実際の状態です①のZ軸周りの角速度座標系は③座標系にあります。の新しい座標。
同様に、dp / dtも1回転変換する必要がありますが、dr / dtは回転する必要はありません。
dy / dt、dp / dt、dr / dtはすべて、状態③の新しい座標に変換され、座標系の新しい座標が追加されます。これは、実際には、状態③でジャイロスコープ自体によって読み取られたジャイロデータです。
したがって、dr / dtの角速度からジャイロスコープによって読み取られる角速度gxへの変換関係の導出プロセスは次のとおりです。
また、ここでの状態③は状態nと解釈され、状態n時に読み取ったジャイロスコープのデータから、dy / dtなどの角速度データを逆分解して、状態n +1の姿勢を更新することができます。逆解は逆行列です。つまり、次のようになります。
6姿勢融合
上記の分析から、加速度計は静止時の重力加速度に応じてロール角とピッチ角を計算でき、角度の計算は現在の姿勢にのみ関連していることがわかります。ジャイロスコープは、時間間隔の角速度を積分して各時間の角度変換量を取得し、それを最後の姿勢角に加算して新しい姿勢角を取得します。ジャイロスコープは、ロール、ピッチ、ヨーの3つの角度を計算できます。
実際、加速度は静止した瞬間にのみより正確な姿勢をとることができ、ジャイロスコープは回転中の姿勢の変化にのみ敏感であり、ジャイロスコープ自体にエラーがある場合、エラーは連続時間積分後に増加し続けます。したがって、補完的な融合のためには、2つによって計算された姿勢を組み合わせる必要があります。もちろん、ここではロールとピッチのみを統合できます。これは、加速度計がヨーを取得しないためです。
Kは比例係数であり、0.4など実際の状況に応じて調整する必要があります。
7MATLAB式の導出
上記の導出計算プロセスの一部は、MATLABによって支援され、手動計算エラーを防ぐことができます。
最初に3つの回転行列Yを定義します。
% 旋转顺序：Z,Y,X（从大地坐标系到IMU坐标系）
% 定义一些符号 r=row p=pitch y=yaw
syms r p y
% 3个旋转矩阵（坐标系旋转,注意负号的位置！）
M_x = [ 1, 0, 0;
0, cos(r), sin(r);
0, -sin(r), cos(r)];
M_y = [cos(p), 0, -sin(p);
0, 1, 0;
sin(p), 0, cos(p)];
M_z = [ cos(y), sin(y), 0;
-sin(y), cos(y), 0;
0, 0, 1];ジャイロスコープの変換行列を導出します
%% 推导陀螺仪的变换矩阵
%定义一些符号 drdt dpdt dydt 指的是分别对 roll pitch yaw求导，也就是角速度
syms drdt dpdt dydt
% 绕x轴转动 row 的角速度
dr_t = [drdt;
0;
0];
% 绕y轴转动 pitch 的角速度
dp_t = [ 0;
dpdt;
0];
% 绕z轴转动 yaw 的角速度
dy_t = [ 0;
0;
dydt];
% [矩阵X*矩阵Y*yaw角速度(绕Z)] + [矩阵X*pitch角速度(绕Y)] + [roll角速度(绕X)]
% IMU_gyro实际就是IMU测得的3个陀螺仪数据
IMU_gyro = M_x*M_y*dy_t + M_x*dp_t + dr_t;
fprintf('M_x*M_y*dy_t + M_x*dp_t + dr_t=\r\n')
disp(IMU_gyro)
% roll pitch yaw角速度组成的列向量，这个实际是要求的大地坐标系的3个角速度
rpy_t = [drdt;
dpdt;
dydt];
%手动分解IMU_gyro为矩阵M_gyro与列向量rpy_t相乘的形式
%根据IMU_gyro写出M_gyro，该矩阵将大地坐标系的角速度转换为IMU坐标系
M_gyro = [ 1, 0, -sin(p);
0, cos(r),cos(p)*sin(r);
0,-sin(r),cos(p)*cos(r)];
%验证一下
if M_gyro * rpy_t==IMU_gyro
fprintf('M_gyro is true\r\n');
else
fprintf('M_gyro is false\r\n');
end
fprintf('M_gyro=\r\n')
disp(M_gyro)
% 对M_gyro求逆矩阵，用于将IMU坐标系的陀螺仪角速度值转换到大地坐标系
M_gyro_inv = inv(M_gyro);
fprintf('M_gyro_inv=\r\n')
disp(M_gyro_inv)
% matlab求解的逆矩阵需要在再手工化简
M_gyro_inv_ =[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p);
0, cos(r), -sin(r);
0, sin(r)/cos(p), cos(r)/cos(p)];
% 验证一下，M_gyro_inv_ * M_gyro_inv应该是单位矩阵
fprintf('M_gyro_inv_ * M_gyro=\r\n')
disp(M_gyro_inv_ * M_gyro)
fprintf('M_gyro_inv_ =\r\n')
disp(M_gyro_inv_)
実行後の出力：
M_x*M_y*dy_t + M_x*dp_t + dr_t=
drdt - dydt*sin(p)
dpdt*cos(r) + dydt*cos(p)*sin(r)
dydt*cos(p)*cos(r) - dpdt*sin(r)
M_gyro is true
M_gyro=
[ 1, 0, -sin(p)]
[ 0, cos(r), cos(p)*sin(r)]
[ 0, -sin(r), cos(p)*cos(r)]
M_gyro_inv=
[ 1, (sin(p)*sin(r))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), (cos(r)*sin(p))/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
[ 0, cos(r)/(cos(r)^2 + sin(r)^2), -sin(r)/(cos(r)^2 + sin(r)^2)]
[ 0, sin(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2), cos(r)/(cos(p)*cos(r)^2 + cos(p)*sin(r)^2)]
M_gyro_inv_ * M_gyro=
[ 1, 0, cos(r)^2*sin(p) - sin(p) + sin(p)*sin(r)^2]
[ 0, cos(r)^2 + sin(r)^2, 0]
[ 0, 0, cos(r)^2 + sin(r)^2]
M_gyro_inv_ =
[ 1, (sin(p)*sin(r))/cos(p), (cos(r)*sin(p))/cos(p)]
[ 0, cos(r), -sin(r)]
[ 0, sin(r)/cos(p), cos(r)/cos(p)]
プログラムは最初にM_xM_y dy_t + M_x * dp_t + dr_tを計算し、次にそれをM_gyroとrpy_tを乗算する形式に手動で分解し、最後にM_gyroの逆行列M_gyro inv、つまり、使用するジャイロスコープの角速度値を取得します。 IMU座標系回転行列は、測地座標系に変換されます。
加速度計の変換行列を導出します
%% 推导加速度计的变换矩阵
M_acc=M_x*M_y*M_z;
fprintf('M_acc=\r\n')
disp(M_acc)
%重力向量
syms g
acc = [0;
0;
g];
IMU_acc=M_acc*acc;
fprintf('IMU_acc=\r\n')
disp(IMU_acc)
実行後の出力：
M_acc=
[ cos(p)*cos(y), cos(p)*sin(y), -sin(p)]
[ cos(y)*sin(p)*sin(r) - cos(r)*sin(y), cos(r)*cos(y) + sin(p)*sin(r)*sin(y), cos(p)*sin(r)]
[ sin(r)*sin(y) + cos(r)*cos(y)*sin(p), cos(r)*sin(p)*sin(y) - cos(y)*sin(r), cos(p)*cos(r)]
IMU_acc=
-g*sin(p)
g*cos(p)*sin(r)
g*cos(p)*cos(r)
計算されたIMU_accは、IMUの最終姿勢が配置されている座標系で加速度計が感じる重力加速度ベクトルの座標です。