機器人 - 無人機基礎(5) - 飛控中的傳感器

一、加速度計

1.1 加速度計原理

1.2?加速度計校準

1.2.1 誤差模型

1.2.2 關于MPU6050校準方式與代碼思路

二、陀螺儀

2.1 陀螺儀原理

2.1.1 科里奧利力

2.1.2 陀螺儀原理

2.2?陀螺儀校準

三、加速度計與陀螺儀濾波部分

四、磁力計

4.1 磁力計原理

4.2 磁力計校準

4.2.1 干擾源

4.2.2 數值模型

4.2.3 最小二乘法找最優參數

4.2.4 簡單一些的校準方式 - 包含代碼思路

4.2.5 磁力計濾波

五、超聲波傳感器

5.1 超聲波簡介部分

5.2 數據采集和濾波部分

六、氣壓傳感器

6.1 簡單介紹 - 沒必要看

七、激光測距傳感器

7.1 簡單介紹 - 沒必要看

八、視覺傳感器

8.1 光流傳感器

8.1.1 光流傳感器簡單介紹

8.2?視覺里程計

8.2.1 攝像頭部分

8.2.2 基于特征的稀疏法

一、加速度計

1.1 加速度計原理

首先說一下以前沒有GNSS全球衛星定位系統時，導彈是用慣性測量元件獲取信息；
比如加速度計、角速度計；
我們知道獲取到了加速度，可以根據加速度對時間進行積分得到速度，對速度進行積分又可以得到路程；對角速度進行積分可以得到角度；

看上去確實很棒，但是慣性測量元件本身會有誤差，在通過時間積分之后誤差會累積...大多數情況下，成本越低...誤差越大...也累積得越快...
在出現GNSS后，現代導彈是利用衛星和慣導同步獲取信息的，等于說衛星定位系統來給慣導系統做補償，來修正這個累積的誤差值；

?

人體的慣導+GNSS

耳蝸中有液體，液體有慣性，運動時耳中神經可以感受到運動特征，所以把耳蝸比作慣導；
眼睛比作GNSS，但是工作方式不同；

那么我們睜著眼睛走路，耳蝸在感受運動特征時，即使出現誤差值，可以利用眼睛作修正；
但是閉著眼睛走路，僅僅只有耳蝸作為定位，沒有修正，誤差值一直累積，導致人很難走直線；
耳蝸受損時，保持平衡的能力受損也類似；

1.2?加速度計校準

1.2.1 誤差模型

這里的誤差模型中，am'是原始數據，ba‘是零點偏移補償，

首先，什么是零點偏移補償？

因為芯片生產、裝配有瑕疵，哪怕靜止（理論加速度為 0），它也會輸出一個錯誤的小數值，這時候需要ba’零點偏移補償，來把這個誤差抵消；

?

Sa是尺度因子（尺度比例補償）

加速度計測量時，可能存在 “量程縮放不準” 的問題。比如真實加速度是 1g，理論該輸出 1，但因為芯片 “放大 / 縮小” 了信號，實際輸出 0.9（或 1.1）。Sa 就是給 x 軸 “修正比例”，讓它從錯誤的 0.9→1（或 1.1→1 ）。乘以Sa后，得到?“消除零點偏移 + 修正尺度比例后的理論值”?。

安裝 / 扭轉誤差補償

加速度計芯片理論上 x 軸要和 “實際測量方向” 完全對齊，但實際安裝時，可能歪了一點點（比如繞 y 軸轉了個小角度），導致測出來的 x 軸數據 “混進了 y 軸的分量”。旋轉矩陣Ra 就像 “把歪掉的坐標系，旋轉回正確方向”，把混進的錯誤分量 “轉回去”，得到?“消除所有誤差后的真實加速度am”?。

說這些可能難懂，我們把加速度計想成個出廠時不準的尺子，
(1).首先零點飄了，零刻度線位置不對，這時候我們用ba’將其恢復到正確位置；
(2).接著，刻度不均勻，我們利用Sa重新刻刻度；
(3).尺子不夠直，是歪的，我們利用Ra把尺子"掰正"；
這樣使得加速度計這個“尺子”的誤差被修正；

