姿態(tài)篇：四.非線(xiàn)性最小二乘與飛控傳感器校準(zhǔn)

昵稱(chēng)59692568 2018-11-15

展開(kāi)全文

前言

搞好了傳感器,，那意味著飛控已經(jīng)完成了一半。

不用猜了,，這句話(huà)正是鄙人說(shuō)的:),。飛控的軟硬件相關(guān)工作，絕大部分是圍繞傳感器展開(kāi)的,，它們的重要性,，可能比你想象中的更大。

硬件上,，為了尋找性能優(yōu)秀又符合產(chǎn)品實(shí)際需求的傳感器如陀螺儀,、加速度計(jì)、磁力計(jì)等,，我們經(jīng)常需要對(duì)來(lái)自不同廠商的樣品進(jìn)行實(shí)際性能測(cè)試與評(píng)估,，即使實(shí)際上常用的型號(hào)就那么幾個(gè)。為了提升飛控的性能,，許多傳感器問(wèn)題我們還需要從硬件上進(jìn)行解決,，如降低電源噪聲,、隔離機(jī)身震動(dòng)甚至還要為傳感器提供恒定溫度的工作環(huán)境。

軟件上,，傳感器校準(zhǔn),、濾波與融合更是飛控程序中的重點(diǎn)，作為一名渣渣飛控工程師,，渾渾噩噩工作數(shù)年,，發(fā)現(xiàn)到頭來(lái)大部分時(shí)間都是花在了傳感器數(shù)據(jù)的處理上面，而且未來(lái)需要解決的問(wèn)題,，依然和傳感器有關(guān),。

本篇文章將結(jié)合實(shí)際飛控代碼，以循序漸進(jìn)的方式向讀者講述,，如何使用最優(yōu)化算法,，解決飛控中最基本的傳感器校準(zhǔn)問(wèn)題。

一.傳感器校準(zhǔn)簡(jiǎn)述

飛控上會(huì)使用各種各樣的傳感器,，但對(duì)于多旋翼而言,，最核心的是陀螺儀無(wú)疑。離開(kāi)了陀螺儀,，你甚至無(wú)法讓一架多旋翼飛行器正常離地,。其次則是加速度計(jì)，提供姿態(tài)觀測(cè),，讓飛行器可以得到一個(gè)較為客觀的自身姿態(tài)觀測(cè)信息,，使得自穩(wěn)飛行成為可能（能夠自動(dòng)保持自身姿態(tài)水平或一定角度）。除此之外,，還有許許多多的的其它傳感器,，共同為飛控提供穩(wěn)定飛行所需要的數(shù)據(jù)。

其中,，陀螺儀,、加速度計(jì)和磁力計(jì)在飛行前（出廠前），均需要進(jìn)行校準(zhǔn),，才能達(dá)到正常工作所需要的性能,。這便引出了一個(gè)問(wèn)題：什么是傳感器校準(zhǔn)（標(biāo)定）。

答案很簡(jiǎn)單,，所有事物都不是非黑即白,，而是時(shí)刻處于一個(gè)不可預(yù)知的混沌狀態(tài)。工廠按照特定的技術(shù)及工藝標(biāo)準(zhǔn)制造生產(chǎn)出一批傳感器,，這些傳感器中所有個(gè)體的實(shí)際狀態(tài)處于一個(gè)隨機(jī)分布且均值為期望誤差的序列上,。簡(jiǎn)單來(lái)說(shuō)，我們買(mǎi)來(lái)10個(gè)傳感器,，這些傳感器均存在著特定誤差且這10個(gè)傳感器的誤差各不一致,。這就要求我們?cè)趯?shí)際使用這些傳感器時(shí),，需要對(duì)每一個(gè)進(jìn)行單獨(dú)的校準(zhǔn)，得到特定的校準(zhǔn)參數(shù),，從而正常發(fā)揮傳感器的性能,。

二.傳感器誤差類(lèi)型

接下來(lái)我們會(huì)面臨下面兩個(gè)問(wèn)題：

傳感器有哪些誤差
如何測(cè)量傳感器的誤差

如果從我們目前所使用的MEMS傳感器的自身原理及制造工藝出發(fā)，分析傳感器的誤差來(lái)源,，那可能需要長(zhǎng)篇大論一番了,，但那是超出作者能力的事情，也不是本篇文章所討論的重點(diǎn),。

根據(jù)實(shí)際經(jīng)驗(yàn)，我們可以把傳感器誤差分為兩大類(lèi)：

零偏誤差（offset）
刻度誤差（scale）

這兩個(gè)是我們最容易理解,，也是飛控中三大傳感器均需要解決的誤差類(lèi)型,，其中

