Hinweis: Einige Bilder in diesem Artikel sind mit horizontalen Linien platziert. Wenn die Anzeige nicht bequem ist, können Sie auf den gelesenen Originaltext am Ende des Artikels klicken, um zur Webversion zu springen.
1IMU Einstellungsberechnung
Die IMU oder Trägheitsmesseinheit umfasst im Allgemeinen ein dreiachsiges Gyroskop und einen dreiachsigen Beschleunigungsmesser. Der vorherige Artikel MPU6050-Lageberechnungsmethode 1-DMP hat die MPU6050-IMU kurz vorgestellt und erhält das Quaternionsergebnis der Lageberechnung direkt über seine interne DMP-Verarbeitungseinheit. In diesem Artikel werden Euler-Winkel und Rotationsmatrix verwendet, um die Lage der Originaldaten des Gyroskops und des Beschleunigungsmessers mithilfe einer Softwareberechnung zu lösen. Die beiden Einstellungen werden komplementär und zusammengeführt, und schließlich wird die Echtzeitlage der IMU festgelegt erhalten.
Die in diesem Artikel für die Haltungsberechnung ausgewählte Rotationssequenz lautet ZYX. Das heißt, das IMU-Koordinatensystem stimmt zunächst mit dem geodätischen Koordinatensystem überein und dreht sich dann um seine eigenen Z-, Y- und X-Achsen. Passen Sie hier zunächst den Namen von an jede Drehung und jedes Symbol:
-
Drehen Sie um die Z-Achse der IMU: Kurswinkel Gieren, Drehwinkel y
-
Um die Y-Achse der IMU drehen: Nickwinkel, Drehwinkel p
- Um die X-Achse der IMU drehen: Rollwinkelreihe, Drehwinkel r
Das schematische Diagramm von drei Umdrehungen ist wie folgt:
Darüber hinaus sind die tatsächlichen Bedeutungen von Rollen, Neigen und Gieren wie folgt:
2 Rotationsmatrix
Informationen zur Rotationsmatrix finden Sie in der 3D-Rotationsmatrix-Ableitung und -Mnemonik sowie in der 3D-Rotationsmatrix-Ableitung und in der Mnemonik-Ergänzung. Hier sind nur die 3 Rotationsmatrizen aufgeführt, die in diesem Artikel verwendet werden müssen. Beachten Sie, dass diese 3 Rotationsmatrizen sind Rotationsmatrix für die Koordinatentransformation.
3 Eulerwinkeldrehung
Informationen zur Euler-Winkeldrehung finden Sie zuerst unter Euler-Winkeldrehung. Hier muss erläutert werden, dass sich die in diesem Artikel verwendete Achse in der ZYX-Reihenfolge dreht.
4Beschleunigungsmesser zur Berechnung des Lagewinkels
Der Beschleunigungsmesser misst die Beschleunigung, die er fühlt. Wenn er sich in Ruhe befindet, beschleunigt er seine Bewegung nicht. Aufgrund des Effekts der Gravitationsbeschleunigung ist die Beschleunigung nach der Theorie der Relativbewegung das Gegenteil der Gravitationsbeschleunigung Daten sind direkt. Die englische Abkürzung für Beschleunigungsmesser lautet acc, und der erste Buchstabe a wird unten verwendet, um Beschleunigungsmesserdaten darzustellen.
Die Beschleunigung verwendet die Beschleunigung der Schwerkraft im Moment der Ruhe und berechnet die Haltung:
-
Wenn der Beschleunigungsmesser horizontal platziert ist, dh wenn die Z-Achse aufrecht steht, kann die Z-Achse den Wert 1 g (g ist die Erdbeschleunigung) und die X-Achse und die Y-Achse können 0 in beide Richtungen anzeigen, was gilt kann als (0, 0, g) aufgezeichnet werden.
- Wenn sich der Beschleunigungsmesser in einer bestimmten Position dreht, erzeugt die Gravitationsbeschleunigung entsprechende Komponenten auf den drei Beschleunigungsachsen. Sein Wesen sind die Koordinaten von (0, 0, g) im geodätischen Koordinatensystem im eigenen Koordinatensystem des neuen Beschleunigungsmessers. Die 3 vom Beschleunigungsmesser gelesenen Werte sind die neuen Koordinaten des Vektors (0, 0, g).
Die Drehung der Haltung verwendet 3 Umdrehungen in der ZYX-Sequenz, und die obige Beschreibung kann ausgedrückt werden als:
Wenn Sie diese Gleichung lösen, können Sie die Roll- und Nickwinkel erhalten (da die Erdbeschleunigung beim Drehen um Z konstant ist, kann der Beschleunigungsmesser den Gierwinkel nicht berechnen).
Der Zersetzungsprozess des 3-Rotationsprozesses ist wie folgt:
5 Gyro berechnet den Einstellungswinkel
Die vom Gyroskop gemessene Winkelgeschwindigkeit um die drei Achsen. Daher kann der Winkel durch Integrieren der Winkelgeschwindigkeit erhalten werden. Die englische Abkürzung für Gyroskop ist Gyro, und der erste Buchstabe g wird unten verwendet, um Gyroskopdaten darzustellen.
Wie in der folgenden Abbildung gezeigt, sind die Lagewinkel der IMU im n-ten Moment r, p, y, was bedeutet, dass das IMU-Koordinatensystem von der Ausgangsposition um den Winkel y um Z, den Winkel p um Y, gedreht wird. und der Winkel r um X, um zu erhalten Für die endgültige Haltung muss die Haltung im nächsten Moment (n + 1) zu diesem Zeitpunkt berechnet werden. Angenommen, der Einstellungswinkel zum Zeitpunkt n + 1 ist r + Δr, p + Δp, y + Δy, und diese Einstellung hat auch 3 Umdrehungen erfahren. Um die Fluglage zum Zeitpunkt n + 1 zu berechnen, addieren Sie einfach die entsprechende Lagewinkeländerung auf Basis der Fluglage zum Zeitpunkt n. Das Ausmaß der Änderung des Lagewinkels kann durch die Winkelgeschwindigkeit und den angenommenen Zeitraum integriert werden.
Hier ist die konstante dr / dt-Winkelgeschwindigkeit in der roten Box tatsächlich die imaginäre Winkelgeschwindigkeit, die für die Lageaktualisierung verwendet wird. Die Lageaktualisierung basiert auf dem geodätischen Koordinatensystem und der vom Gyroskop im n-ten Zustand gelesenen Winkelgeschwindigkeit ist ein eigenes Koordinatensystem. Als Referenz müssen die gelesenen Gyro-Gyroskop-Daten transformiert werden, bevor sie zur Berechnung und Aktualisierung der n + 1-ten Haltung verwendet werden können.
Wie versteht man die konstante dr / dt-Winkelgeschwindigkeit? Schauen Sie sich die folgende Abbildung an oder zerlegen Sie sie in 3 Umdrehungen:
Schauen Sie sich zuerst dy / dt an. Dies ist die Winkelgeschwindigkeit des Gierwinkels um die Z-Achse während der 3 Umdrehungen. Die 3 Umdrehungen drehen sich zuerst um die Z-Achse. Der Einheitsvektor in Richtung der Z-Achse kann als [0 0 ausgedrückt werden 1] T, T repräsentiert die Vektortransposition, also repräsentiert [0 0 dy / dt] T die Winkelgeschwindigkeit um Z in den Koordinaten des Zustands ① in der Figur. Da das Koordinatensystem danach zwei Umdrehungen um Y und X erfährt, hat die Winkelgeschwindigkeit von [0 0 dy / dt] T nach zwei Umdrehungen auch die Koordinaten im endgültigen Koordinatensystem (Zustand ③). Führen Sie zwei Transformationen durch. [Gx_Z gy_Z gz_Z] T in der Figur repräsentiert die äquivalente Drehwinkelgeschwindigkeit der Winkelgeschwindigkeit des Gierwinkels um die Z-Achse während der 3 Umdrehungen in der Endlage, was tatsächlich der Zustand ist Koordinatensystem befindet sich im Zustand ③Koordinatensystem Die neuen Koordinaten in.
In gleicher Weise muss dp / dt auch einer Rotationstransformation unterzogen werden, während dr / dt keine Rotation durchlaufen muss.
Dy / dt, dp / dt, dr / dt werden alle in die neuen Koordinaten im Zustand ③ transformiert und die neuen Koordinaten im Koordinatensystem addiert. Dies sind tatsächlich die Kreiseldaten, die vom Gyroskop selbst im Zustand ③ gelesen werden.
Daher ist der Ableitungsprozess der Umwandlungsbeziehung von der Winkelgeschwindigkeit von dr / dt zur Winkelgeschwindigkeit gx, die vom Gyroskop gelesen wird, wie folgt:
Ferner wird der Zustand ③ hier als Zustand n verstanden, und dann können gemäß den zum Zeitpunkt des Zustands n gelesenen Gyroskopdaten die Winkelgeschwindigkeitsdaten wie dy / dt umgekehrt aufgelöst werden, um die Lage des Zustands n + 1 zu aktualisieren. Die inverse Lösung ist die inverse Matrix, nämlich:
6 Einstellungsfusion
Aus der obigen Analyse ist ersichtlich, dass der Beschleunigungsmesser Roll- und Nickwinkel gemäß der Erdbeschleunigung im Ruhezustand berechnen kann und die Winkelberechnung sich nur auf die aktuelle Haltung bezieht. Das Gyroskop integriert die Winkelgeschwindigkeit in das Zeitintervall, um den Winkelumwandlungsbetrag für jedes Mal zu erhalten, und addiert ihn zum letzten Lagewinkel, um den neuen Lagewinkel zu erhalten. Das Gyroskop kann die drei Winkel von Rollen, Neigen und Gieren berechnen.
Tatsächlich kann die Beschleunigung nur im Moment der Ruhe eine genauere Fluglage erreichen, und das Gyroskop reagiert nur empfindlich auf Fluglagenänderungen während der Drehung. Wenn das Gyroskop selbst einen Fehler aufweist, steigt der Fehler nach kontinuierlicher Zeitintegration weiter an. Daher ist es notwendig, die von beiden berechneten Körperhaltungen für die komplementäre Fusion zu kombinieren. Natürlich können hier nur Roll und Pitch integriert werden, da der Beschleunigungsmesser nicht giert.
K ist der Proportionalitätskoeffizient, der entsprechend der tatsächlichen Situation angepasst werden muss, z. B. 0,4.
7MATLAB-Formelableitung
Einige der oben genannten Ableitungsberechnungsprozesse können von MATLAB unterstützt werden, um manuelle Berechnungsfehler zu vermeiden:
Definieren Sie zuerst 3 Rotationsmatrizen 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];Leiten Sie die Transformationsmatrix des Gyroskops ab
%% 推导陀螺仪的变换矩阵
%定义一些符号 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_)
Die Ausgabe nach dem Ausführen:
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)]
Das Programm berechnet zuerst M_x M_y dy_t + M_x * dp_t + dr_t und zerlegt es dann manuell in die Form der Multiplikation von M_gyro und rpy_t und erhält schließlich die inverse Matrix M_gyro inv von M_gyro , dh den zur Verwendung verwendeten Kreiselwinkelgeschwindigkeitswert das IMU-Koordinatensystem Die in das geodätische Koordinatensystem konvertierte Rotationsmatrix.
Leiten Sie die Transformationsmatrix des Beschleunigungsmessers ab
%% 推导加速度计的变换矩阵
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)
Die Ausgabe nach dem Ausführen:
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)
Der berechnete IMU_acc ist die Koordinate des Gravitationsbeschleunigungsvektors, den der Beschleunigungsmesser im Koordinatensystem empfindet, in dem sich die endgültige Haltung der IMU befindet.