1.2.2 關于MPU6050校準方式與代碼思路

使用MPU6050時，假如沒有高精度平臺情況下，可以考慮用下列簡易的方法進行校準；
(1).把裝了加速度計的飛控水平放置，這時候Z軸朝著正下方，XY軸的重力分量是0，Z軸測到加速度量為1g；
(2).這時候，XY軸上讀到的數據就是零點飄移，Z軸的數據-1g也可以認為是Z軸的零點飄移；

下列代碼中：
sum[7] 前三個是存加速度計ax/ay/az 原始數據累加的；
acc_sum_cnt 是統計加速度計采樣次數，判斷有沒有達到校準所需要的多次采樣；
/**********************************************************
* 函數原型:  void MPU6050_calibration()
* 功  能:   校準加速度計和陀螺儀
**********************************************************/
void MPU6050_calibration(void)
{static uint16_t acc_sum_cnt = 0, gyro_sum_cnt = 0;static int32_t sum[7] = {0,0,0,0,0,0};if(mpu6050.Acc_CALIBRATE){acc_sum_cnt++;sum[0] += mpu6050.ax_raw;sum[1] += mpu6050.ay_raw;sum[2] += mpu6050.az_raw - 4096; if(acc_sum_cnt >= MEMS_CALIB_AVG_NUM){mpu6050.Ax_offset = (float)sum[0]/MEMS_CALIB_AVG_NUM;mpu6050.Ay_offset = (float)sum[1]/MEMS_CALIB_AVG_NUM;mpu6050.Az_offset = (float)sum[2]/MEMS_CALIB_AVG_NUM;acc_sum_cnt = 0;if(mpu6050.Acc_CALIBRATE == 1){Param_SaveAccelOffset(mpu6050.Ax_offset, mpu6050.Ay_offset, mpu6050.Az_offset);}mpu6050.Acc_CALIBRATE = 0;sum[0] = sum[1] = sum[2] = 0;}}if(mpu6050.Gyro_CALIBRATE){gyro_sum_cnt++;sum[3] += mpu6050.gx_raw;sum[4] += mpu6050.gy_raw;sum[5] += mpu6050.gz_raw; if(gyro_sum_cnt >= MEMS_CALIB_AVG_NUM){mpu6050.Gx_offset = (float)sum[3]/MEMS_CALIB_AVG_NUM;mpu6050.Gy_offset = (float)sum[4]/MEMS_CALIB_AVG_NUM;mpu6050.Gz_offset = (float)sum[5]/MEMS_CALIB_AVG_NUM;gyro_sum_cnt = 0;if(mpu6050.Gyro_CALIBRATE == 1){Param_SaveGyroOffset(mpu6050.Gx_offset, mpu6050.Gy_offset, mpu6050.Gz_offset);}mpu6050.Gyro_CALIBRATE = 0;sum[3] = sum[4] = sum[5] = 0;}}
}

二、陀螺儀

2.1 陀螺儀原理

2.1.1 科里奧利力

用立創eda畫的...先湊合著看...

這里我只是提一嘴，所以不夠嚴謹，以理解個大概就行...需要深入的可以去查一下；

首先，假設有個圓盤，它不動的。那么盤上點A有個物體，受到F作用到點B直線運動，此時它運動路程也是直線的，很好理解吧...

現在，在次基礎上，圓盤以ω逆時針運動，那么路徑是下面這樣的吧

我們可以引入一個虛擬力， $F_{c} =- 2m \omega \times v$ ?，來表述另物體m改變運動軌跡的力...

2.1.2 陀螺儀原理

現在，假如有兩個小方塊，沿著x軸，同時做反向振動 - 你動我也動，方向相反，但是速度大小相同；根據剛剛上面說到的科里奧利力，會發現它們兩個受到的科里奧利力相反，這一對相反的力會把兩個小塊往相反的方向拉，導致它們和固定電極的間距變化；從而導致電容差分變化；

說人話一點就是，陀螺儀里面兩小玩意在那振動，你讓這陀螺儀旋轉了，里面電容變化了，然后這個電容變化被映射到角速度上面去，然后反饋回來給我們知道；