零偏誤差： 指?jìng)鞲衅髟诹慵?lì)下的輸出誤差。如陀螺儀在靜止?fàn)顟B(tài)下,，其角速度測(cè)量值理論為0,，而實(shí)際上我們將飛控靜止放置，陀螺儀依然會(huì)輸出諸如1°/s之類(lèi)的數(shù)據(jù),，這便是該陀螺儀傳感器的零偏誤差,。

刻度誤差： 指?jìng)鞲衅髟谝欢?lì)輸入下，其輸出值與輸入值的比值,。比如我們將飛控放在一個(gè)以500°/s恒定角速度運(yùn)動(dòng)的轉(zhuǎn)臺(tái)上,，而陀螺儀實(shí)際測(cè)量得到的角速度輸出為495°/s，500 / 495 ≈ 1.01 便是該陀螺儀的刻度誤差,。

從上述說(shuō)明中我們可以看出,，要正確測(cè)量傳感器的誤差，必須向傳感器施加特定的激勵(lì)信號(hào),，通過(guò)比較傳感器的實(shí)際輸出與激勵(lì)輸入量,，計(jì)算得到傳感器的誤差。

不同類(lèi)型的傳感器,，所需要的激勵(lì)信號(hào)源也不一樣,。如陀螺儀，要提供精確的激勵(lì)信號(hào),，必須依靠專(zhuān)業(yè)的轉(zhuǎn)臺(tái)設(shè)備,，這是個(gè)人及小公司均不具備的條件。好在對(duì)于大多數(shù)應(yīng)用而言,，陀螺儀的刻度誤差在一個(gè)可以忍受的范圍內(nèi),，因此可被我們忽略。但對(duì)于精益求精的產(chǎn)品而言,，要提升飛控性能到極致,，刻度誤差校準(zhǔn)還是必須的,，如某行業(yè)龍頭，其飛控產(chǎn)品及整機(jī)產(chǎn)品,，出廠前均需使用轉(zhuǎn)臺(tái)進(jìn)行校準(zhǔn),。缺少這些條件的個(gè)人愛(ài)好者，就只能暫時(shí)忽略這一項(xiàng),，只校準(zhǔn)陀螺儀的零偏誤差了,，而大多數(shù)情況下，陀螺儀的零偏校準(zhǔn)方式很簡(jiǎn)單,，使用均值校準(zhǔn)即可,。因此，陀螺儀的校準(zhǔn)不在本篇文章的討論范圍內(nèi),。

而本文中討論的重點(diǎn)：加速度計(jì)與磁力計(jì),，其信號(hào)激勵(lì)源均來(lái)自于自然界，前者為地球的重力場(chǎng),，后者為地球的地磁場(chǎng),。

三.傳感器校準(zhǔn)的簡(jiǎn)單方式

1.加速度計(jì)

假設(shè)有下面的情形，我們將飛控水平放置于水平面,，測(cè)得加速度計(jì)z軸輸出值 g_1= 0.97g ,，然后將飛控翻轉(zhuǎn)并同樣水平置于水平面，再測(cè)得加速度計(jì)z軸輸出值 g_2 = -0.99g ,。

設(shè)z軸零偏誤差為 scale_z ,，刻度誤差為 offset_z ，已知實(shí)際重力加速度為g,，可以得到下列兩條等式：

從而可以計(jì)算得到：

將這個(gè)方法推廣到其余兩軸,，便能得到其校準(zhǔn)參數(shù) scale_x, scale_y,offset_x,offset_y 。

缺陷：

這個(gè)校準(zhǔn)方法較為簡(jiǎn)單,，但是其可靠性建立于一個(gè)理想條件下：加速度計(jì)在采集每一個(gè)方向的數(shù)據(jù)時(shí),，必須嚴(yán)格貼合水平面。否則只能得到一個(gè)不太準(zhǔn)確的校準(zhǔn)數(shù)值,。實(shí)際應(yīng)用中,，我們很難保證校準(zhǔn)時(shí)讓飛控的每一個(gè)面保持水平，因?yàn)轱w行器本身就是一個(gè)不規(guī)則物體,，更多時(shí)候我們采集到的加速度數(shù)據(jù)為 [0.05g,0.08g,0,95g] ,，而非 [0,0,0.98g] ?；蛘哒f(shuō),，這種方式的實(shí)現(xiàn)成本過(guò)高，難以實(shí)際應(yīng)用,。

2.磁力計(jì)

磁力計(jì)的簡(jiǎn)單校準(zhǔn)方法原理等同于加速度計(jì),，即采集得到磁力計(jì)的三個(gè)軸的測(cè)量極值（最大與最小值）,，然后單獨(dú)計(jì)算出每個(gè)軸的零偏誤差與刻度誤差。實(shí)現(xiàn)區(qū)別在于由于傳感器激勵(lì)信號(hào)源不一樣,，我們需要將磁力計(jì)在三維空間中進(jìn)行各個(gè)方向的360°旋轉(zhuǎn),，才能盡可能準(zhǔn)確采集到極值。

