[深入淺出多旋翼飛控開發(fā)][姿態(tài)篇][四][非線性最小二乘與飛控傳感器校準(zhǔn)]

作者：王偉韻
QQ : 352707983
Github

前言

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

不用猜了撰豺，這句話正是鄙人說的:)。飛控的軟硬件相關(guān)工作书幕，絕大部分是圍繞傳感器展開的台汇，它們的重要性苟呐，可能比你想象中的更大牵素。

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

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

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

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

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

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

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

二.傳感器誤差類型

接下來我們會(huì)面臨下面兩個(gè)問題：

• 傳感器有哪些誤差
• 如何測量傳感器的誤差

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

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

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

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

零偏誤差： 指傳感器在零激勵(lì)下的輸出誤差苍糠。如陀螺儀在靜止?fàn)顟B(tài)下，其角速度測量值理論為0啤誊，而實(shí)際上我們將飛控靜止放置岳瞭，陀螺儀依然會(huì)輸出諸如1°/s之類的數(shù)據(jù)，這便是該陀螺儀傳感器的零偏誤差蚊锹。

刻度誤差： 指傳感器在一定激勵(lì)輸入下瞳筏，其輸出值與輸入值的比值。比如我們將飛控放在一個(gè)以500°/s恒定角速度運(yùn)動(dòng)的轉(zhuǎn)臺(tái)上牡昆，而陀螺儀實(shí)際測量得到的角速度輸出為495°/s姚炕，500 / 495 ≈ 1.01 便是該陀螺儀的刻度誤差。

從上述說明中我們可以看出丢烘，要正確測量傳感器的誤差柱宦，必須向傳感器施加特定的激勵(lì)信號(hào)，通過比較傳感器的實(shí)際輸出與激勵(lì)輸入量播瞳，計(jì)算得到傳感器的誤差掸刊。

不同類型的傳感器，所需要的激勵(lì)信號(hào)源也不一樣赢乓。如陀螺儀忧侧，要提供精確的激勵(lì)信號(hào)石窑，必須依靠專業(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è)人愛好者缚柏，就只能暫時(shí)忽略這一項(xiàng)，只校準(zhǔn)陀螺儀的零偏誤差了碟贾，而大多數(shù)情況下币喧，陀螺儀的零偏校準(zhǔn)方式很簡單，使用均值校準(zhǔn)即可袱耽。因此杀餐，陀螺儀的校準(zhǔn)不在本篇文章的討論范圍內(nèi)。

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

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

1.加速度計(jì)

假設(shè)有下面的情形琼讽，我們將飛控水平放置于水平面，測得加速度計(jì)z軸輸出值洪唐，然后將飛控翻轉(zhuǎn)并同樣水平置于水平面钻蹬，再測得加速度計(jì)z軸輸出值。

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

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

將這個(gè)方法推廣到其余兩軸功炮，便能得到其校準(zhǔn)參數(shù)溅潜。

缺陷：

這個(gè)校準(zhǔ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ù)為，而非萝招÷旖铮或者說，這種方式的實(shí)現(xiàn)成本過高槐沼，難以實(shí)際應(yīng)用曙蒸。

2.磁力計(jì)

磁力計(jì)的簡單校準(zhǔn)方法原理等同于加速度計(jì)，即采集得到磁力計(jì)的三個(gè)軸的測量極值（最大與最小值）岗钩，然后單獨(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ì)于飛行器使用者來說可能頗為頻繁视搏，而飛行器大小又各不一致审孽，在很多情況下，我們是很難將飛行器翻來覆去地在各個(gè)方向上進(jìn)行旋轉(zhuǎn)浑娜，其可操作性非常低佑力，且不方便普通用戶的使用。如果旋轉(zhuǎn)不全面棚愤，便無法采集到磁力計(jì)的真實(shí)極值搓萧，校準(zhǔn)效果會(huì)大打折扣杂数，甚至校準(zhǔn)值嚴(yán)重偏離真實(shí)值宛畦。

四.建立傳感器誤差模型

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

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

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

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

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

設(shè)三軸加速度計(jì)的零偏誤差與刻度誤差分別為

某一時(shí)刻該三軸加速度計(jì)的測量值分別為

設(shè)當(dāng)前時(shí)刻真實(shí)加速度向量為

有以下關(guān)系：

靜止?fàn)顟B(tài)下耘纱，不管加速度計(jì)方向如何，其測量得到的加速度為重力加速度在各軸上的投影毕荐。而在地球上束析，重力加速度向量模值總是等于。假設(shè)單位為憎亚，便有以下等式成立：

