[發明專利]基于粒子濾波的機器人定位方法

有效

• 申請號: 202010092204.8
• 申請日: 2020-02-14
• 公開（公告）號: CN111324116B
• 公開（公告）日: 2021-09-21
• 發明（設計）人: 秦小麟;陳駿嶺;李星羅;張彤;鮑斌國
• 申請（專利權）人: 南京航空航天大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 葛瀟敏

1.基于粒子濾波的機器人定位方法，其特征在于，該方法包括如下步驟：

步驟1：在地面上設置J個RFID標簽作為標志點；在機器人上安裝傳感器，用于探測機器人與每個標志點之間的距離；

步驟2：機器人根據工作人員發送的控制指令，從當前位置移動到實際位置D；令工作人員需要機器人移動到的理想位置為u；以理想位置u為中心，令采樣次數為t，進行第一次采樣，即t＝1，采樣n個位置坐標，并作為樣本集，將這n個位置坐標作為樣本粒子；t＝1,2,…,max，max為總的采樣次數；

具體采樣方法為：隨機生成n個整數，根據采樣公式生成與n個整數相對應的n個位置坐標，所述采樣公式為：

其中x_q為第q個整數，X為與x_q對應的位置坐標，Σ為控制采樣的范圍，q＝1,2,…,n；

步驟3：根據樣本集中每個樣本粒子與每個標志點之間的距離和機器人與每個標志點之間的距離，計算每個樣本粒子的匹配度，將匹配度歸一化處理，從而獲得每個樣本粒子的權值；

步驟4：計算樣本集的有效粒子數，判斷樣本集的有效粒子數是否小于等于預設好的有效粒子數閾值，若是，則轉步驟9；否則轉步驟5：

步驟5：刪除權值小于預設好的權值閾值的樣本粒子；則剩余的粒子個數為n-y個，其中y為權值小于權值閾值的樣本粒子的個數；t+1；

步驟6：進行第t次采樣：以機器人的理想位置u為中心，利用采樣公式，采樣y個位置坐標，從而補充樣本粒子集；

步驟7：在上一輪采樣中剩余的粒子中隨機選擇一個粒子，基于隨機選擇的粒子和步驟6中采樣的y個位置坐標，進行交叉計算，從而得到y個新的位置坐標，并將該y個新的位置坐標替換原y個位置坐標；得到新的樣本集；

步驟8：判斷t是否小于max，若是，將新的樣本集作為用于下次計算的樣本集，并轉步驟3；否則，轉步驟9；

步驟9：計算新的樣本集中所有樣本粒子的坐標的加權平均值；并將該加權平均值作為機器人的實際位置D的坐標；

所述步驟3中計算每個樣本粒子的匹配度的具體方法為：

其中M_i為第i個樣本粒子的匹配度，j為第j個標志點；d_ij為第i個樣本粒子與第j個標志點的距離，為機器人傳感器測得機器人與第j個標志點的實際距離；i＝1,2,…,n；j＝1,2,…,J；

所述步驟4中計算樣本集的有效粒子數，采用如下公式計算樣本集的有效粒子數：

2.根據權利要求1所述的基于粒子濾波的機器人定位方法，其特征在于，所述步驟7中得到y個新的位置坐標的具體方法為：將y個位置坐標中的第w個位置坐標的橫坐標作為第w個新的位置坐標的橫坐標，將隨機選擇的粒子的縱坐標作為第w個新的位置坐標的縱坐標；w＝1,2,…,y。