缺陷：

實(shí)際應(yīng)用中,，磁力計(jì)校準(zhǔn)對(duì)于飛行器使用者來(lái)說(shuō)可能頗為頻繁,，而飛行器大小又各不一致，在很多情況下,，我們是很難將飛行器翻來(lái)覆去地在各個(gè)方向上進(jìn)行旋轉(zhuǎn),，其可操作性非常低，且不方便普通用戶(hù)的使用,。如果旋轉(zhuǎn)不全面,，便無(wú)法采集到磁力計(jì)的真實(shí)極值，校準(zhǔn)效果會(huì)大打折扣,，甚至校準(zhǔn)值嚴(yán)重偏離真實(shí)值。

四.建立傳感器誤差模型

對(duì)于程序員而言,，解決實(shí)際問(wèn)題的標(biāo)準(zhǔn)姿勢(shì)如下：

提出實(shí)際問(wèn)題--->建立數(shù)學(xué)模型--->轉(zhuǎn)換為程序--->求解

而上一節(jié)中,，我們描述了一個(gè)較為簡(jiǎn)單的用于加速度計(jì)與磁力計(jì)的校準(zhǔn)方法，但是其前提條件較為苛刻,，且校準(zhǔn)效果不佳,。于是，在這里我們提出了一個(gè)待解決的問(wèn)題：找到一種使用便捷并且效果更好的校準(zhǔn)算法,。使用便捷,，意味著約束條件越少越好，即在校準(zhǔn)算法對(duì)傳感器的采樣點(diǎn)的數(shù)量和位置要求不能過(guò)于嚴(yán)格的情況下依然可以取得良好的校準(zhǔn)效果,。

為了解決這個(gè)問(wèn)題,，得到傳感器的誤差參數(shù)，首先我們需要建立傳感器的誤差模型,，并將其轉(zhuǎn)換為數(shù)學(xué)方程,，從而進(jìn)行求解。

以加速度計(jì)為例建立傳感器誤差模型

設(shè)三軸加速度計(jì)的零偏誤差與刻度誤差分別為
o_x ,o_y,o_z,s_x,s_y,s_z
某一時(shí)刻該三軸加速度計(jì)的測(cè)量值分別為
x,y,z
設(shè)當(dāng)前時(shí)刻真實(shí)加速度向量為
a=(a_x,a_y,a_z)
有以下關(guān)系：
$\begin{cases} a_x=(x-o_x)s_x\\ a_y=(y-o_y)s_y\\ a_z=(z-o_z)s_z\\ \end{cases}$

靜止?fàn)顟B(tài)下,，不管加速度計(jì)方向如何,，其測(cè)量得到的加速度為重力加速度在各軸上的投影。而在地球上,，重力加速度向量模值總是等于,。假設(shè) a_x,a_y,a_z 單位為，便有以下等式成立：
a_x^2+a_y^2+a_z^2=1
即
(x-o_x)^2s_x^2+(y-o_y)^2s_y^2+(z-o_z)^2s_z^2=1

至此,，我們將加速度計(jì)的誤差求解轉(zhuǎn)化成為了一個(gè)數(shù)學(xué)問(wèn)題：已知,，由上一條等式,，求解出

五.求解誤差方程

上一節(jié)中，我們得到了傳感器的誤差方程： (x-o_x)^2s_x^2+(y-o_y)^2s_y^2+(z-o_z)^2s_z^2=1
求解方程,，對(duì)于經(jīng)歷過(guò)九年義務(wù)教育的我們來(lái)說(shuō)是一件非常熟悉的事情,，但這里存在著幾個(gè)棘手的問(wèn)題：

該方程為六元二次方程，存在著6個(gè)未知數(shù),，如果只存在一組觀測(cè)值,，那將會(huì)有無(wú)窮多解。因此我們需要獲得多組觀測(cè)值,，以構(gòu)建方程組,，從而求出唯一解，滿(mǎn)足方程組里的每一個(gè)方程
對(duì)于六元二次方程而言,，使用六組獨(dú)立的觀測(cè)值,，構(gòu)建出包含6個(gè)方程的方程組，理論上可以求出方程組的唯一解,?？墒鞘聦?shí)上，我們從傳感器上獲取到的觀測(cè)值,，都是帶有一定隨機(jī)噪聲的,，即觀測(cè)值并非完全準(zhǔn)確，從而導(dǎo)致求得的方程解未必精確
使用計(jì)算機(jī),，我們可以輕松求出線(xiàn)性方程組的唯一精確解（如高斯消元法）,，而非線(xiàn)性方程問(wèn)題，無(wú)論是從理論上還是計(jì)算方法上,，都要復(fù)雜得多,，大部分時(shí)候，對(duì)于無(wú)特定形式的非線(xiàn)性方程,，是沒(méi)有辦法直接求得精確解的,。