即

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

五.求解誤差方程

上一節(jié)中丁恭，我們得到了傳感器的誤差方程：
求解方程，對(duì)于經(jīng)歷過九年義務(wù)教育的我們來說是一件非常熟悉的事情斋日，但這里存在著幾個(gè)棘手的問題：

• 該方程為六元二次方程牲览，存在著6個(gè)未知數(shù)，如果只存在一組觀測值恶守，那將會(huì)有無窮多解第献。因此我們需要獲得多組觀測值，以構(gòu)建方程組兔港，從而求出唯一解庸毫，滿足方程組里的每一個(gè)方程

• 對(duì)于六元二次方程而言，使用六組獨(dú)立的觀測值衫樊，構(gòu)建出包含6個(gè)方程的方程組飒赃，理論上可以求出方程組的唯一解】瞥蓿可是事實(shí)上载佳，我們從傳感器上獲取到的觀測值，都是帶有一定隨機(jī)噪聲的臀栈，即觀測值并非完全準(zhǔn)確蔫慧，從而導(dǎo)致求得的方程解未必精確

• 使用計(jì)算機(jī)，我們可以輕松求出線性方程組的唯一精確解（如高斯消元法）权薯，而非線性方程問題姑躲，無論是從理論上還是計(jì)算方法上，都要復(fù)雜得多盟蚣，大部分時(shí)候黍析，對(duì)于無特定形式的非線性方程，是沒有辦法直接求得精確解的屎开。

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

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

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

以傳感器誤差方程為例彬祖，設(shè)為某特定解下方程的誤差，有

對(duì)于方程組而言品抽，其誤差的平方和函數(shù)為

所謂最小二乘法储笑，便是求得一組特定解，使得最小(求函數(shù)極值)圆恤，該條件下的解突倍，便是方程組的最優(yōu)近似解。

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

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

接下來，我們將討論求解非線性最小二乘的具體算法模狭。

六.高斯牛頓法

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

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

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

要搞明白高斯牛頓法构资，我們要先來了解一下牛頓法

1.牛頓法

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

首先給定一個(gè)初始解陨簇，然后在處進(jìn)行一階泰勒展開吐绵，得到

令

求解得到，相比于河绽，有拦赠，即新解比初始解更接近最優(yōu)解。當(dāng)然該解和最優(yōu)解之間還有一定未知的距離葵姥，于是我們將新解作為初始解荷鼠，重復(fù)這一步驟，經(jīng)過不斷地迭代計(jì)算榔幸，最終會(huì)收斂于允乐。

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

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

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

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

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

• 可以看出削咆，牛頓法實(shí)際上等同于尋找函數(shù)的局部極值點(diǎn)牍疏，因此必須選取一個(gè)相對(duì)接近的初始點(diǎn)，否則會(huì)導(dǎo)致無法正確收斂

• 需要求解目標(biāo)函數(shù)的Hessian矩陣（二階導(dǎo)）的逆矩陣拨齐，計(jì)算比較復(fù)雜

2.高斯牛頓法

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

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

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

首先定義目標(biāo)函數(shù)

其中

對(duì)應(yīng)了加速度計(jì)誤差方程中的六個(gè)變量羽峰，即

而為傳感器誤差方程的誤差平方和函數(shù)趟咆，即

設(shè)定一個(gè)初解添瓷，將目標(biāo)函數(shù)在附近泰勒展開，并舍棄高階無窮項(xiàng)值纱，近似有：

我們的目標(biāo)是求出特定解鳞贷，使有最小值。當(dāng)函數(shù)有最小值（屬于極小值）時(shí)虐唠，其一階導(dǎo)為0悄晃，即

從而得到了高斯牛頓法的迭代公式：

設(shè)

則有

這里便是迭代的關(guān)鍵了，給定初解后凿滤，每一次計(jì)算出妈橄，便得到了一個(gè)新解，然后使用新解不斷進(jìn)行迭代計(jì)算翁脆，使其逐漸逼近最優(yōu)解眷蚓。

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

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

中的為函數(shù)的Hessian矩陣（實(shí)際上是S函數(shù)的二階導(dǎo)）

前文提到枫疆，高斯牛頓法對(duì)牛頓法所做的改進(jìn)爵川，便是對(duì)Hessian矩陣進(jìn)行了簡化，其中有

忽略二階微分項(xiàng)息楔，近似有

于是有

這里的實(shí)際上就是函數(shù)（傳感器誤差方程的誤差函數(shù)）的雅可比矩陣