2.2?陀螺儀校準

這部分直接參考加速度計那里

三、加速度計與陀螺儀濾波部分

因為無人機電機旋轉產生的振動容易引起傳感器與加速度計的高頻噪聲；

所以在采集數據之后，可以試著采用平滑濾波，或者FIR、IIR濾波進行處理；
濾波算法 - 滑動平均濾波_滑動平均濾波算法原理-CSDN博客
這篇文章有我之前工作中用過的一種簡單的濾波方法，直接復制粘貼就能用了；

四、磁力計

4.1 磁力計原理

利用磁致電阻AMR來檢測空間中磁感應強度大小；
磁場的強弱變化會導致AMR阻值發生變化；

AMR磁阻效應：
(1).首先，磁場方向 / 強度變化時，阻值也跟著變化；
(2).制造AMR時，先用強磁預磁化，讓材料內部形成一個主磁域，或者說是給它固定一個初始磁場方向；這時，材料有一個“敏感軸”，與主磁域垂直方向，磁場變化最敏感；
(3).根據資料顯示，為了使測量結果以線性的方式變化，AMR材料上的金屬導線是呈45°角排列；

(4).外界有磁場時，AMR主磁域方向會變化，導致磁場方向和電流夾角也變化，夾角的變化導致AMR阻值變化；
之后利用惠斯通電橋檢測AMR阻值變化；
磁場變化的時候，假設R1R2增加a，R3R4減少a，電橋失衡，那么會出現一個微小電壓被檢測到；

4.2 磁力計校準

4.2.1 干擾源

(1).干擾源在機體外，這種一般來說比較遠，能量小，不用怕可以忽略；但是也存在能量大的，比如高壓線、金屬礦產等異常磁場區，遇到這種就只能避免用磁力計推算航向了；
(2).干擾源在機身上，機載電子設備或者電機；
假如說，沒有干擾的情況下，地球磁場矢量是均勻分布在磁力計X、Y、Z軸上，形成三維圓球；
那么，受到上述兩種干擾后，這種圓球就會畸變成橢球形；

4.2.2 數值模型

(1).數值模型：
原始數據： $m'=[x_m ,y_m,z_m]^T$

校準后的數據： $m=[x_c,y_c,z_c]^T$

圓心偏移參數： $b'=[b_x,b_y,b_z]^T$

比例尺度參數:

矯正后的值滿足下列公式：

這里校準本質是“還原”，原始數據先抵消偏移，再修正尺度，得到校準值；
在上面說過，理想狀態（無干擾）下，數據應該是分布在圓球上面，那么 $x^2+y^2+z^2=R^2$

(2).開始引入誤差：
但是，實際校準總是域理論會有些差別，這里引入誤差u；

$u=x_c^2+y_c^2+z_c^2-R^2$
把 $x_c$ 、 $y_c$ 、 $z_c$ 再分別用上面的式子帶入，
會得到：

然后轉換成

則誤差為：

這里是通過 “特征向量+系數向量”的定義，把二次函數拆解成線性組合，
u里面包含
二次項： $s_{mx}^2x_m^2$ 、 $s_{my}^2y_m^2$ 、 $s_{mz}^2z_m^2$
一次項： $2 x_m b_x s_{mx}^2$ 、 $2 y_m b_y s_{my}^2$ 、 $2 z_m b_z s_{mz}^2$
常數項： $b_x^2s_{mx}^2+b_y^2s_{my}^2+b_z^2s_{mz}^2-R^2$
我們先定義向量V，把所有xm、ym、zm的項按照順序排進去；

然后利用向量P，把我們誤差u里面的東西，都提取出來...

就完事了...

4.2.3 最小二乘法找最優參數

然后要開始對u求平方和，
為什么？因為誤差u 表現出來的是“校準效果好不好”，那么我們要讓所有采樣點的誤差平方和最小；
為了找最小值，對?U?中所有參數（a,b,c,d,e,f,g?）求偏導，并令偏導為 0，如下列式子

這里為了簡化計算，定義
于是把那個很長的式子轉成?
但是因為是齊次線性方程組，解不唯一，所以