為了解決上述問(wèn)題，這里引進(jìn)了一種數(shù)學(xué)方法：最小二乘法

最小二乘法（又稱(chēng)最小平方法）是一種數(shù)學(xué)優(yōu)化技術(shù),。它通過(guò)最小化誤差的平方和尋找數(shù)據(jù)的最佳函數(shù)匹配,。利用最小二乘法可以簡(jiǎn)便地求得未知的數(shù)據(jù)，并使得這些求得的數(shù)據(jù)與實(shí)際數(shù)據(jù)之間誤差的平方和為最小,。

對(duì)于現(xiàn)實(shí)中的絕大部分問(wèn)題,，我們都是無(wú)法求得精確解的，于是退而求其次,，使用最小二乘法,，我們可以得到其近似解。

以傳感器誤差方程為例，設(shè)為某特定解下方程的誤差,，有
r=1 - (x-o_x)^2s_x^2+(y-o_y)^2s_y^2+(z-o_z)^2s_z^2
對(duì)于方程組而言,，其誤差的平方和函數(shù)為
$S=\sum_{i=0}^{n} r_i^2$
所謂最小二乘法，便是求得一組特定解,，使得最小(求函數(shù)極值),，該條件下的解，便是方程組的最優(yōu)近似解,。

對(duì)于線(xiàn)性方程組,，可將誤差函數(shù) S=0 轉(zhuǎn)換為矩陣方程并進(jìn)行求解即可，可惜現(xiàn)實(shí)問(wèn)題大部分均為非線(xiàn)性問(wèn)題,，如本文中的傳感器誤差方程便是一個(gè)非線(xiàn)性方程,，是無(wú)法變換為矩陣方程形式的，因此我們必須換一個(gè)思路來(lái)解決問(wèn)題,。

假定該函數(shù)存在著局部最小值,，那給定一個(gè)初始解，通過(guò)不斷地迭代計(jì)算,，從而找到誤差平方和函數(shù)的局部最優(yōu)解,，這便是非線(xiàn)性最小二乘的基本思想。

接下來(lái),，我們將討論求解非線(xiàn)性最小二乘的具體算法,。

六.高斯牛頓法

非線(xiàn)性最小二乘的求解算法有許多種，比較常見(jiàn)的有高斯牛頓法,，Levenberg-Marquardt法（LM），DogLeg法等,。其中高斯牛頓法是最為經(jīng)典的一種算法,，APM飛控中便是使用了該算法，而LM法可以看成是高斯牛頓法的改良版,，魯棒性更好,，在實(shí)際工程中應(yīng)用更廣泛，天穹飛控中則使用了LM法,。而LM法的基礎(chǔ)部分和高斯牛頓法完全一致,，因此我們從高斯牛頓法開(kāi)始講起。

由于算法原理細(xì)節(jié)部分較為復(fù)雜,，需要用到大量公式,，為了方便理解，將結(jié)合天穹飛控的實(shí)際代碼進(jìn)行講解,，高斯牛頓法和LM法的具體實(shí)現(xiàn)大部分是一樣的,，所以可以結(jié)合著來(lái)看。對(duì)于初學(xué)者來(lái)說(shuō)徹底弄懂這一部分的原理對(duì)自己能力的提升幫助較大，因?yàn)榇祟?lèi)最優(yōu)化算法在飛控之外的非常多工程領(lǐng)域都是有著大量應(yīng)用的,，如計(jì)算機(jī)視覺(jué)和機(jī)器學(xué)習(xí)等,。

天穹飛控中的LM法完整代碼

要搞明白高斯牛頓法，我們要先來(lái)了解一下牛頓法

1.牛頓法