中的另外一個(gè)成員為函數(shù)的一階導(dǎo)的列向量寝贡，即

又有

故可改寫為

于是我們終于得到了的最終形式

或者換一個(gè)形式

這就成了一個(gè)線性方程組，未知量是值依，使用高斯消元法圃泡，便能求解出的唯一解。

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

隨著迭代次數(shù)的增加褒链，最終會(huì)得到一個(gè)無限接近最優(yōu)的解唁情，但是考慮到時(shí)間成本和實(shí)際使用需求疑苔，在校準(zhǔn)程序中我們設(shè)置一個(gè)閾值甫匹，當(dāng)方程解達(dá)到一定精度時(shí)（定義為迭代步長的平方），停止迭代，結(jié)束此次校準(zhǔn)兵迅。

看完了抢韭，還是有點(diǎn)懵，怎么辦恍箭？

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

主程序

/**********************************************************************************************************
*函 數(shù) 名: GaussNewton
*功能說明: 高斯牛頓法求解傳感器誤差方程交洗，得到校準(zhǔn)參數(shù)
*形    參: 傳感器采樣數(shù)據(jù)（6組） 零偏誤差指針 比例誤差指針 數(shù)據(jù)向量長度
*返 回 值: 無
**********************************************************************************************************/
void GaussNewton(Vector3f_t inputData[6], Vector3f_t* offset, Vector3f_t* scale, float length)
{
    uint32_t cnt    = 0;
    double   eps    = 0.000000001;
    double   change = 100.0;
    float    data[3];  
    float    beta[6];      //方程解
    float    delta[6];     //迭代步長
    float    JtR[6];       //梯度矩陣
    float    JtJ[6][6];    //Hessian矩陣
   
    //設(shè)定方程解初值
    beta[0] = beta[1] = beta[2] = 0;
    beta[3] = beta[4] = beta[5] = 1 / length;

    //開始迭代骑科，當(dāng)?shù)介L小于eps時(shí)結(jié)束計(jì)算，得到方程近似最優(yōu)解
    while(change > eps) 
    {
        //矩陣初始化
        ResetMatrices(JtR, JtJ);

        //計(jì)算誤差方程函數(shù)的梯度JtR和Hessian矩陣JtJ
        for(uint8_t i=0; i<6; i++) 
        {
            data[0] = inputData[i].x;
            data[1] = inputData[i].y;
            data[2] = inputData[i].z;
            UpdateMatrices(JtR, JtJ, beta, data);
        }

        //高斯消元法求解方程：JtJ * delta = JtR构拳，得到delta
        GaussEliminateSolveDelta(JtR, JtJ, delta);

        //計(jì)算迭代步長
        change = delta[0]*delta[0] +
                 delta[0]*delta[0] +
                 delta[1]*delta[1] +
                 delta[2]*delta[2] +
                 delta[3]*delta[3] / (beta[3]*beta[3]) +
                 delta[4]*delta[4] / (beta[4]*beta[4]) +
                 delta[5]*delta[5] / (beta[5]*beta[5]);

        //更新方程解
        for(uint8_t i=0; i<6; i++) 
        {
            beta[i] -= delta[i];
        }
            
        //限制迭代次數(shù)
        if(cnt++ > 100)
            break;
    }

    //更新校準(zhǔn)參數(shù)
    scale->x  = beta[3] * length;
    scale->y  = beta[4] * length;
    scale->z  = beta[5] * length;
    offset->x = beta[0];
    offset->y = beta[1];
    offset->z = beta[2];
}

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

• ResetMatrices，用于初始化矩陣變量

static void ResetMatrices(float JtR[6], float JtJ[6][6])
{
    int16_t j,k;
    for(j=0; j<6; j++) 
    {
        JtR[j] = 0.0f;
        for(k=0; k<6; k++) 
        {
            JtJ[j][k] = 0.0f;
        }
    }
}