4.2.4 簡單一些的校準方式 - 包含代碼思路

(1).水平勻速旋轉，收集X、Y數據；
(2).Z軸勻速旋轉設備90°，收集Z數據；
(3).代碼部分

MagMAX和MagMIN 是用于存儲磁力計收集到的最大最小值；
MagSum是數據范圍；

首先采樣規定小于400，防止一些電磁干擾產生極端異常值；
然后更新最大最小值...
然后計算偏移量，這里用*0.5f，我印象里乘法比除法計算量小一些；

void HMC58831_CalOffset_Mag(void)
{static Vector3f_str MagMAX = { -100, -100, -100 };static Vector3f_str MagMIN = { 100, 100, 100 }, MagSum;static uint16_t cnt_m = 0;if (Mag_CALIBRATED){//采樣規定小于400if (ABS(HMC58831.Mag_Adc.x) < 400 && ABS(HMC58831.Mag_Adc.y) < 400 && ABS(HMC58831.Mag_Adc.z) < 400){//求采樣中的最大值MagMAX.x = MAX(HMC58831.Mag_Adc.x, MagMAX.x);MagMAX.y = MAX(HMC58831.Mag_Adc.y, MagMAX.y);MagMAX.z = MAX(HMC58831.Mag_Adc.z, MagMAX.z);//求采樣中的最小值MagMIN.x = MIN(HMC58831.Mag_Adc.x, MagMIN.x);MagMIN.y = MIN(HMC58831.Mag_Adc.y, MagMIN.y);MagMIN.z = MIN(HMC58831.Mag_Adc.z, MagMIN.z);}if (cnt_m == CALIBRATING_MAG_CYCLES){    //利用最大最小值求偏移HMC58831.Mag_Offset.x = (int16_t)((MagMAX.x + MagMIN.x) * 0.5f);HMC58831.Mag_Offset.y = (int16_t)((MagMAX.y + MagMIN.y) * 0.5f);HMC58831.Mag_Offset.z = (int16_t)((MagMAX.z + MagMIN.z) * 0.5f);MagSum.x = MagMAX.x - MagMIN.x;MagSum.y = MagMAX.y - MagMIN.y;MagSum.z = MagMAX.z - MagMIN.z;//以x為基準進行比例縮放HMC58831.Mag_Gain.y = MagSum.x / MagSum.y;HMC58831.Mag_Gain.z = MagSum.x / MagSum.z;if (Mag_CALIBRATED){    //保存數據Param_SaveMagOffset(&HMC58831.Mag_Offset);}Mag_CALIBRATED = 0;cnt_m = 0;}cnt_m++;}
}

4.2.5 磁力計濾波

平滑濾波，參考加速度計濾波那里，我放了篇博客，里面有代碼可以直接復制粘貼拿去用；

五、超聲波傳感器

5.1 超聲波簡介部分

首先為什么要用超聲波？
我們雖然可以靠GPS獲取位置數據，但是有障礙物的時候，衛星給到的信號有噪聲。在室內甚至用不了GPS，所以需要利用超聲波傳感器進行高度檢測；

超聲波傳感器原理？
首先超過20kHz的聲波稱為超聲波，比如40kHz，這種聲波波長短，方向性好，對固液體穿透力很大；超聲波傳感器利用發射器往一個方向發射超聲波，這時候開始計時，超聲波在空氣中以聲速(340m/s)傳播，跑到雜質或者分界面返回，接收器接收到反射回來的超聲波停止計時。那么
測量距離 = (計時器記錄的時間*聲速)/2 ，可以估算出障礙物距離；

盲區：
(1).檢測盲區：超聲波傳感器在發射超聲波時，發射頭會有一段短暫的 “恢復期”，在這個時間段內，它無法準確識別接收到的反射波是來自近處物體還是發射頭自身的干擾信號，所以無法檢測位于超聲波發射 / 接收器前段的部分物體，從而形成檢測盲區。
例子：假設你在一個房間里使用超聲波傳感器來檢測前方是否有障礙物。把超聲波傳感器放在桌子上，當一個很小的物體，比如一顆彈珠，放在距離傳感器非常近（處于檢測盲區）的地方時，傳感器可能不會檢測到彈珠的存在，就好像彈珠在傳感器 “眼皮底下” 隱身了一樣，仍然顯示前方沒有障礙物。

(2).角度引發的三角誤差：超聲波傳感器通過發射超聲波并接收反射波來計算距離，其原理基于直線傳播。當被測物體與傳感器成一定角度時，超聲波實際傳播的路徑是一個斜邊，而我們希望得到的是物體到傳感器的垂直距離（直角邊）。根據三角函數原理，斜邊長度大于直角邊，這就導致所測距離與實際距離存在偏差，即三角誤差。
例子：想象在一個空曠的停車場，你用超聲波傳感器測量斜停在旁邊的汽車的距離。傳感器發射超聲波，由于汽車是斜著的，超聲波沿著斜線路徑傳播到汽車并反射回來。傳感器計算出的距離是按照傳播路徑（斜邊）來算的，而實際上我們想知道的是汽車與傳感器的垂直距離，兩者之間就會有誤差。比如實際垂直距離是 3 米，但由于汽車斜停，傳感器測出的距離可能是 3.5 米。

5.2 數據采集和濾波部分

這里其實我沒寫什么東西，

因為目前基本都是用超聲波模塊，不同的模塊手冊、說明書不同，實際上還是要根據廠家給到的說明書來應用采集；

濾波方面還是得做的，首先原始數據容易受到供電紋波等方面影響產生高頻噪聲，其次超聲波受到溫度、傳輸介質(這里一般指空氣)變化也會產生影響；
這里簡單點的也用平滑濾波得了...溫度的畫，挺多模塊自帶溫度補償功能；

六、氣壓傳感器

6.1 簡單介紹 - 沒必要看

原理：氣壓傳感器利用的是地球表面大氣密度不相等（地表密度大，高層密度小）進行測量的；里面有個薄膜，接了個柔性電阻，氣壓升降的時候，薄膜帶動頂針，電阻器阻止變化；

為什么有？ 有時候無人機飛行高度超過超聲波傳感器探測范圍；

七、激光測距傳感器

7.1 簡單介紹 - 沒必要看

D=ct
D表示與目標間的距離，
c表示光在空氣中的傳播速度(3*10^8m/s)，
t光往返一次的時間

八、視覺傳感器

8.1 光流傳感器

8.1.1 光流傳感器簡單介紹

原理：利用一個或者多個視覺傳感器系統，在時間上采集圖像序列，然后利用圖像序列中的像素強度數據的時域變化和相關性來確定像素位置的“運動”，從而通過算法識別出圖像中的物體相對于視覺傳感器幾何中心的運動信息。

我理解的是在時域上對圖像進行采樣，然后利用分辨圖像上像素點不同時刻位置，與相關性來判斷像素的“速度”；

光流計算方法：
(1).基于匹配的方法；
(2).頻域方法；
(3).梯度方法；

8.2?視覺里程計

8.2.1 攝像頭部分

從下圖中可以知道，

這里 z 稱為 p 點的深度值，除此之外，相機焦距光圈中心有不同參數，用矩陣C來表示；

這里 fx 和 fy 是x、y軸方向的焦距，cx 和 cy 是x、y軸方向的偏移；
然后，還得算上三維旋轉(旋轉矩陣R) 和三維位移(平移向量D)

8.2.2 基于特征的稀疏法

這種方法主要思想是把兩張圖片中具有代表性的點(特征點)提取出來，針對這些點進行空間位置估計、追蹤運動情況；圖像中非特征點都被丟棄；
相關的算法有：
(1).尺度不變特征變換 SIFT
(2).角點檢測 Harris
(3).快速角點特征提取 FAST
(4).快速魯棒特征提取 SURF

對于單目視覺系統來說，獲取到的匹配點信息是二維圖像(沒包含深度)；
對于雙目視覺系統來說，獲取到的匹配點信息是三維圖像；

SLAM系統中，視覺SLAM采用視覺里程計對周邊環境建立點云的集合描述，通過大量空間點來估計周邊地圖的樣貌；