為求得函數(shù)最優(yōu)解（極值點(diǎn)）,，牛頓法將問(wèn)題轉(zhuǎn)換為了求解 $f^{$

首先給定一個(gè)初始解 x_0 ,，然后在 x_0 處進(jìn)行一階泰勒展開(kāi)，得到
$f^{$
令
$f^{$
求解得到,，相比于 x_0 ,，有 $f^{$ < f^{'}="" (x_0)'="">，即新解比初始解更接近最優(yōu)解,。當(dāng)然該解和最優(yōu)解之間還有一定未知的距離,，于是我們將新解作為初始解，重復(fù)這一步驟,，經(jīng)過(guò)不斷地迭代計(jì)算,，最終會(huì)收斂于 $f^{$ 。

牛頓法尋找極值點(diǎn)的動(dòng)畫(huà)示意圖：

牛頓法尋找極值點(diǎn)的動(dòng)畫(huà)示意圖

牛頓法的優(yōu)點(diǎn)：

收斂速度快,，且可用于求解任意連續(xù)函數(shù)的最優(yōu)化問(wèn)題

牛頓法的缺點(diǎn)：

可以看出,，牛頓法實(shí)際上等同于尋找函數(shù)的局部極值點(diǎn)，因此必須選取一個(gè)相對(duì)接近的初始點(diǎn),，否則會(huì)導(dǎo)致無(wú)法正確收斂
需要求解目標(biāo)函數(shù)的Hessian矩陣（二階導(dǎo)）的逆矩陣,，計(jì)算比較復(fù)雜

2.高斯牛頓法

高斯牛頓法是一種非線(xiàn)性最小二乘最優(yōu)化方法。其利用了目標(biāo)函數(shù)的泰勒展開(kāi)式把非線(xiàn)性函數(shù)的最小二乘化問(wèn)題化為每次迭代的線(xiàn)性函數(shù)的最小二乘化問(wèn)題

高斯牛頓法實(shí)際上是牛頓法的簡(jiǎn)化形式,，或者說(shuō)改進(jìn)版,。多維情況下，當(dāng)維數(shù)較大時(shí),，牛頓法中的Hessian矩陣求逆是往往行不通的,，而高斯牛頓法對(duì)牛頓法中的Hessian矩陣進(jìn)行了化簡(jiǎn)，使得問(wèn)題可解,。

下面講解高斯牛頓法的具體實(shí)現(xiàn)

首先定義目標(biāo)函數(shù)
$S(\beta)= \sum_{i=0}^{5}r _i^2( \beta)$
其中
$\beta=[\beta_0,\beta_1,\beta_2,\beta_3,\beta_4,\beta_5]$
對(duì)應(yīng)了加速度計(jì)誤差方程中的六個(gè)變量,，即
[o_x ,o_y,o_z,s_x,s_y,s_z]
而 $r _i^2( \beta)$ 為傳感器誤差方程的誤差平方和函數(shù)，即
$r _i^2( \beta)=(1 - (x_i-o_x)^2s_x^2+(y_i-o_y)^2s_y^2+(z_i-o_z)^2s_z^2)^2$

設(shè)定一個(gè)初解 $\beta_\alpha$ ,，將目標(biāo)函數(shù) $S(\beta)$ 在 $\beta_\alpha$ 附近泰勒展開(kāi),，并舍棄高階無(wú)窮項(xiàng)，近似有：
$S(\beta)= S(\beta_\alpha)+[\bigtriangledown S(\beta_\alpha)]^T(\beta-\beta_\alpha)+\frac{1}{2}(\beta-\beta_\alpha)^TH_S(\beta_\alpha)(\beta-\beta_\alpha)$
我們的目標(biāo)是求出特定解,，使 $S(\beta)$ 有最小值,。當(dāng)函數(shù)有最小值（屬于極小值）時(shí)，其一階導(dǎo)為0,，即
$\bigtriangledown S(\beta)=\bigtriangledown S(\beta_\alpha)+H_S(\beta_\alpha)(\beta-\beta_\alpha)=0$
從而得到了高斯牛頓法的迭代公式：
$\beta=\beta_\alpha - [H_S(\beta_\alpha)]^{-1}\bigtriangledown S(\beta_\alpha)$
設(shè)
$\bigtriangleup = [H_S(\beta_\alpha)]^{-1}\bigtriangledown S(\beta_\alpha)$
則有
$\beta=\beta_\alpha - \bigtriangleup$
這里便是迭代的關(guān)鍵了,，給定初解后,，每一次計(jì)算出 $\bigtriangleup$ ，便得到了一個(gè)新解,，然后使用新解不斷進(jìn)行迭代計(jì)算,，使其逐漸逼近最優(yōu)解。

可能看到這里,，你依然是一臉懵逼,，但沒(méi)有關(guān)系，你只需要知道,，為了求解出函數(shù)的最優(yōu)解,，也就是傳感器的最優(yōu)校準(zhǔn)參數(shù)，我們只需要不斷去計(jì)算 $\bigtriangleup$ 就行了,。

接下來(lái)便講解如何計(jì)算 $\bigtriangleup$ (這里是整個(gè)算法實(shí)現(xiàn)的重點(diǎn)）

$\bigtriangleup$ 中的 H_S 為函數(shù)的Hessian矩陣（實(shí)際上是S函數(shù)的二階導(dǎo)）
$H_S=\left[ \begin{matrix} \frac{\partial^2 S}{\partial \beta_0^2}&\cdots&\frac{\partial^2 S}{\partial \beta_0 \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial^2 S}{\partial \beta_5 \beta_0}&\cdots&\frac{\partial^2 S}{\partial \beta_5^2}&\\ \end{matrix} \right]$
前文提到,，高斯牛頓法對(duì)牛頓法所做的改進(jìn)，便是對(duì)Hessian矩陣進(jìn)行了簡(jiǎn)化,，其中有
$\frac{\partial^2 S}{\partial \beta_i \beta_j}=2\sum_{k}(\frac{\partial r_k}{\partial \beta_i} \frac{\partial r_k}{\partial \beta_j} + r_k\frac{\partial^2 r_k}{\partial \beta_i \beta_j} )$
忽略二階微分項(xiàng),，近似有
$\frac{\partial^2 S}{\partial \beta_i \beta_j}=2\sum_{k}\frac{\partial r_k}{\partial \beta_i} \frac{\partial r_k}{\partial \beta_j}$
于是有
$H_S=2 \left[ \begin{matrix} \frac{\partial r_0}{\partial \beta_0}&\cdots&\frac{\partial r_0}{\partial \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial r_5}{\partial \beta_0}&\cdots&\frac{\partial r_5}{\partial \beta_5}&\\ \end{matrix} \right]^T \left[ \begin{matrix} \frac{\partial r_0}{\partial \beta_0}&\cdots&\frac{\partial r_0}{\partial \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial r_5}{\partial \beta_0}&\cdots&\frac{\partial r_5}{\partial \beta_5}&\\ \end{matrix} \right] =2J_r^TJ_r$
這里的 J_r 實(shí)際上就是函數(shù)（傳感器誤差方程的誤差函數(shù)）的雅可比矩陣

$\bigtriangleup$ 中的另外一個(gè)成員 $\bigtriangledown S$ 為函數(shù)的一階導(dǎo)的列向量，即
$\bigtriangledown S = \left[ \begin{matrix} \frac{\partial S}{\partial \beta_0}\\ \vdots\\ \frac{\partial S}{\partial \beta_5}\\ \end{matrix} \right]$
又有
$\frac{\partial S}{\partial \beta_i}=\sum_{k}\frac{\partial (r_k^2)}{\partial \beta_i} =2 \sum_{k}\frac{\partial r_k}{\partial \beta_i} r_k$
故 $\bigtriangledown S$ 可改寫(xiě)為
$\bigtriangledown S =\left[ \begin{matrix} \frac{\partial S}{\partial \beta_0}\\ \vdots\\ \frac{\partial S}{\partial \beta_5}\\ \end{matrix} \right] =2 \left[ \begin{matrix} \frac{\partial r_0}{\partial \beta_0}&\cdots&\frac{\partial r_0}{\partial \beta_5}&\\ \vdots&\ddots&\vdots&\\ \frac{\partial r_5}{\partial \beta_0}&\cdots&\frac{\partial r_5}{\partial \beta_5}&\\ \end{matrix} \right]^T \left[ \begin{matrix} r_0\\ \vdots\\ r_5\\ \end{matrix} \right] =2J_r^Tr$

于是我們終于得到了 $\bigtriangleup$ 的最終形式
$\bigtriangleup = [J_r(\beta_\alpha)^T J_r(\beta_\alpha)]^{-1} J_r(\beta_\alpha)^T r(\beta_\alpha)$
或者換一個(gè)形式
$J_r(\beta_\alpha)^T J_r(\beta_\alpha) \bigtriangleup =J_r(\beta_\alpha)^T r(\beta_\alpha)$
這就成了一個(gè)線(xiàn)性方程組,，未知量是 $\bigtriangleup$ ,，使用高斯消元法，便能求解出 $\bigtriangleup$ 的唯一解,。

至此,，高斯牛頓法的所有計(jì)算步驟均結(jié)束，我們只需要不斷進(jìn)行迭代計(jì)算：求解出 $\bigtriangleup$ ,，從而得到新解 $\beta$ ,，然后進(jìn)行下一次迭代，不斷逼近函數(shù)最優(yōu)解,。

隨著迭代次數(shù)的增加,，最終會(huì)得到一個(gè)無(wú)限接近最優(yōu)的解，但是考慮到時(shí)間成本和實(shí)際使用需求,，在校準(zhǔn)程序中我們?cè)O(shè)置一個(gè)閾值 eps ，當(dāng)方程解達(dá)到一定精度時(shí)（定義為迭代步長(zhǎng)的平方 $\bigtriangleup ^2 < eps$ ）,，停止迭代,，結(jié)束此次校準(zhǔn)。

看完了,，還是有點(diǎn)懵,，怎么辦？

下面貼上高斯牛頓法的完整實(shí)現(xiàn)代碼,，結(jié)合代碼進(jìn)行理解和分析,，事半功倍，如果還沒(méi)看懂，那就多看幾遍,。

主程序

主程序中調(diào)用了三個(gè)子函數(shù),，其中：

ResetMatrices，用于初始化矩陣變量

UpdateMatrices,，用于計(jì)算求解 $\bigtriangleup$ 所用到的 $J_r(\beta_\alpha)^T J_r(\beta_\alpha)$ (Hessian矩陣）和 $J_r(\beta_\alpha)^T r(\beta_\alpha)$ 這兩個(gè)矩陣

GaussEliminateSolveDelta,，使用高斯消元法求解 $\bigtriangleup$

事實(shí)上，在實(shí)際應(yīng)用中使用高斯牛頓法,，會(huì)出現(xiàn)許許多多的問(wèn)題,，因此下面介紹一種基于高斯牛頓改良優(yōu)化而來(lái)的算法。

七.Levenberg-Marquardt法

實(shí)際應(yīng)用中,，高斯牛頓法有著如下缺點(diǎn)：

初始點(diǎn)選取較為嚴(yán)苛,，若初始點(diǎn)距離極小值點(diǎn)過(guò)遠(yuǎn)，迭代步長(zhǎng)過(guò)大會(huì)導(dǎo)致迭代下一代的函數(shù)值不一定小于上一代的函數(shù)值
高斯牛頓法中的Hessian矩陣在某些情況下可能是非正定的（不可逆）
殘差過(guò)大時(shí)可能導(dǎo)致高斯牛頓法無(wú)法收斂

而Levenberg-Marquardt法（LM法,，又稱(chēng)阻尼最小二乘法）改善了高斯牛頓法的不足之處,，并結(jié)合了高斯牛頓法和梯度下降法的優(yōu)點(diǎn)，而實(shí)際上所做的改動(dòng)非常小,。

在高斯牛頓法中,，迭代步長(zhǎng) $\bigtriangleup$ 被定義為
$\bigtriangleup = [H_S(\beta_\alpha)]^{-1}\bigtriangledown S(\beta_\alpha)$
而在LM法中，加入了阻尼因子 $\mu$ ,，有
$\bigtriangleup = [H_S(\beta_\alpha) + \mu I]^{-1}\bigtriangledown S(\beta_\alpha)$
從而消除了Hessian矩陣非正定的情況,，更重要的是迭代過(guò)程中阻尼因子 $\mu$ 是可變的，用于調(diào)整下降方向,，當(dāng)：

$\mu$ 趨近于0時(shí),，算法退化為高斯牛頓法
$\mu$ 很大時(shí)，效果相當(dāng)于梯度下降法

使用LM法時(shí),，需要加入一些 $\mu$ 的調(diào)整策略,，使得距離解較遠(yuǎn)的時(shí)候，使用梯度下降法,，距離極小值較近的時(shí)候,，使 $\mu$ 減小，這樣就近似高斯牛頓法,，能得到比梯度下降法更快的收斂速度,。

算法的具體實(shí)現(xiàn)這里就不貼出來(lái)了，可以到這里查看：天穹飛控中的LM法

八.傳感器數(shù)據(jù)采集

前幾節(jié)介紹了如何使用非線(xiàn)性最小二乘法校準(zhǔn)傳感器,，下面簡(jiǎn)單說(shuō)明校準(zhǔn)時(shí)采集所需傳感器數(shù)據(jù)的具體步驟,。

1.加速度計(jì)

在天穹飛控中，加速度校準(zhǔn)使用標(biāo)準(zhǔn)的六面采集法,，需要將飛控按照上,、下,、前、后,、左,、右六個(gè)方向靜止放置一段時(shí)間（2s左右），順序隨意且飛控方向大致準(zhǔn)確即可,，飛控自動(dòng)檢測(cè)當(dāng)前加速度計(jì)方向,，并采集多組數(shù)據(jù)取平均值，最后得到6組方向各異的數(shù)據(jù),，導(dǎo)入LM校準(zhǔn)函數(shù),，便能求出加速度計(jì)的6個(gè)校準(zhǔn)參數(shù)。

加速度校準(zhǔn)采集代碼

天穹飛控兼容mavlink,，可以使用QGroundControl地面站完成加速度計(jì)的校準(zhǔn)步驟,，如下圖：

使用QGC校準(zhǔn)加速度計(jì).jpg

點(diǎn)擊左側(cè)的Accelerometer按鈕，確認(rèn)OK,，飛控便會(huì)進(jìn)入加速度校準(zhǔn)流程,，同時(shí)QGC會(huì)顯示出當(dāng)前校準(zhǔn)進(jìn)度以及圖片提示，當(dāng)對(duì)應(yīng)方向采集數(shù)據(jù)完畢時(shí),，對(duì)應(yīng)圖片外框會(huì)變綠,，紅色的則是還沒(méi)有采集數(shù)據(jù)的方向。

2.磁力計(jì)

磁力計(jì)的數(shù)據(jù)采集方式采用比較通用的兩圈旋轉(zhuǎn)方式,，即：將飛控水平旋轉(zhuǎn)一圈,，然后再豎直旋轉(zhuǎn)一圈。在這個(gè)過(guò)程中,，記錄下各軸的最大和最小值數(shù)據(jù),。

磁力計(jì)校準(zhǔn)采集代碼

同樣可以使用QGC進(jìn)行磁力計(jì)校準(zhǔn)，點(diǎn)擊Compass并確認(rèn)OK,。開(kāi)始按照上述步驟旋轉(zhuǎn)飛控,，完成校準(zhǔn)。

然而相對(duì)加速度計(jì)校準(zhǔn)而言,，磁力計(jì)校準(zhǔn)對(duì)算法的要求更高,。由于加速度計(jì)校準(zhǔn)環(huán)境通常比較單一，且靜止?fàn)顟B(tài)下加速度計(jì)數(shù)據(jù)基本不受外界干擾,，所以采集到的數(shù)據(jù)都是比較準(zhǔn)確的,，使得算法很容易收斂，實(shí)測(cè)大概只需4-5次的迭代運(yùn)算,，就可以達(dá)到所需精度。

而磁力計(jì)數(shù)據(jù)采集在實(shí)際應(yīng)用過(guò)程中,，可能存在以下問(wèn)題：

校準(zhǔn)環(huán)境比較復(fù)雜,，而且校準(zhǔn)頻率非常頻繁,。用戶(hù)可能是在室內(nèi)校準(zhǔn)，也可能是在城市的建筑間校準(zhǔn),，或者是在充滿(mǎn)了未知磁場(chǎng)干擾的環(huán)境下校準(zhǔn),。這便導(dǎo)致了在校準(zhǔn)期間，可能由于各種未知的外部干擾,，使得磁力計(jì)數(shù)據(jù)采集出現(xiàn)“野值”
為了簡(jiǎn)化校準(zhǔn)步驟,，我們采用了較為簡(jiǎn)單的兩圈旋轉(zhuǎn)方式，但由于地磁場(chǎng)的分布方向原因,，實(shí)際上這樣我們采集到的磁力計(jì)數(shù)據(jù)并不是在整個(gè)球面上分布均勻的,，如下圖所示（圖片來(lái)自正在開(kāi)發(fā)中的天穹地面站）：

磁力計(jì)數(shù)據(jù)分布示意圖

為了解決問(wèn)題1，我們對(duì)采集到的數(shù)據(jù)做了一定的濾波平滑處理,，同時(shí)實(shí)時(shí)計(jì)算數(shù)據(jù)的一個(gè)平均模值,，當(dāng)新值的大小超過(guò)平均模值一定比例時(shí)，拋棄這個(gè)值,，避免在構(gòu)建誤差方程組時(shí),，引入包含較大干擾量的觀測(cè)值，導(dǎo)致算法無(wú)法收斂或最終計(jì)算出來(lái)的傳感器校準(zhǔn)參數(shù)存在誤差,。

問(wèn)題2考驗(yàn)了算法的性能,，即在樣本分布不均勻的情況下是否依然可以收斂，經(jīng)過(guò)測(cè)試和驗(yàn)證,，天穹飛控中使用的LM法是能夠經(jīng)受得住這個(gè)考驗(yàn)的,。

九.總結(jié)

本篇文章介紹了飛控中最基本的傳感器誤差問(wèn)題，并提出了有效的解決方法,。其中所使用到的非線(xiàn)性最小二乘算法,，在現(xiàn)代計(jì)算機(jī)工程領(lǐng)域有著非常廣泛的應(yīng)用，所以即使對(duì)飛控技術(shù)興趣不大的童鞋,，本文也是值得一看的,。

而對(duì)于我們的飛控而言，完成了傳感器基本誤差的校準(zhǔn),，只是相當(dāng)于邁出了第一步,，后面還有無(wú)數(shù)難題待解決，再接下來(lái)的教程中,，會(huì)陸續(xù)針對(duì)不同的問(wèn)題尋求最優(yōu)的解決方案,，當(dāng)然由于本人水平有限，只能起到拋磚引玉的作用,，希望有更多的有識(shí)之士,，加入這個(gè)開(kāi)源飛控項(xiàng)目，一起完善,，共同進(jìn)步,。

本站是提供個(gè)人知識(shí)管理的網(wǎng)絡(luò)存儲(chǔ)空間,，所有內(nèi)容均由用戶(hù)發(fā)布，不代表本站觀點(diǎn),。請(qǐng)注意甄別內(nèi)容中的聯(lián)系方式,、誘導(dǎo)購(gòu)買(mǎi)等信息，謹(jǐn)防詐騙,。如發(fā)現(xiàn)有害或侵權(quán)內(nèi)容,，請(qǐng)點(diǎn)擊一鍵舉報(bào)。

轉(zhuǎn)藏 分享

QQ空間 QQ好友新浪微博微信

獻(xiàn)花（0） +1

來(lái)自：昵稱(chēng)59692568 > 《數(shù)學(xué)》

舉報(bào)/認(rèn)領(lǐng)