發(fā)明涉及一種機(jī)器人狀態(tài)估計方法，具體涉及一種依據(jù)圖優(yōu)化理論，融合UWB定位坐標(biāo)與編碼器里程信息的機(jī)器人位姿估計方法。

背景技術(shù)：

GPS全球衛(wèi)星定位系統(tǒng)已經(jīng)在室外有了廣泛應(yīng)用，但是在室內(nèi)由于信號衰減無法準(zhǔn)確定位。于是，基于藍(lán)牙、WIFI、UWB信號的室內(nèi)基站補(bǔ)充了GPS的不足。目前，無論是室外的GPS還是室內(nèi)基站，都只能給出定位標(biāo)簽的坐標(biāo)信息，而無法給出被定位對象的對方向信息。在汽車上，方向信息是不斷定位求差分得到的，這是基于汽車移動速度較快，且無法橫向移動這兩點(diǎn)推測出的。在室內(nèi)較慢移動的機(jī)器人則無法使用此法。也可增加一個定位標(biāo)簽，每臺設(shè)備上使用兩個標(biāo)簽的坐標(biāo)差向量來給出方向信息，但這會增加一個標(biāo)簽的成本，這對絕大多數(shù)低成本機(jī)器人是不可接受的。

技術(shù)實(shí)現(xiàn)要素：

本申請解決了在不額外增加定位標(biāo)簽的前提下，結(jié)合UWB定位坐標(biāo)與輪子編碼器信息融合估計雙主動輪機(jī)器人的位置與方向測量的問題。

為解決上述技術(shù)問題，本申請采用以下技術(shù)方案：一種基于圖優(yōu)化的UWB定位與編碼器融合位姿估計方法,包括以下步驟:

1、傳感器的安裝:

傳感器包括基站和定位標(biāo)簽，基站的安裝高度至少大于機(jī)器人；定位標(biāo)簽需安裝在離機(jī)器人自轉(zhuǎn)中心點(diǎn)有一段距離的地方，此距離需要大于定位算法的誤差的標(biāo)準(zhǔn)差3倍以上，該誤差指根據(jù)傳感器測量的機(jī)器人坐標(biāo)與機(jī)器人真實(shí)坐標(biāo)之間的距離，且定位標(biāo)簽不在機(jī)器人自轉(zhuǎn)轉(zhuǎn)軸上；

2、初始位姿的獲取:

.機(jī)器人開機(jī)后先測出定位標(biāo)簽的坐標(biāo)；

.然后以小于10秒一圈的速度自轉(zhuǎn)一圈，同時在自轉(zhuǎn)過程中不斷定位，得到一組坐標(biāo)，將這組坐標(biāo)取平均即可得到機(jī)器人自轉(zhuǎn)中心點(diǎn)坐標(biāo)；

c.由定位標(biāo)簽坐標(biāo)以及自轉(zhuǎn)中心點(diǎn)坐標(biāo)，求得機(jī)器人的初始位姿。初始位姿的位置由中心點(diǎn)坐標(biāo)給出，初始位姿的方向由中心點(diǎn)坐標(biāo)和定位標(biāo)簽坐標(biāo)的連線給出；

3、迭代:

a.機(jī)器人初始位姿共x、y、θ這3個自由度，x、y、θ分別表示機(jī)器人位置的橫向坐標(biāo)、縱向坐標(biāo)以及機(jī)器人的朝向，考慮到測量精度，此處存在3個誤差。

b.在機(jī)器人在運(yùn)動過程中，讀出一系列由編碼器輸出的測量值。兩個輪子不妨設(shè)為2n個測量值，這2n個測量值對應(yīng)2n個誤差。

c.根據(jù)上述2n+3個變量，使用經(jīng)典的測程法可以計算出機(jī)器人的當(dāng)前位姿，計算的位姿即為機(jī)器人位姿的最優(yōu)估計。

d.機(jī)器人在運(yùn)動過程中根據(jù)UWB定位數(shù)據(jù)以及經(jīng)典的GPS定位算法得到一個二自由度的定位坐標(biāo)，此處只考慮機(jī)器人做平面平行運(yùn)動，該坐標(biāo)與上述基于測程法計算出的位姿中的位置必然不相等，存在一個距離偏差dis。加上上述2n+3個誤差總共2n+4個誤差,此2n+4個誤差的自由度是2n+3。

e.定義2n+4個誤差的二范數(shù)為位姿估計總誤差，或者說位姿估計的評價函數(shù)，考慮到前2n+3個誤差將決定最后一個誤差，則位姿估計評價函數(shù)可以看作前2n+3個誤差的函數(shù)，優(yōu)化目標(biāo)為位姿估計評價函數(shù)取值最小。

