詳解：三邊測(cè)距定位算法
[導(dǎo)讀]“定位”這個(gè)概念想必大家并不陌生，可以說(shuō)定位技術(shù)的迅猛發(fā)展，給我們的日常生活帶來(lái)了極大的便利。比如，手機(jī)的定位導(dǎo)航系統(tǒng)讓“路癡”再也不怕出遠(yuǎn)門了；航天飛行器可以自由地穿梭于浩瀚的宇宙卻逃不出人類的掌控；無(wú)人駕駛技術(shù)更是離不開定位系統(tǒng)的支撐……通過(guò)GPS定位系統(tǒng)實(shí)現(xiàn)無(wú)人機(jī)的空中懸停...
“定位”這個(gè)概念想必大家并不陌生，可以說(shuō)定位技術(shù)的迅猛發(fā)展，給我們的日常生活帶來(lái)了極大的便利。
比如，手機(jī)的定位導(dǎo)航系統(tǒng)讓“路癡”再也不怕出遠(yuǎn)門了；航天飛行器可以自由地穿梭于浩瀚的宇宙卻逃不出人類的掌控；無(wú)人駕駛技術(shù)更是離不開定位系統(tǒng)的支撐……
通過(guò)GPS定位系統(tǒng)實(shí)現(xiàn)無(wú)人機(jī)的空中懸停定點(diǎn)飛行
接下來(lái)，將向大家介紹一種簡(jiǎn)單的定位算法：三邊測(cè)距定位算法。
【簡(jiǎn)介】
簡(jiǎn)單地說(shuō)，實(shí)現(xiàn)定位你只需要做好以下兩點(diǎn)：測(cè)量值? 從某種意義上說(shuō)，幾乎所有你能測(cè)量的數(shù)據(jù)都取決于“位置”，并且可以利用測(cè)得的數(shù)據(jù)進(jìn)行定位。最理想的情況是測(cè)量那些對(duì)“位置”非常敏感的數(shù)據(jù)。例如，測(cè)量溫度可能會(huì)知道你身在哪個(gè)大陸，當(dāng)然這樣的結(jié)果并不準(zhǔn)確，但是如果測(cè)量的是你相對(duì)于某些點(diǎn)的距離或者角度，那么對(duì)你的位置的定位可能會(huì)更精確一些。參考點(diǎn) ?描述一個(gè)確定的位置，正確的方法就是描述它相對(duì)于某些參考點(diǎn)的位置。參考點(diǎn)可以是“你的家”、“北極星”或“天空中的一些衛(wèi)星”。對(duì)我們來(lái)說(shuō)，我們將使用錨。利用3個(gè)錨點(diǎn)就可以描述一個(gè)二維的坐標(biāo)系統(tǒng)，在這個(gè)系統(tǒng)中我們可以找到我們的位置。對(duì)于三維定位系統(tǒng)，我們則需要4個(gè)錨點(diǎn)。
下面是一些例子:
| 定位系統(tǒng) | 測(cè)量值 | 參考點(diǎn) |
| GPS | 距離* | 衛(wèi)星 |
| Pozyx | 距離 | 錨 |
| 攝像機(jī) | 視頻圖像 | 相機(jī)指定方向 |
| WiFi、指紋識(shí)別 | 接收信號(hào)強(qiáng)度 | 數(shù)據(jù)庫(kù)中的指紋 |
| 數(shù)字羅盤 | 磁場(chǎng)矢量 | 磁場(chǎng)北極 |
| 航位推算 | 加速度和角速度（來(lái)自陀螺儀） | 最初的位置和方向 |
注意：一些GPS接收器也使用多普勒頻移定位
【三邊測(cè)距法】
最常用的定位方法是使用基本的幾何圖形來(lái)估計(jì)位置。通過(guò)測(cè)量與錨點(diǎn)的距離，就可以確定你的位置。如果我們只知道自己與錨點(diǎn)的距離，那么我們的位置肯定會(huì)在以錨點(diǎn)P為圓心以測(cè)得距離d為半徑的圓上。如果我們用3個(gè)錨進(jìn)行距離測(cè)量，我們會(huì)發(fā)現(xiàn)我們的位置在三個(gè)圓的交點(diǎn)上，如圖1所示。這種方法被稱為三邊測(cè)距法（如果使用的錨點(diǎn)數(shù)量超過(guò)3個(gè)，則稱為多邊測(cè)距法）。這種方法的困難在于測(cè)量中總會(huì)有一些噪音，測(cè)量并不完美。因此，圓不會(huì)在一個(gè)點(diǎn)相交。為了解決這個(gè)問題，我們?cè)囍页鲎罱咏袌A的點(diǎn)。
圖1：三邊測(cè)量?注：你可以選擇跳過(guò)該算法的描述，直接參閱下一篇文章：超寬帶是如何工作的。【一個(gè)基本的算法】
在本節(jié)中，我們將介紹一種簡(jiǎn)單的算法，它可以從一系列的范圍測(cè)量中計(jì)算出位置。這個(gè)基本算法不是最優(yōu)的，但是當(dāng)范圍測(cè)量足夠精確時(shí)，它就會(huì)很好地工作。我們將解釋2D定位的算法。位置P由坐標(biāo)x和y給出。第i個(gè)錨點(diǎn)pi的位置坐標(biāo)為(xi,yi)，如果我們有N個(gè)錨，那么i的取值為從1到N，這些錨點(diǎn)的坐標(biāo)是已知的。
現(xiàn)在，位置P與第i個(gè)錨點(diǎn)之間的距離由di表示，di由下面的公式給出：
?對(duì)等式兩邊取平方：? ? 上面方程的問題在于含有非線性項(xiàng)x^2和y^2。我們可以通過(guò)從di^2中減去dN^2來(lái)消除這些非線性項(xiàng)，得到N-1個(gè)方程，其中第i個(gè)方程式為：
現(xiàn)在我們有了一些關(guān)于坐標(biāo)x和y的線性方程，這很好，因?yàn)榫€性方程很容易求解。我們把它寫成矩陣的形式：
其中：我們現(xiàn)在可以解這個(gè)方程組了。
如果我們恰好有3個(gè)錨點(diǎn)：N=3，我們就會(huì)得到兩個(gè)方程來(lái)求解兩個(gè)未知數(shù)，通過(guò)求解下面的方程，我們可以找到P的位置：
如果我們有超過(guò)3個(gè)錨點(diǎn)：N>3，我們得到的方程的數(shù)量要多于未知數(shù)的數(shù)量，此時(shí)A的逆矩陣是不存在的。為了解決這個(gè)問題，我們可以利用偽逆算子來(lái)計(jì)算這個(gè)位置。這就產(chǎn)生了下面的方程式：
請(qǐng)注意，上面的公式將盡可能地將坐標(biāo)x和y與所有不同的方程相匹配。因此，當(dāng)使用更多的錨時(shí)，定位的準(zhǔn)確性也會(huì)隨之提高。
上面描述的算法被稱為線性最小二乘算法。“線性”是因?yàn)槲覀儼逊匠踢M(jìn)行了線性化（通過(guò)平方）和“最小二乘”是因?yàn)榫仃嚨模▊危┠婢仃嚂?huì)使所有方程的平方誤差最小化。
【拓展】
上面描述的算法是一種非常簡(jiǎn)單和低復(fù)雜度的算法。如果你想了解更多關(guān)于先進(jìn)定位技術(shù)的知識(shí)，建議你尋找以下主題：非線性最小二乘、卡爾曼濾波、粒子濾波、置信傳播……作者：趙得江來(lái)源：平行機(jī)器人版權(quán)歸原作者所有，如有侵權(quán)，請(qǐng)聯(lián)系刪除。
下載文檔