• UpdateMatrices置森，用于計(jì)算求解所用到的(Hessian矩陣）和這兩個(gè)矩陣

static void UpdateMatrices(float JtR[6], float JtJ[6][6], float beta[6], float data[3])
{
    int16_t j, k;
    float dx, b;
    float residual = 1.0;
    float jacobian[6];
    
    for(j=0; j<3; j++) 
    { 
        b = beta[3+j];
        dx = (float)data[j] - beta[j];
        //計(jì)算殘差 (傳感器誤差方程的誤差)
        residual -= b*b*dx*dx;
        
        //計(jì)算雅可比矩陣
        jacobian[j] = 2.0f*b*b*dx;
        jacobian[3+j] = -2.0f*b*dx*dx;
    }
    
    for(j=0; j<6; j++) 
    {
        //計(jì)算函數(shù)梯度
        JtR[j] += jacobian[j]*residual;
        
        for(k=0; k<6; k++) 
        {
            //計(jì)算Hessian矩陣（簡化形式斗埂，省略二階偏導(dǎo)），即雅可比矩陣與其轉(zhuǎn)置的乘積
            JtJ[j][k] += jacobian[j]*jacobian[k];
        }
    }
}

• GaussEliminateSolveDelta凫海，使用高斯消元法求解

static void GaussEliminateSolveDelta(float JtR[6], float JtJ[6][6], float delta[6])
{
    int16_t i,j,k;
    float mu;
   
    //逐次消元呛凶，將線性方程組轉(zhuǎn)換為上三角方程組
    for(i=0; i<6; i++)
    {
        //若JtJ[i][i]不為0，將該列在JtJ[i][i]以下的元素消為0
        for(j=i+1; j<6; j++)
        {
            mu = JtJ[i][j] / JtJ[i][i];
            if(mu != 0.0f) 
            {
                JtR[j] -= mu * JtR[i];
                for(k=j; k<6; k++)
                {
                    JtJ[k][j] -= mu * JtJ[k][i];
                }
            }
        }
    }

    //回代得到方程組的解
    for(i=5; i>=0; i--)
    {
        JtR[i] /= JtJ[i][i];
        JtJ[i][i] = 1.0f;
        
        for(j=0; j<i; j++)
        {
            mu = JtJ[i][j];
            JtR[j] -= mu * JtR[i];
            JtJ[i][j] = 0.0f;
        }
    }

    for(i=0; i<6; i++)
    {
        delta[i] = JtR[i];
    }
}

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

七.Levenberg-Marquardt法

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

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

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

在高斯牛頓法中，迭代步長被定義為

而在LM法中贮配，加入了阻尼因子谍倦，有

從而消除了Hessian矩陣非正定的情況，更重要的是迭代過程中阻尼因子是可變的泪勒，用于調(diào)整下降方向昼蛀，當(dāng)：

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

使用LM法時(shí)叼旋， 需要加入一些的調(diào)整策略仇哆，使得距離解較遠(yuǎn)的時(shí)候，使用梯度下降法夫植，距離極小值較近的時(shí)候讹剔，使減小，這樣就近似高斯牛頓法详民，能得到比梯度下降法更快的收斂速度延欠。

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

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

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

1.加速度計(jì)

在天穹飛控中，加速度校準(zhǔn)使用標(biāo)準(zhǔn)的六面采集法谒出，需要將飛控按照上隅俘、下、前笤喳、后为居、左、右六個(gè)方向靜止放置一段時(shí)間（2s左右）杀狡，順序隨意且飛控方向大致準(zhǔn)確即可蒙畴，飛控自動(dòng)檢測當(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ì)變綠，紅色的則是還沒有采集數(shù)據(jù)的方向呐矾。

2.磁力計(jì)

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

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

同樣可以使用QGC進(jìn)行磁力計(jì)校準(zhǔn)，點(diǎn)擊Compass并確認(rèn)OK晨炕。開始按照上述步驟旋轉(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í)測大概只需4-5次的迭代運(yùn)算愿阐，就可以達(dá)到所需精度。

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

• 校準(zhǔn)環(huán)境比較復(fù)雜缨历，而且校準(zhǔn)頻率非常頻繁。用戶可能是在室內(nèi)校準(zhǔn)糙麦，也可能是在城市的建筑間校準(zhǔn)辛孵，或者是在充滿了未知磁場干擾的環(huán)境下校準(zhǔn)。這便導(dǎo)致了在校準(zhǔn)期間赡磅，可能由于各種未知的外部干擾魄缚，使得磁力計(jì)數(shù)據(jù)采集出現(xiàn)“野值”

• 為了簡化校準(zhǔn)步驟，我們采用了較為簡單的兩圈旋轉(zhuǎn)方式焚廊，但由于地磁場的分布方向原因冶匹，實(shí)際上這樣我們采集到的磁力計(jì)數(shù)據(jù)并不是在整個(gè)球面上分布均勻的，如下圖所示（圖片來自正在開發(fā)中的天穹地面站）：

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

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

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

九.總結(jié)

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

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

最后再留一下傳送門：

天穹飛控
飛控交流論壇
飛控交流Q群：472648354