一種慣性導航系統的誤差校正方法
2023-06-02 00:19:51
一種慣性導航系統的誤差校正方法
【專利摘要】本發明公開了一種慣性導航系統的誤差校正方法，在至少具有運動控制系統、慣性導航系統、雷射掃描傳感器的機器人上使用；包括以下步驟：1）設置全局坐標系；2）機器人從起點開始不斷掃描環境信息；3）對掃描數據點進行濾波處理；4）將濾波後數據點分割為不同的類別；5）採用IEP算法對每一個類別進行直線特徵識別；6）利用最小二乘準則從直線特徵的數據點集中擬合出直線，並計算其特徵參數；7）從上述步驟中得到的雷射掃描數據中提取兩組直線特徵參數；8）將兩組直線特徵參數變換到全局坐標系下，確定兩直線是否來自同一物體；9）上述步驟中L1，L2為同一直線時，利用機器人的位姿變化量來校正位姿。
【專利說明】一種慣性導航系統的誤差校正方法
【技術領域】
[0001]本發明屬於移動機器人定位【技術領域】，特別涉及一種慣性導航系統的誤差校正方法。
【背景技術】
[0002]在移動機器人的應用中，導航定位是機器人系統的關鍵功能。導航是指移動機器人通過傳感器獲得環境信息和自身位姿狀態，實現在障礙物的環境中自主運動到目的地。導航的關鍵和前提是定位，定位是指實時確定機器人自身在環境中的位置和姿態。
[0003]目前較為常見的機器人定位技術主要是根據先驗的環境信息，結合當前機器人位置信息和傳感器獲取的信息確定機器人位姿狀態。定位技術主要分為絕對定位和相對定位，相對定位通過計算相對於初始位姿的位姿變化來確定當前的位姿狀態，常見的有裡程計、慣性導航系統。裡程計、慣性導航系統等長時間或大尺度下累積誤差較大，定位精度較差，無法準確、省力地為機器人提供位姿信息。
【發明內容】
[0004]本發明的目的是為了解決上述問題，提供一種能夠對慣性導航系統的航位誤差進行校正，減少誤差的誤差校正方法。
[0005]為此，本發明的技術方案是:一種慣性導航系統的誤差校正方法，在至少具有運動控制系統、慣性導航系統、雷射掃描傳感器的機器人上使用；其特徵在於:它包括以下步驟:
1)首先在新的未知環境下，設置起點位姿Z0，並以此為基準設置全局坐標系，即以起點位置為坐標系原點，以起點運動朝向為I軸正向，按右手法則確定X軸正向；
2)機器人從起點開始運動，機器人所載雷射傳感器不斷掃描環境信息得到以雷射傳感器為原點的角度、距離極坐標點數據；機器人每運動一定距離就要處理一次雷射掃描數據；慣性導航系統不斷輸出機器人當前的航位信息；
3)對雜亂無規律的雷射掃描數據點進行濾波處理，去除在有效計算距離範圍之外的無用點；
4)將濾波後數據點根據其分布的連續特性聚類處理，分割為不同的類別；方法為計算相鄰兩雷射掃描點距離nd_laser，若該距離小於等於設定閾值thr_cate則判定為同一類另IJ，同一類別代表該點集來自障礙物的連續表面；同時設置合適的閾值thr_cate，將數據點較少的類別剔除以加快數據處理速度；
5)採用IEP算法對每一個類別進行直線特徵識別，在每個類別中找出構成直線特徵的數據點集；設置合理的閾值thr_cate，將數據點較少的直線點集剔除；
6)利用最小二乘準則從直線特徵的數據點集中擬合出直線，並計算其特徵參數；
以機器人為中心坐標系與雷射傳感器為中心的坐標系重合，所述參數為在雷射傳感器坐標系下，直線到原點距離d、直線與坐標原點的垂線與極坐標軸夾角theta、直線端點坐標pi (x, y)，p2(x, y)和線性相關係數r ;其中d，theta直線方程參數，利用線段端點pl，p2計算線段長度和直線匹配；利用線性係數r作為判別其直線特徵相似程度，優先利用線性係數高的直線特徵進行定位計算；
7)從上述步驟中得到的雷射掃描數據中提取兩組直線特徵參數；提取雷射掃描數據中Tl時刻的某一直線參數LI (rl，thetal)，此時刻機器人位姿坐標為Zl (XI，Yl，THETAI);提取雷射掃描數據中T2時刻的某一直線參數L2(r2，theta2)，此時刻機器人位姿為Z2(X1，Yl，THETAI)；
8)將步驟7)中提取的兩組直線特徵參數變換到全局坐標系下，根據判別標準確定兩直線是否來自同一物體；
採用的判別標準為:兩直線距離差絕對值小於設定閾值lthr_d，且兩直線角度差絕對值小於設定閾值lthr_theta，且兩直線對應端點距離小於設定閾值lthr_pd ；
9)上述步驟中LI，L2為同一直線時，則Tl時刻到T2時刻機器人位姿變化量為deltaZ，利用該位姿變化量來校正位姿Z2=Z I+de I taZ。
[0006]本發明以慣性導航的輸出位姿為基礎，利用雷射掃描獲取自然環境中的直線特徵，通過以相同直線的兩次雷射掃描數據提取的特徵參數為基準，反推計算坐標系變換關係，雷射掃描器坐標系下特徵直線位姿變化即為兩次雷射掃描間機器人在全局坐標系下位姿變化量，以此校正慣性導航誤差，創建高精度的環境特徵地圖。
【專利附圖】
【附圖說明】
[0007]以下結合附圖和本發明的實施方式來做進一步詳細說明。
[0008]圖1本發明的直線特徵提取流程圖。
[0009]圖2本發明的誤差校正流程圖。
【具體實施方式】
[0010]參見附圖。本實施例在至少具有運動控制系統、慣性導航系統、雷射掃描傳感器的機器人上使用；它包括以下步驟:
1)首先在新的未知環境下，設置起點位姿Z0，並以此為基準設置全局坐標系，即以起點位置為坐標系原點，以起點運動朝向為I軸正向，按右手法則確定X軸正向；
2)機器人從起點開始運動，機器人所載雷射傳感器不斷掃描環境信息得到以雷射傳感器為原點的角度、距離極坐標點數據；機器人每運動一定距離就要處理一次雷射掃描數據；
慣性導航系統不斷輸出機器人當前的航位信息，利用該航位信息不斷計算當前點與上一次掃描數據處理位置的距離delta_d，當該距離大於等於設定的閾值delta_dthreshod時則進行下面的雷射掃描數據處理，並將delta_d清0，從當前點重新開始計算delta_d ；
3)如圖1所示從雷射掃描數據中提取直線特徵過程:對雜亂無規律的雷射掃描數據點進行濾波處理，去除在有效計算距離範圍之外的無用點；
4)將濾波後數據點根據其分布的連續特性聚類處理，分割為不同的類別；方法為計算相鄰兩雷射掃描點距離nd_laser，若該距離小於等於設定閾值thr_cate則判定為同一類另IJ，同一類別代表該點集來自障礙物的連續表面；同時設置合適的閾值thr_cate，將數據點較少的類別剔除以加快數據處理速度；
5)採用IEP算法對每一個類別進行直線特徵識別，在每個類別中找出構成直線特徵的數據點集；設置合理的閾值thr_cate，將數據點較少的直線點集剔除；
6)利用最小二乘準則從直線特徵的數據點集中擬合出直線，並計算其特徵參數； 假設以機器人為中心坐標系與雷射傳感器為中心的坐標系重合，所述參數為在雷射傳感器坐標系下，直線到原點距離d、直線與坐標原點的垂線與極坐標軸夾角theta、直線端點坐標pi (X，y)，p2(x，y)和線性相關係數r ;其中d、theta直線方程參數，利用線段端點pl，p2計算線段長度和直線匹配；利用線性係數!■作為判別其直線特徵相似程度，優先利用線性係數高的直線特徵進行定位計算；
7)從上述步驟中得到的雷射掃描數據中提取兩組直線特徵參數；提取雷射掃描數據中Tl時刻的某一直線參數LI (rl, thetal)，此時刻機器人位姿坐標為Zl (XI，Yl，THETAI);提取雷射掃描數據中T2時刻的某一直線參數L2(r2，theta2)，此時刻機器人位姿為Z2(X1，Yl，THETAI)；
8)將步驟7)中提取的兩組直線特徵參數變換到全局坐標系下，根據判別標準確定兩直線是否來自同一物體；
採用的判別標準為:兩直線距離差絕對值小於設定閾值lthr_d，且兩直線角度差絕對值小於設定閾值lthr_theta，且兩直線對應端點距離小於設定閾值lthr_pd ；
9)上述步驟中LI，L2為同一直線時，則Tl時刻到T2時刻機器人位姿變化量為deltaZ，利用該位姿變化量來校正位姿Z2=Z I+de I taZ。
[0011]如圖2所示為誤差校正過程，其中LI表示提取的特徵直線在雷射傳感器坐標系下參數，{LI，...}表示特徵直線集，{LI』，...}表示將直線集{LI，...}變換到全局坐標系下，{ (LI』』，L2』』)，...}表示由{LI，...}到{L2，...}的對應直線對映射集；
在兩次雷射掃描中相同的特徵直線變換全局坐標系下其特徵參數會存在差異，主要原因為慣性導航導致的當前位姿存在誤差，即坐標變換關係存在誤差，因而導致環境中相同直線在全局坐標下有不同參數；因而以相同直線的兩次雷射掃描數據提取的特徵參數為基準，反推計算坐標系變換關係，雷射掃描器坐標系下特徵直線位姿變化即為兩次雷射掃描間機器人在全局坐標系下位姿變化量，以此校正慣性導航輸出的位姿狀態；其中採用最近鄰法判別兩次不同掃描數據中是否為同一直線。
【權利要求】
1.一種慣性導航系統的誤差校正方法，在至少具有運動控制系統、慣性導航系統、雷射掃描傳感器的機器人上使用；其特徵在於:它包括以下步驟: 1)首先在新的未知環境下，設置起點位姿Z0，並以此為基準設置全局坐標系，即以起點位置為坐標系原點，以起點運動朝向為I軸正向，按右手法則確定X軸正向； 2)機器人從起點開始運動，機器人所載雷射傳感器不斷掃描環境信息得到以雷射傳感器為原點的角度、距離極坐標點數據；機器人每運動一定距離就要處理一次雷射掃描數據；慣性導航系統不斷輸出機器人當前的航位信息； 3)對雜亂無規律的雷射掃描數據點進行濾波處理，去除在有效計算距離範圍之外的無用點； 4)將濾波後數據點根據其分布的連續特性聚類處理，分割為不同的類別；方法為計算相鄰兩雷射掃描點距離nd_laser，若該距離小於等於設定閾值thr_cate則判定為同一類另IJ，同一類別代表該點集來自障礙物的連續表面；同時設置合適的閾值thr_cate，將數據點較少的類別剔除以加快數據處理速度； 5)採用IEP算法對每一個類別進行直線特徵識別，在每個類別中找出構成直線特徵的數據點集；設置合理的閾值thr_cate，將數據點較少的直線點集剔除； 6)利用最小二乘準則從直線特徵的數據點集中擬合出直線，並計算其特徵參數； 以機器人為中心坐標系與雷射傳感器為中心的坐標系重合，所述參數為在雷射傳感器坐標系下，直線到原點距離d、直線與坐標原點的垂線與極坐標軸夾角theta、直線端點坐標pi (X，y)，p2(x, y)和線性相關係數r ;其中d，theta直線方程參數，利用線段端點pl，p2計算線段長度和直線匹配；利用線性係數r作為判別其直線特徵相似程度，優先利用線性係數高的直線特徵進行定位計算； 7)從上述步驟中得到的雷射掃描數據中提取兩組直線特徵參數；提取雷射掃描數據中Tl時刻的某一直線參數LI (rl，thetal)，此時刻機器人位姿坐標為Zl (XI，Yl，THETAI);提取雷射掃描數據中T2時刻的某一直線參數L2(r2，theta2)，此時刻機器人位姿為Z2(X1，Yl，THETAI)； 8)將步驟7)中提取的兩組直線特徵參數變換到全局坐標系下，根據判別標準確定兩直線是否來自同一物體； 採用的判別標準為:兩直線距離差絕對值小於設定閾值lthr_d，且兩直線角度差絕對值小於設定閾值lthr_theta，且兩直線對應端點距離小於設定閾值lthr_pd ； 9)上述步驟中LI，L2為同一直線時，則Tl時刻到T2時刻機器人位姿變化量為deltaZ，利用該位姿變化量來校正位姿Z2=Z I+de I taZ。
【文檔編號】G01C25/00GK104501829SQ201410677883
【公開日】2015年4月8日 申請日期:2014年11月24日 優先權日:2014年11月24日
【發明者】陳如申, 黎勇躍, 王海瑞, 馬麗豔 申請人:杭州申昊科技股份有限公司