技術特征：

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

（1）傳感器的安裝:

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

（2）初始位姿的獲取:

.機器人開機后先測出定位標簽的坐標；

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

c.由定位標簽坐標以及自轉中心點坐標，求得機器人的初始位姿。初始位姿的位置由中心點坐標給出，初始位姿的方向由中心點坐標和定位標簽坐標的連線給出；

（3）迭代:

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

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

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

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

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

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