# IMU使用方法、校正以及視覺化
###### tags: `IMU` `控制組`
#### 以下所有的code已放在github上的Controller/IMU_read/imu的資料夾裡
https://github.com/NCTU-AUV/Controller/tree/team/2020/IMU_read/imu
## 使用方法
#### 在使用之前必須先在arduino上安裝以下兩個程式庫
##### 此為接收IMU資訊以及校正IMU的程式庫
![](https://i.imgur.com/AsqswzJ.png)
##### 此為Madgwick/Mahony/Kalman filter的程式庫
![](https://i.imgur.com/xAP1Abk.png)
---
#### 在理解code之前先了解一點旋轉座標表示法(只需要知道表示方法即可)
1. quaternion
https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B8
2. euler angle
https://zh.wikipedia.org/zh-tw/%E6%AC%A7%E6%8B%89%E8%A7%92
3. rotation matrix
https://zh.wikipedia.org/wiki/%E6%97%8B%E8%BD%AC%E7%9F%A9%E9%98%B5
#### 而底下的code選擇quaternion的方式僅僅是因為視覺化較方便,在控制的使用上還是能改成euler angle較為直觀。
#### 只是euler angle有一個需要的點,當pitch的值超過90°後會變成-89°而不是91°,而且row和yaw會變為-180°
#### 再來是filter的簡單比較
| | Madgwick | Mahony | Kalman |
| --------- |:--------:|:------:|:------:|
| effective | Medium | Low | High |
| effort | Medium | Low | High |
#### 考慮到arduino的計算力以及filter的準確率,我們選擇折衷的Madgwick filter
---
### IMU的取值方法
```arduino=
#include "MPU9250.h" //讀IMU值的library
#include <Adafruit_AHRS_Madgwick.h> //filter的library
MPU9250 IMU(Wire, 0x68);
Adafruit_Madgwick filter;
int state, hz = 100; //state用來確認IMU的狀態,hz為取IMU資料的頻率
float ax, ay, az; //a: Accelerometer
float gx, gy, gz; //g: Gyroscope
float mx, my, mz; //m: Magnetometer
float q0, q1, q2, q3; //q: Quaternion
float StartTime; //控制輸出時間
void setup() {
Serial.begin(115200);
state = IMU.begin();
if(state < 0) {
while(1) {
Serial.println(state);
delay(1000);
}
}
filter.begin(hz); //設定filter的頻率
//以下要更改可以參考reference
IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G); //設定Accelerometer的值的範圍
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS); //設定Gyroscope的值的範圍
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ); //設定Digital Low Pass Filter的bandwidth
IMU.setSrd(1000/hz-1); //hz(output rate) = 1000 / (1+SRD)
//IMU校正的部分,此部分因應不同的IMU而改
IMU.setAccelCalX(0.0848, 1.0008);
IMU.setAccelCalY(0.0848, 1.0057);
IMU.setAccelCalZ(0.5504, 0.9842);
IMU.setGyroBiasX_rads(0.0136);
IMU.setGyroBiasY_rads(-0.0669);
IMU.setGyroBiasZ_rads(0.0121);
IMU.setMagCalX(33.6164, 1.1795);
IMU.setMagCalY(65.2783, 0.5858);
IMU.setMagCalZ(-16.7558, 2.2467);
StartTime = millis()+1000/hz; //固定輸出的時間
}
void loop() {
IMU.readSensor(); //讀IMU測到的值
//將取出的值已m/(s*s)的單位存起來
ax = IMU.getAccelX_mss();
ay = IMU.getAccelY_mss();
az = IMU.getAccelZ_mss();
//將取出的值已degree的單位存起來
gx = IMU.getGyroX_rads()/M_PI*180;
gy = IMU.getGyroY_rads()/M_PI*180;
gz = IMU.getGyroZ_rads()/M_PI*180;
//將取出的值已uT的單位存起來
mx = IMU.getMagX_uT();
my = IMU.getMagY_uT();
mz = IMU.getMagZ_uT();
//將值送入filter內
filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);
//將濾過的值以Quaternion的表示法存起來
filter.getQuaternion(&q0, &q1, &q2, &q3);
//當到達需要輸出的時間後就輸出
if(millis() > StartTime) {
Serial.print(q0); Serial.print(", "); Serial.print(q1); Serial.print(", "); Serial.print(q2); Serial.print(", "); Serial.println(q3);
StartTime += 1000/hz;
}
}
```
---
## 校正
1. accelerometer
#### 在校正時,必須讓IMU的用不同的面(立方體)保持不動10秒,共六面
```arduino=
#include "MPU9250.h" //讀IMU值的library
MPU9250 IMU(Wire,0x68);
int state, hz = 100; //state用來確認IMU的狀態,hz為取IMU資料的頻率
//accelerometer校正分為兩部分,bias和scale
float axb, ayb, azb; //bias
float axs, ays, azs; //scale
void setup() { //此部分和IMU的code相同
Serial.begin(115200);
state = IMU.begin();
if(state < 0) {
while(1) {
Serial.println(state);
delay(1000);
}
}
IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
IMU.setSrd(1000/hz-1);
}
void loop() {
for(int i = 1; i <= 6; i++) { //收集六面資料
Serial.print(i);
state = IMU.calibrateAccel(); //收集資料以測量bias和scale
Serial.println(state); //如果此面收集資料成功則螢幕會顯示1
delay(10000); //每一面為10秒
}
//X軸的bias和scale
axb = IMU.getAccelBiasX_mss();
axs = IMU.getAccelScaleFactorX();
//Y軸的bias和scale
ayb = IMU.getAccelBiasY_mss();
ays = IMU.getAccelScaleFactorY();
//Z軸的bias和scale
azb = IMU.getAccelBiasZ_mss();
azs = IMU.getAccelScaleFactorZ();
//將值顯示出來
Serial.print(axb, 4); Serial.print(", "); Serial.print(axs, 4); Serial.print(", ...");
Serial.print(ayb, 4); Serial.print(", "); Serial.print(ays, 4); Serial.print(", ...");
Serial.print(azb, 4); Serial.print(", "); Serial.println(azs, 4);
delay(10000);
}
```
2. gyroscope
#### 在校正時,只須讓IMU在平穩的地方保持不動即可
```arduino=
#include "MPU9250.h" //讀IMU值的library
MPU9250 IMU(Wire,0x68);
int state, hz = 100; //state用來確認IMU的狀態,hz為取IMU資料的頻率
//gyroscope的校正只有bias
float gxb, gyb, gzb;
void setup() { //此部分和IMU的code相同
Serial.begin(115200);
state = IMU.begin();
if(state < 0) {
while(1) {
Serial.println(state);
delay(1000);
}
}
IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
IMU.setSrd(1000/hz-1);
}
void loop() {
state = IMU.calibrateGyro(); //收集資料以測量bias
gxb = IMU.getGyroBiasX_rads(); //X軸的bias
gyb = IMU.getGyroBiasY_rads(); //Y軸的bias
gzb = IMU.getGyroBiasZ_rads(); //Z軸的bias
//將值顯示出來
Serial.print(gxb, 4); Serial.print(", ");
Serial.print(gyb, 4); Serial.print(", ");
Serial.println(gzb, 4);
delay(10000);
}
```
3. magnetometer
#### 在校正時必須持續且緩慢地將IMU以8的字型擺動(大約2~3分鐘),直到顯示數字為止
```arduino=
#include "MPU9250.h" //讀IMU值的library
MPU9250 IMU(Wire,0x68);
int state, hz = 100; //state用來確認IMU的狀態,hz為取IMU資料的頻率
//magnetometer校正分為兩部分,bias和scale
float hxb, hyb, hzb; //bias
float hxs, hys, hzs; //scale
void setup() { //此部分和IMU的code相同
Serial.begin(115200);
state = IMU.begin();
if(state < 0) {
while(1) {
Serial.println(state);
delay(1000);
}
}
IMU.setAccelRange(MPU9250::ACCEL_RANGE_16G);
IMU.setGyroRange(MPU9250::GYRO_RANGE_2000DPS);
IMU.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
IMU.setSrd(1000/hz-1);
}
void loop() {
//開始測量bias和scale
Serial.println("Start");
state = IMU.calibrateMag();
Serial.println(state); // 如果測量結果成功則會顯示1
//X軸的bias和scale
hxb = IMU.getMagBiasX_uT();
hxs = IMU.getMagScaleFactorX();
//Y軸的bias和scale
hyb = IMU.getMagBiasY_uT();
hys = IMU.getMagScaleFactorY();
//Z軸的bias和scale
hzb = IMU.getMagBiasZ_uT();
hzs = IMU.getMagScaleFactorZ();
//將值顯示出來
Serial.print(hxb, 4); Serial.print(", "); Serial.print(hxs, 4); Serial.print(", ...");
Serial.print(hyb, 4); Serial.print(", "); Serial.print(hys, 4); Serial.print(", ...");
Serial.print(hzb, 4); Serial.print(", "); Serial.println(hzs, 4);
delay(10000);
}
```
## 視覺化
#### 此部分是使用matlab來完成
```matlab=
clc
clear
close force all
%%
A = serialport('COM4', 115200); %對應到arduino的COM和Baud rate
%%
Quaternion = readline(A); %取值
Quaternion = str2num(Quaternion); %#ok<*ST2NM> %string to num
RM = quat2rotm(Quaternion); %quaternion to rotation matrix
%畫圖
axis equal
xlim([-1 1]);
ylim([-1 1]);
zlim([-1 1]);
hold on
%取值
X = plot3([0 RM(1, 1)], [0 RM(2, 1)], [0 RM(3, 1)], 'r-');
Y = plot3([0 RM(1, 2)], [0 RM(2, 2)], [0 RM(3, 2)], 'g-');
Z = plot3([0 RM(1, 3)], [0 RM(2, 3)], [0 RM(3, 3)], 'b-');
pause(0.1);
%%{
while 1
if(A.NumBytesAvailable>20)
read(A, A.NumBytesAvailable-20, 'char'); %將要取值的那行之前的所有資料
%先read,不然會一直取到第一行
readline(A); %將要取值的前一行取乾淨
Quaternion = readline(A); %取值
Quaternion = str2num(Quaternion); %#ok<*ST2NM> %string to num
RM = quat2rotm(Quaternion); %Quaternion to rotation matrix
%更新X軸座標值
X.XData = [0 RM(1, 1)];
X.YData = [0 RM(2, 1)];
X.ZData = [0 RM(3, 1)];
%更新Y軸座標值
Y.XData = [0 RM(1, 2)];
Y.YData = [0 RM(2, 2)];
Y.ZData = [0 RM(3, 2)];
%更新Z軸座標值
Z.XData = [0 RM(1, 3)];
Z.YData = [0 RM(2, 3)];
Z.ZData = [0 RM(3, 3)];
pause(0.01);
end
end
%
}
```