基於EKF/EFIR混合濾波的室內移動機器人定位方法和裝置與流程
2023-06-08 06:49:36 3
本發明涉及複雜環境下組合定位技術領域，尤其涉及一種基於ekf/efir混合濾波的室內移動機器人定位方法。
背景技術：
近年來，隨著科學技術的發展和人民生活水平的提高，機器人逐步走進人們的生活。作為機器人為人類提供高質量服務的基礎，面向機器人的定位正逐漸成為該領域的研究熱點。
在機器人定位技術方面，為了克服全球定位系統(globalpositioningsystem,gps)為代表的全球導航衛星系統(globalnavigationsatellitesystem,gnss)在室內環境下無法克服因信號受遮擋導致定位精度下降甚至失鎖的缺點，學者們提出將無線傳感器網絡(wirelesssensorsnetwork,wsn)技術應用到室內機器人定位領域。例如sweattm.等提出基於wifi的室內移動機器人通信和定位算法；miahm.s.等對基於射頻識別(radiofrequencyidentification,rfid)技術的移動機器人定位算法進行了研究。需要指出的是，上述無線定位技術的精度為米級，不能滿足室內機器人高精度導航定位的要求。為了獲取更高的定位精度，學者們提出將超寬帶技術(ultrawideband，uwb)應用於移動機器人室內導航定位中[8]。但是需要指出的是，傳統的室內定位方法都需要預先獲取參考節點的位置信息，這限制了這一技術的靈活性。
在數據融合方法方面，目前應用最為廣泛的是卡爾曼濾波算法(kalmanfilter,kf)及其相應的改進方法，如擴展卡爾曼濾波(extendedkf,ekf)、無跡卡爾曼濾波(unscentedkf,ukf)等。但需要指出的是kf及其改進算法的精度依賴於濾波模型的精度，特別是噪聲特性的描述。然而在實際應用中很難獲取機器人所處導航環境的精確環境噪聲信息。為了克服這一問題，學者們提出了基於有限脈衝響應結構(finiteimpulseresponse,fir)的數據融合算法，以提高濾波算法的魯棒性。例如shmaliyys提出了一種面向線性系統的fir濾波器，並將其應用到機器人無線定位領域。但需要指出的是，fir濾波器雖然具有較高的魯棒性，但其精度略差於獲取到精確噪聲描述的kf及其相應改進算法。除此之外，有限脈衝響應結構也使得fir濾波器的運行效率弱於kf。
目前需要本領域技術人員迫切解決的一個技術問題是：如何在定位過程中既保證精度又兼顧效率。
技術實現要素：
為了解決上述問題，本發明提供了一種基於ekf/efir混合濾波的室內移動機器人定位方法，該方法構建uwb定位標籤所獲取到的距離(即參考節點與目標機器人之間的距離)信息、目標機器人的位置和速度信息之間的模型，以降低室內複雜導航環境對組合導航精度的影響，為濾波器完成對導航信息的高精度預估打下基礎。
為了實現上述目的，本發明採用如下技術方案：
一種基於ekf/efir混合濾波的室內移動機器人定位方法，包括以下步驟：
(1)以目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置作為狀態量，以uwb定位標籤測量得到的參考節點與定位標籤之間的距離作為系統觀測量構建uwb機器人定位模型；
(2)以所述uwb機器人定位模型為基礎，利用uwb定位標籤測量得到的某參考節點與定位標籤之間的距離信息，通過ekf/efir混合濾波器估計機器人的位置以及參考節點的位置：首先採用ekf算法進行預估，檢測ekf算法性能，若未能達到預設精度，則用efir濾波器對ekf算法進行補充，最終得到當前時刻目標機器人最優的位置信息。
作為狀態量的目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置的初值根據實際需要自行設定。
所述ekf/efir混合濾波器的狀態方程為：
其中，(xk，yk)分別為移動機器人在k時刻的x和y方向的位置；(vxk，vyk)分別為移動機器人在k時刻的x和y方向的速度；t為採樣周期；為k時刻參考節點的位置向量，表示參考節點在k時刻在x和y方向的位置，其中，l為參考節點的數目；ωk為滿足均值為零正態分布的系統噪聲，其協方差矩陣為q。
所述ekf/efir混合濾波器的觀測方程為：
其中，yk＝[d1d2…dl]t為k時刻通過uwb定位系統測量的到的參考節點與未知節點之間的距離；vk為系統的觀測噪聲矩陣，其協方差矩陣為r，表示參考節點k時刻在x和y方向的位置，l為參考節點的數目。
所述ekf/efir濾波器中ekf算法的迭代過程為：
其中，表示ekf在k時刻預估的狀態向量，表示ekf由k-1時刻到k時刻預估的狀態向量，fk表示表示k時刻的系統矩陣，表示ekf由k-1時刻到k時刻的最小預測均方誤差矩陣；表示ekfk時刻的最小預測均方誤差矩陣；kk表示ekf在k時刻的誤差增益矩陣；i表示單位陣。
所述ekf/efir濾波器中ekf算法的性能採用信道質量評價指標dk來評價：
判斷dk與門限door的關係，如果dk<door，則信道質量正常，反之，則信道質量差，進入efir濾波器。
所述ekf/efir濾波器中efir算法的迭代過程為：
首先，若當前採樣時刻大於efir預先設置的窗口n，定義m＝k-n+1，s＝m+m-1，gs＝i；
然後，從s+1時刻到k時刻，執行以下迭代過程：
其中，表示efir在j時刻預估的狀態向量，表示efir由j-1時刻到j時刻預估的狀態向量，fj表示表示j時刻的系統矩陣，表示efir由j-1時刻到j時刻的最小預測均方誤差矩陣；表示efirj時刻的最小預測均方誤差矩陣；kj表示ekf在j時刻的誤差增益矩陣；i表示單位陣；
最後，
所述ekf/efir混合濾波器的迭代過程為：
首先採用ekf濾波器進行一步預估：
若dk＜door，則繼續進行ekf濾波器的更新：
pk＝(i-kkhk)pk|k-1
反之，則信道質量差，進入efir濾波器；
若當前採樣時刻大於efir預先設置的窗口n，定義m＝k-n+1，s＝m+m-1，gs＝1，
從s+1時刻到k時刻，執行以下迭代過程：
其中，表示efir在j時刻預估的狀態向量，表示efir由j-1時刻到j時刻預估的狀態向量，fj表示表示j時刻的系統矩陣，表示efir由j-1時刻到j時刻的最小預測均方誤差矩陣；表示efirj時刻的最小預測均方誤差矩陣；kj表示ekf在j時刻的誤差增益矩陣；i表示單位陣。
最後，
本發明還提供了一種基於ekf/efir混合濾波的室內移動機器人定位裝置，包括：
uwb機器人定位模型構建模塊，以目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置作為狀態量，以uwb定位標籤測量得到的參考節點與定位標籤之間的距離作為系統觀測量構建uwb機器人定位模型；
機器人定位模塊，以所述uwb機器人定位模型為基礎，利用uwb定位標籤測量得到的某參考節點與定位標籤之間的距離信息，通過ekf/efir混合濾波器估計機器人的位置以及參考節點的位置：首先採用ekf算法進行預估，檢測ekf算法性能，若未能達到預設精度，則用efir濾波器對ekf算法進行補充，最終得到當前時刻目標機器人最優的位置信息。
本發明的有益效果：
1、移動機器人定位方法採用改進的uwb定位模型，該模型以目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置作為狀態量，以uwb定位標籤測量得到的參考節點與定位標籤之間的距離作為系統觀測量構建uwb機器人定位模型；在此基礎上，通過ekf/efir濾波將uwb獲取的參考節點到未知節點之間的距離信息進行數據融合，最終得到當前時刻最優的目標機器人位置信息和對參考節點位置信息的預估。提高了數據融合濾波器的精度和魯棒性。
2、可用於室內環境下的移動機器人中高精度定位。
附圖說明
圖1為一種基於ekf/efir混合濾波的室內移動機器人定位方法的示意圖。
具體實施例
下面結合附圖對本發明進行詳細說明：
實施例1：
本實施例提供了一種基於ekf/efir混合濾波的室內移動機器人定位方法，示意圖如圖1，該方法包括：
(1)以目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置作為狀態量，以uwb定位標籤測量得到的參考節點與定位標籤之間的距離作為系統觀測量構建uwb機器人定位模型；
其中，作為狀態量的目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置的初值根據實際需要自行設定。
參考節點位置可以任意選定，本實施例中要求參考節點靜止。
(2)以所述uwb機器人定位模型為基礎，利用uwb定位標籤測量得到的某參考節點與定位標籤之間的距離信息，通過ekf/efir混合濾波器估計機器人的位置以及參考節點的位置：首先採用ekf算法進行預估，檢測ekf算法性能，若未能達到預設精度，則用efir濾波器對ekf算法進行補充，最終得到當前時刻目標機器人最優的位置信息。
其中，所述ekf/efir混合濾波器的狀態方程為：
其中，(xk，yk)分別為移動機器人在k時刻的x和y方向的位置；(vxk，vyk)分別為移動機器人在k時刻的x和y方向的速度；t為採樣周期；為k時刻參考節點的位置向量，表示參考節點在k時刻在x和y方向的位置，其中，l為參考節點的數目；ωk為滿足均值為零正態分布的系統噪聲，其協方差矩陣為q。
所述ekf/efir混合濾波器的觀測方程為：
其中，yk＝[d1d2…dl]t為k時刻通過uwb定位系統測量的到的參考節點與未知節點之間的距離；vk為系統的觀測噪聲矩陣，其協方差矩陣為r，表示參考節點k時刻在x和y方向的位置，l為參考節點的數目。
所述ekf/efir濾波器中ekf算法的迭代過程為：
其中，表示ekf在k時刻預估的狀態向量，表示ekf由k-1時刻到k時刻預估的狀態向量，fk表示表示k時刻的系統矩陣，表示ekf由k-1時刻到k時刻的最小預測均方誤差矩陣；表示ekfk時刻的最小預測均方誤差矩陣；kk表示ekf在k時刻的誤差增益矩陣；i表示單位陣。
所述ekf/efir濾波器中ekf算法的性能採用信道質量評價指標dk來評價：
判斷dk與門限door的關係，如果dk<door，則信道質量正常，反之，則信道質量差，進入efir濾波器。
所述無線通信信道中ekf/efir濾波器中efir算法的迭代過程為：
首先，若當前採樣時刻大於efir預先設置的窗口n，定義m＝k-n+1，，，m。m-1·gs，1；
然後，從s+1時刻到k時刻，執行以下迭代過程：
其中，表示efir在j時刻預估的狀態向量，表示efir由j-1時刻到j時刻預估的狀態向量，fj表示表示j時刻的系統矩陣，表示efir由j-1時刻到j時刻的最小預測均方誤差矩陣；表示efirj時刻的最小預測均方誤差矩陣；kj表示ekf在j時刻的誤差增益矩陣；i表示單位陣；
最後，
所述ekf/efir混合濾波器的迭代過程為：
首先採用ekf濾波器進行一步預估：
若dk＜door，則繼續進行ekf濾波器的更新：
pi＝(l-kkhk)pk|k-1
反之，則信道質量差，進入efir濾波器；
若當前採樣時刻大於efir預先設置的窗口n，定義m＝k-n+1，s＝m+m-1，gs＝1，
從s+1時刻到k時刻，執行以下迭代過程：
其中，表示efir在j時刻預估的狀態向量，表示efir由j-1時刻到j時刻預估的狀態向量，fj表示表示j時刻的系統矩陣，表示efir由j-1時刻到j時刻的最小預測均方誤差矩陣；表示efirj時刻的最小預測均方誤差矩陣；kj表示ekf在j時刻的誤差增益矩陣；i表示單位陣。
最後，
為了更清楚地表達ekf/efir濾波器的算法運行過程，將運行過程進行如下表示。
ekf/efir濾波器中ekf算法的迭代過程為：
其中，
ekf/efir濾波器中efir算法的迭代過程為：
ekf/efir混合濾波器的迭代過程為：
實施例2：
基於實施例1所述的方法，本發明還提供了一種基於ekf/efir混合濾波的室內移動機器人定位裝置，包括：
uwb機器人定位模型構建模塊，以目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置作為狀態量，以uwb定位標籤測量得到的參考節點與定位標籤之間的距離作為系統觀測量構建uwb機器人定位模型；
其中，作為狀態量的目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置的初值根據實際需要自行設定。
參考節點位置可以任意選定，本實施例中要求參考節點靜止。
機器人定位模塊，以所述uwb機器人定位模型為基礎，利用uwb定位標籤測量得到的某參考節點與定位標籤之間的距離信息，通過ekf/efir混合濾波器估計機器人的位置以及參考節點的位置：首先採用ekf算法進行預估，檢測ekf算法性能，若未能達到預設精度，則用efir濾波器對ekf算法進行補充，最終得到當前時刻目標機器人最優的位置信息。
本裝置基於實施例1中所述的方法，ekf/efir混合濾波器具體的算法流程可參見實施例1。
本發明對uwb定位模型進行了改進，以目標機器人x向和y向的位置和速度以及各uwb參考節點的x向和y向位置作為狀態量，以uwb定位標籤測量得到的參考節點與定位標籤之間的距離作為系統觀測量，提高了數據融合濾波器的精度和魯棒性；採用本發明的方法，提高了室內環境下的移動機器人的定位精度。
本領域技術人員應該明白，上述的本發明的各模塊或各步驟可以用通用的計算機裝置來實現，可選地，它們可以用計算裝置可執行的程序代碼來實現，從而，可以將它們存儲在存儲裝置中由計算裝置來執行，或者將它們分別製作成各個集成電路模塊，或者將它們中的多個模塊或步驟製作成單個集成電路模塊來實現。本發明不限制於任何特定的硬體和軟體的結合。
上述雖然結合附圖對本發明的具體實施方式進行了描述，但並非對本發明保護範圍的限制，所屬領域技術人員應該明白，在本發明的技術方案的基礎上，本領域技術人員不需要付出創造性勞動即可做出的各種修改或變形仍在本發明的保護範圍以內。