f.對2n+3個自變量的位姿估計評價函數(shù)求偏微分，得到梯度，向負(fù)梯度方向迭代，直到函數(shù)值最小，此時使用前2n+3個誤差，加上前2n+3個測量值得到對2n+3個真值的估計，用這2n+3個估計使用測程法計算出最后對機(jī)器人位姿的估計，則此估計是在二范數(shù)意義下對機(jī)器人位姿的最優(yōu)估計。

有益效果，該方法采用UWB定位與編碼器融合來估算機(jī)器人位姿，對機(jī)器人位姿的估計精度高、計算量少，在一般手機(jī)使用的芯片上可實(shí)時計算。

附圖說明

圖1 .編碼器里程估計的位姿，與UWB定位點(diǎn)構(gòu)成的矢量圖；

圖2.算法計算過程流程圖。

具體實(shí)施方式

一種基于圖優(yōu)化的UWB定位與編碼器融合位姿估計方法,包括以下步驟:

（1）傳感器的安裝

傳感器包括基站和定位標(biāo)簽，基站的安裝高度至少大于機(jī)器人；定位標(biāo)簽需安裝在離機(jī)器人自轉(zhuǎn)中心點(diǎn)有一段距離的地方，此距離需要大于定位算法的誤差的標(biāo)準(zhǔn)差3倍以上，該誤差指根據(jù)傳感器測量的機(jī)器人坐標(biāo)與機(jī)器人真實(shí)坐標(biāo)之間的距離，且定位標(biāo)簽不在機(jī)器人自轉(zhuǎn)轉(zhuǎn)軸上。

（2）初始位姿的獲取

a.機(jī)器人開機(jī)后先測出定位標(biāo)簽的坐標(biāo)。

b.然后以小于10秒一圈的速度自轉(zhuǎn)一圈，同時在自轉(zhuǎn)過程中不斷定位，得到一組坐標(biāo)。將這組坐標(biāo)取平均即可得到機(jī)器人自轉(zhuǎn)中心點(diǎn)坐標(biāo)。

c.由定位標(biāo)簽坐標(biāo)以及自轉(zhuǎn)中心點(diǎn)坐標(biāo)，可以求得機(jī)器人的初始位姿。初始位姿的位置由中心點(diǎn)坐標(biāo)給出，初始位姿的方向由中心點(diǎn)坐標(biāo)和定位標(biāo)簽坐標(biāo)的連線給出。

（3）迭代

a.機(jī)器人初始位姿共x、y、θ這3個自由度，x、y、θ分別表示機(jī)器人位置的橫向坐標(biāo)、縱向坐標(biāo)以及機(jī)器人的朝向，考慮到測量精度，此處存在3個誤差。

b. 機(jī)器人在運(yùn)動過程中，讀出一系列編碼器的測量值。兩個輪子不妨設(shè)為2n個測量值，這2n個測量值對應(yīng)2n個誤差。

c.根據(jù)上述2n+3個變量，使用經(jīng)典的測程法可以計算出機(jī)器人的當(dāng)前位姿?？紤]到這2n+3個變量的誤差是不相關(guān)的（或相關(guān)性非常微弱），故據(jù)此使用測程法計算的位姿即為機(jī)器人位姿的最優(yōu)估計。

d. 機(jī)器人在運(yùn)動過程中根據(jù)UWB定位數(shù)據(jù)以及經(jīng)典的GPS定位算法得到一個二自由度的定位坐標(biāo)，此處只考慮機(jī)器人做平面平行運(yùn)動，該坐標(biāo)與上述基于測程法計算出的位姿中的位置必然不相等，存在一個距離偏差dis。加上上述2n+3個誤差總共2n+4個誤差。由于最后一個dis是根據(jù)前2n+3個變量及其誤差計算出來的，則2n+4個測量誤差中的前2n+3個與最后一個具有相關(guān)性（此處由于只關(guān)注定位坐標(biāo)與其他方法計算的位置的距離，故定位坐標(biāo)的x、y兩個分量只得到dis一個誤差），或者說這2n+4個誤差的自由度是2n+3。

e. 定義2n+4個誤差的二范數(shù)為位姿估計總誤差，或者說位姿估計的評價函數(shù)，考慮到前2n+3個誤差將決定最后一個誤差，則位姿估計評價函數(shù)可以看作前2n+3個誤差的函數(shù)。優(yōu)化目標(biāo)為位姿估計評價函數(shù)取值最小。

f. 對2n+3個自變量的位姿估計評價函數(shù)求偏微分，得到梯度，向負(fù)梯度方向迭代，直到函數(shù)值最小，此時使用前2n+3個誤差，加上前2n+3個測量值得到對2n+3個真值的估計，用這2n+3個估計使用測程法計算出最后對機(jī)器人位姿的估計。則此估計是在二范數(shù)意義下對機(jī)器人位姿的最優(yōu)估計。