室內外環境定位方法與流程
1.本發明涉及機器人導航技術領域，特別涉及一種室內外環境定位方法。
背景技術：
2.目前的自主導航機器人按應用場景主要可以分為室外機器人和室內機器人兩種，室外機器人根據運動速度的不同，可以分為中低速機器人(《10m/s)和高速機器人。中低速機器人主要用于園區配送，安防巡檢，草坪割草，而高速機器人主要面向自動駕駛行業。室內機器人又可以稱做自動導引車(agv)，主要用于工業，工廠中有大量物料配送的需求，而agv可以在工廠環境完美的達到高效率，低成本的物流配送。
3.目前的機器人只能單獨用于室外或者室內，無法同時應用于室內和室外，而且，目前的室外激光定位方案存在著大場景地圖過大，計算效率低，難以實時定位和定位魯棒性不夠的問題。
技術實現要素：
4.本發明要解決的技術問題是提供一種定位精度高、魯棒性好的室內外環境定位方法。
5.為了解決上述問題，本發明提供了一種室內外環境定位方法，其包括以下步驟：
6.s10、將室外地圖切割成多個局部地圖，在室內地圖和室外地圖之間設置切換點，機器人通過所述切換點進行室內外切換，以實現在室內定位時加載室內地圖，在室外定位時實時加載機器人所在的局部地圖；
7.s20、使用誤差狀態卡爾曼濾波器融合安裝于機器人上的慣性測量單元、輪式里程計和激光定位系統，對機器人進行定位。
8.作為本發明的進一步改進，在步驟s10中，將室外地圖切割成若干局部地圖，包括：
9.s111、計算室外地圖的x坐標的最大值和最小值，以及y坐標的最大值和最小值；
10.s112、設置切換步長、激光測量范圍和局部地圖邊長；
11.s113、遍歷室外地圖上的點，若該點的x坐標和y坐標都能夠整除所述切換步長，則將該點作為局部地圖的中心點，并將室外地圖切割成若干局部地圖。
12.作為本發明的進一步改進，所述切換步長、激光測量范圍和局部地圖邊長滿足：
13.size≥step+2*m
14.其中，size為局部地圖邊長；step為切換步長；m為激光測量范圍。
15.作為本發明的進一步改進，在室內定位時使用2d激光定位系統，在室外定位時使用3d激光定位系統；在步驟s10中，所述機器人通過所述切換點進行室內外切換，包括：
16.當從室內到達所述切換點時，將2d激光定位系統的當前定位數據傳輸至3d激光定位系統，對3d激光定位系統進行初始化定位；
17.當從室外到達所述切換點時，將3d激光定位系統的當前定位數據傳輸至2d激光定位系統，對2d激光定位系統進行初始化定位。
18.作為本發明的進一步改進，通過以下步驟判斷初始化定位時是否成功，步驟包括：
19.計算當前激光點云根據初始化定位位姿變換之后的點云中每一個點和地圖點云之間的最短歐氏距離的平均值，然后將平均值通過指數函數得到定位的分數，如果分數大于閾值，則初始化定位成功，否則不成功。
20.作為本發明的進一步改進，步驟s20包括：
21.s21、狀態初始化步驟；包括：進行位姿初始化，并進行誤差狀態卡爾曼濾波器初始化；
22.s22、模型預測步驟；包括：利用慣性測量單元測量的角速度和線性加速度積分來預測位姿，然后更新誤差狀態的雅可比矩陣和協方差矩陣；
23.s23、觀測校正步驟；包括：先獲取輪式里程計的速度觀測值，并且根據ndt算法配準當前點云數據和地圖得到位姿，然后將觀測量傳遞到誤差狀態卡爾曼濾波器，計算誤差狀態卡爾曼增益，然后計算誤差狀態值和協方差矩陣，然后將誤差狀態值加到名義狀態值中更新名義狀態值，然后重置誤差狀態值。
24.作為本發明的進一步改進，s22模型預測步驟中，采用如下公式：
[0025][0026][0027][0028]
式中，p為機器人位置；r為機器人方向的旋轉矩陣；dt為時間間隔；v代表機器人速度；am代表線加速度測量值；代表線加速度偏置；g代表重力加速度；q代表旋轉方向四元數；ωm代表角速度測量值；ε代表角速度偏置；下標k、k-1分別代表k、k-1時刻；上標t代表轉置；
[0029]
接著根據系統微分方程：計算系統狀態方程：x
k-1
＝f
k-1
x
k-1
+b
k-1uk-1
關于誤差狀態的雅可比矩陣f
k-1
；
[0030]
其中，為系統狀態量的導數；f
t
為系統微分方程狀態雅可比矩陣；e為系統微分方程擾動量；b
t
為系統微分方程擾動雅可比矩陣；f
k-1
為系統狀態方程狀態雅可比矩陣；u為系統狀態方程擾動量，b
k-1
為系統狀態方程擾動雅可比矩陣；之后系統狀態方程狀態使用雅可比矩陣f
k-1
更新誤差狀態的協方差矩陣，公式如下：
[0031][0032]
其中，p是誤差狀態的協方差矩陣。
[0033]
作為本發明的進一步改進，s23觀測校正步驟中，采用如下公式：
[0034]
yk＝gkx
k-1
+ckwk[0035]
其中，y是誤差觀測量，包括位置誤差、角度誤差和速度誤差、x為誤差狀態量、w為觀測擾動量、g和c分別是誤差觀測量關于誤差狀態和觀測擾動的雅可比矩陣；
[0036]
由卡爾曼濾波可知，矯正方程為：
[0037][0038]
pk＝(i-k
kgk
)pk[0039]
xk＝xk+kk(y
k-gkxk)
[0040]
其中，p是誤差狀態的協方差矩陣；r是觀測擾動的協方差矩陣；i是單位矩陣；k為卡爾曼增益；下標k、k-1分別代表k、k-1時刻；上標t代表轉置。
[0041]
本發明還提供了一種電子設備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，所述處理器執行所述程序時實現上述任意一項所述方法的步驟。
[0042]
本發明還提供了一種計算機可讀存儲介質，其上存儲有計算機程序，該程序被處理器執行時實現上述任意一項所述方法的步驟。
[0043]
本發明的有益效果：
[0044]
一、本發明室內外環境定位方法通過在室內地圖和室外地圖之間設置切換點，機器人通過切換點進行室內外切換，實現在室內定位時加載室內地圖，在室外定位時實時加載機器人所在的局部地圖，保證機器人可同時應用于室內和室外。
[0045]
二、本發明針對室外大場景環境，機器人面臨的激光點云地圖占用內存大，計算效率低的問題，通過預先將室外地圖分割成多個較小的局部地圖，根據機器人所在位置實時加載局部地圖，提高了計算效率，降低了系統計算負載。
[0046]
三、本發明使用了誤差狀態卡爾曼濾波器進行融合定位，預測部分使用慣性測量單元，觀測部分使用激光配準的位姿數據和輪式里程計的速度，提升了定位的精確性和魯棒性。
[0047]
上述說明僅是本發明技術方案的概述，為了能夠更清楚了解本發明的技術手段，而可依照說明書的內容予以實施，并且為了讓本發明的上述和其他目的、特征和優點能夠更明顯易懂，以下特舉較佳實施例，并配合附圖，詳細說明如下。
附圖說明
[0048]
圖1是本發明優選實施例中室內外環境定位方法的流程圖；
[0049]
圖2是本發明優選實施例中室外地圖切割的流程圖；
[0050]
圖3是本發明優選實施例中實時加載局部地圖的流程圖；
[0051]
圖4是本發明優選實施例中2d區域切換到3d區域的流程圖；
[0052]
圖5是本發明優選實施例中3d區域切換到2d區域的流程圖；
[0053]
圖6是本發明優選實施例中室內外切換的示意圖；
[0054]
圖7是本發明優選實施例中誤差狀態卡爾曼濾波器的流程圖。
具體實施方式
[0055]
下面結合附圖和具體實施例對本發明作進一步說明，以使本領域的技術人員可以更好地理解本發明并能予以實施，但所舉實施例不作為對本發明的限定。
[0056]
如圖1所示，為本發明優選實施例中室內外環境定位方法，包括以下步驟：
[0057]
步驟s10、將室外地圖切割成多個局部地圖，在室內地圖和室外地圖之間設置切換點，機器人通過所述切換點進行室內外切換，以實現在室內定位時加載室內地圖，在室外定位時實時加載機器人所在的局部地圖；
[0058]
由于室外地圖為大場景地圖，占用的內存較大，以及激光點云和地圖配準時的占
用的cpu也會因此增加。因此，本發明將室外的大場景地圖分割為多個具有一定重合區域的局部地圖。
[0059]
在已知需要切分出來的局部地圖邊界時，可以將大場景地圖在局部地圖邊界以外的點云數據全部刪除，從而得到的即為局部地圖。邊界的形式多種多樣，可選地，本發明采用的立方體邊界。
[0060]
分割后所有的局部地圖必須包含大場景地圖的所有區域，將大場景地圖分割為若干個立方體局部地圖。因為假設機器人從1號局部地圖切換到2號局部地圖時，在1號局部地圖的該切換位置必須有激光雷達測量范圍的視野，換而言之，切換位置距離地圖邊界至少有激光雷達測量范圍的距離，并且切換位置距離1號和2號地圖邊界均至少有激光雷達測量范圍的距離。因此相鄰的局部地圖在該方向上有兩個激光測量距離的重合區域。
[0061]
參照圖2，在其中一實施例中，在步驟s10中，將室外地圖切割成若干局部地圖，包括以下步驟：
[0062]
s111、計算室外地圖的x坐標的最大值和最小值，以及y坐標的最大值和最小值；可分別表示為max_x、min_x、max_y、min_y。
[0063]
s112、設置切換步長、激光測量范圍和局部地圖邊長；其中，所述切換步長、激光測量范圍和局部地圖邊長滿足：
[0064]
size≥step+2*m
[0065]
其中，局部地圖xy平面為正方形，size為局部地圖邊長；step為切換步長；m為激光測量范圍。
[0066]
s113、遍歷室外地圖上的點，若該點的x坐標和y坐標都能夠整除所述切換步長，則將該點作為局部地圖的中心點，并將室外地圖切割成若干局部地圖。
[0067]
具體地，若該點的x坐標和y坐標能夠整除步長step，則將該點作為局部地圖的中心點，其坐標為(x_center,y_center)。該局部地圖xy平面的邊界x軸最大值為中心點x坐標加上二分之一的局部地圖大小(x_center+0.5*size)，x軸最小值為中心點x坐標減去二分之一的局部地圖大小(x_center-0.5*size)，y軸最大值為中心點y坐標加上二分之一的局部地圖大小(y_center+0.5*size)，y軸最小值為中心點y坐標減去二分之一的局部地圖大小(x_center-0.5*size)。z軸不做切割，全部保留。以該局部邊界切割大場景地圖，保存生成的地圖命名為中心點x坐標_中心點y坐標。
[0068]
參照圖3，在其中一實施例中，在步驟s10中，在室外定位時實時加載機器人所在的局部地圖，包括以下步驟：
[0069]
s121、設置切換步長step；
[0070]
s122、獲取當前機器人位置x,y,z；
[0071]
s123、計算當前所在位置所在的局部地圖。當前位置x坐標除以步長與0.5的和向下取整再乘以步長，得到的結果是中心點x坐標，當前位置y坐標除以步長與0.5的和向下取整再乘以步長，得到的結果是中心點y坐標，如果中心點x，y坐標有任何一個改變了說明局部地圖已經改變，此時加載改變以后的局部地圖，如果沒有變化，則不做修改。處理完成后，一個周期結束，返回上一步。
[0072]
參照圖6，在其中一實施例中，在室內(2d區域)定位時使用2d激光定位系統，在室外(3d區域)定位時使用3d激光定位系統；在步驟s10中，所述機器人通過所述切換點進行室
內外切換，包括：
[0073]
當從室內到達所述切換點時，將2d激光定位系統的當前定位數據傳輸至3d激光定位系統，對3d激光定位系統進行初始化定位；參照圖4。
[0074]
當從室外到達所述切換點時，將3d激光定位系統的當前定位數據傳輸至2d激光定位系統，對2d激光定位系統進行初始化定位；參照圖5。
[0075]
具體地，通過以下步驟判斷初始化定位時是否成功，步驟包括：
[0076]
計算當前激光點云根據初始化定位位姿變換之后的點云中每一個點和地圖點云之間的最短歐氏距離的平均值，然后將平均值通過指數函數得到定位的分數，分數體現了點云和地圖之間配準的準確性，如果分數大于閾值，則初始化定位成功，否則不成功。
[0077]
步驟s20、使用誤差狀態卡爾曼濾波器融合安裝于機器人上的慣性測量單元、輪式里程計和激光定位系統，對機器人進行定位。
[0078]
其中，機器人上安裝有多種類型的測量傳感器，包括激光雷達，慣性測量元件、輪式里程計。激光雷達可以掃描出周圍環境的點云數據，在和預先建立的環境地圖配準以后，可以得到機器人在環境中的位置，激光雷達包括2d激光雷達和3d激光雷達，2d激光雷達構成2d激光定位系統，3d激光雷達構成3d激光定位系統；慣性測量單元可以測量車體的三軸角速度和三軸線性加速度，這些數據積分處理后可以得到一段時間內機器人的位置變化量、速度變化量以及角速度變化量；輪式里程計可以測量機器人當前的運動速度。使用這些傳感器融合可以得到定位，融合的方式主要有濾波的方法和優化的方法，濾波的方法計算量更小，為了達到實時性的目的，采用了濾波的框架。濾波的問題可以簡單理解為一種預測和觀測融合的結果，在本發明中，是慣性測量單元預測位姿速度變化，激光點云配準觀測位姿，輪式里程計觀測速度的結果。
[0079]
誤差狀態卡爾曼濾波中有三種狀態值：名義狀態，誤差狀態和真實狀態。真實狀態由名義狀態和誤差狀態組合而成(線性和，四元數乘積或者矩陣乘積)。高頻率的慣性測量單元數據積分得到名義狀態。在該名義狀態中，沒有包含噪聲以及其他的模型缺陷。誤差狀態將會收集這些缺陷，并使用誤差狀態卡爾曼濾波進行估計，在觀測數據到達后完成校正。
[0080]
參照圖7，在其中一實施例中，步驟s20包括：
[0081]
s21、狀態初始化步驟；包括：進行位姿初始化，并進行誤差狀態卡爾曼濾波器初始化；其中包含了預測噪聲、觀測噪聲的設置。
[0082]
s22、模型預測步驟；包括：利用慣性測量單元測量的角速度和線性加速度積分來預測位姿，然后更新誤差狀態的雅可比矩陣和協方差矩陣；
[0083]
具體地，首先使用慣性測量單元測量的三軸角速度和三軸線加速度預測機器人位姿。當前時刻位置等于上一時刻位置，上一時刻速度乘以時間變化量，當前機器人方向的旋轉矩陣與線加速度測量值減去線加速度偏置的結果的乘積與重力加速度的和再乘以時間變化量的平方除以2的值的和。當前速度等于上一時刻速度，當前機器人方向的旋轉矩陣與線加速度測量值減去線加速度偏置的結果的乘積與重力加速度的和再乘以時間變化量的值的和。當前旋轉方向四元數等于上一時刻旋轉方向四元數和角速度減去角速度偏置的差值和時間變化量的乘積轉化的四元數的乘積。采用如下公式：
[0084][0085]
[0086][0087]
式中，p為機器人位置；r為機器人方向的旋轉矩陣；dt為時間間隔；v代表機器人速度；am代表線加速度測量值；代表線加速度偏置；g代表重力加速度；q代表旋轉方向四元數；ωm代表角速度測量值；ε代表角速度偏置；下標k、k-1分別代表k、k-1時刻；上標t代表轉置；
[0088]
接著根據系統微分方程：計算系統狀態方程：x
k-1
＝f
k-1
x
k-1
+b
k-1uk-1
關于誤差狀態的雅可比矩陣f
k-1
；
[0089]
其中，為系統狀態量的導數；f
t
為系統微分方程狀態雅可比矩陣；e為系統微分方程擾動量；b
t
為系統微分方程擾動雅可比矩陣；f
k-1
為系統狀態方程狀態雅可比矩陣；u為系統狀態方程擾動量，b
k-1
為系統狀態方程擾動雅可比矩陣；之后系統狀態方程狀態使用雅可比矩陣f
k-1
更新誤差狀態的協方差矩陣，公式如下：
[0090][0091]
其中，p是誤差狀態的協方差矩陣。
[0092]
s23、觀測校正步驟；包括：先獲取輪式里程計的速度觀測值，并且根據ndt算法配準當前點云數據和地圖得到位姿，然后將觀測量傳遞到誤差狀態卡爾曼濾波器，計算誤差狀態卡爾曼增益，然后計算誤差狀態值和協方差矩陣，然后將誤差狀態值加到名義狀態值中更新名義狀態值，然后重置誤差狀態值。
[0093]
具體地，首先需要通過傳感器獲取觀測數據，在本發明中有兩種傳感器作為觀測，分別是輪式里程計和激光雷達，輪式里程計可以得到機器人本體坐標系下的運動速度，激光雷達得到的激光點云數據和地圖配準可以得到機器人的全局位姿，配準的算法有多種可以選擇，常用的有ndt，gicp，fast_gicp等，激光點云的配準算法通常需要設置目標點云，當前點云和初始估計位姿值。目標點云為地圖點云，當前點云為激光雷達實時獲取到的點云，而初始估計位姿值對配準具有重要的影響，更準確的初始估計位姿值可以避免配準算法進入局部最小值的計算，提高算法魯棒性和效率，初始估計位姿值使用預測得到的位姿可以有較好的準確性。然后通過得到的速度和位姿計算誤差狀態測量，對于觀測方程：
[0094]
yk＝gkx
k-1
+ckwk[0095]
其中，y是誤差觀測量，包括位置誤差、角度誤差和速度誤差、x為誤差狀態量、w為觀測擾動量、g和c分別是誤差觀測量關于誤差狀態和觀測擾動的雅可比矩陣；
[0096]
由卡爾曼濾波可知，矯正方程為：
[0097][0098]
pk＝(i-k
kgk
)pk[0099]
xk＝xk+kk(y
k-gkxk)
[0100]
其中，p是誤差狀態的協方差矩陣；r是觀測擾動的協方差矩陣；i是單位矩陣；k為卡爾曼增益；下標k、k-1分別代表k、k-1時刻；上標t代表轉置。
[0101]
其中，觀測和預測有序地進行，預測傳感器數據到達進行預測步驟，觀測傳感器數據到達執行觀測步驟，不斷地更新系統位姿數據，觀測校正后的數據會發送給機器人進行定位。
[0102]
在一具體實施例中，采用本發明的室內外環境定位方法對機器人進行定位，包括以下步驟：
[0103]
一、地圖分割步驟：
[0104]
1、假設計算出大場景地圖的x,y軸的最大值和最小值max_x,max_y,min_x,min_y分別為1200，700，0，300；
[0105]
2、設置切換步長step為500，激光測量范圍m為100和局部地圖大小size為700(size》＝(step+2*m))；
[0106]
3、遍歷x坐標0到1200，遍歷y坐標300到700，x和y坐標能夠整除步長step(500)的點有3個，分別為(0，500)、(500，500)和(1000，500)。(0，500)為中心點的局部地圖xy平面的四個邊界點分別為(-350，150)、(-350，850)、(350，150)、(350，850)，生成的局部地圖命名為0_500。(500，500)為中心點的局部地圖xy平面的四個邊界點分別為(150，150)、(150，850)、(850，150)、(850，850)，生成的局部地圖命名為500_500。(1000，500)為中心點的局部地圖xy平面的四個邊界點分別為(650，150)、(650，850)、(1350，150)、(1350，850)，生成的局部地圖命名為1000_500。
[0107]
二、實時加載局部地圖步驟：
[0108]
1、設置切換步長step為500，step和地圖分割時設置必須一致；
[0109]
2、獲取當前機器人位置x、y、z，假設分別為248、600、0；
[0110]
3、計算中心點x坐標floor(248/500+0.5)*500＝0；計算中心點y坐標floor(600/500+0.5)*500＝500；
[0111]
4、得到中心點x、y坐標為(0，500)，加載0_500地圖，x坐標248+100《350(局部地圖x軸最大值),激光雷達最大視野仍然在地圖范圍內；
[0112]
5、獲取當前機器人位置x、y、z，假設分別為249、600、0；
[0113]
6、計算中心點x坐標floor(249/500+0.5)*500＝0；計算中心點y坐標floor(600/500+0.5)*500＝500；
[0114]
7、得到中心點x、y坐標為(0，500)，地圖沒有改變，不加載地圖；
[0115]
8、獲取當前機器人位置x,y,z，假設分別為251，600，0；
[0116]
9、計算中心點x坐標floor(251/500+0.5)*500＝500；計算中心點y坐標floor(600/500+0.5)*500＝500；
[0117]
10、得到中心點x、y坐標為(500，500)，加載500_500地圖，x坐標350(0_500局部地圖x軸最大值)《251+100《850(500_500局部地圖x軸最大值)、150(500_500局部地圖x軸最小值)《251-100，激光雷達最大視野不在0_500局部地圖，在500_500局部地圖范圍內，地圖切換成功。
[0118]
三、融合慣性測量單元和輪式里程計和激光雷達的誤差狀態卡爾曼濾波定位步驟：
[0119]
1、系統狀態量x為δp為位置誤差，δv為速度誤差，φ為失準角，ε為角速度偏置，為線加速度偏置；
[0120]
2、獲取前一時刻系統名義狀態值，位置p
k-1
，姿態q
k-1
和速度v
k-1
；
[0121]
3、根據慣性測量單元數據和時間預測當前時刻系統位置pk，姿態qk和速度vk，下標k代表k時刻；
[0122]
4、位置預測：姿態預測：速度預測：
[0123]
5、系統微分方程：
[0124]
6、計算系統微分方程狀態雅可比矩陣
[0125]
其中，是base_link相對于n系(靜止的導航坐標系)的變換的旋轉部分；fn是加速度(n系)；fn×
是fn的反對稱矩陣；i3×3代表3乘3的單位矩陣；i代表單位矩陣；下標3x3代表3乘3，其余下標同理；
[0126]
7、計算系統微分方程擾動雅可比矩陣
[0127]
8、系統狀態方程：x
k-1
＝f
k-1
x
k-1
+b
k-1uk-1
；
[0128]
9、計算f
k-1
＝i+f
t
×
dt，b
k-1
＝b
t
×
dt；
[0129]
10、更新誤差狀態協方差矩陣：
[0130]
11、獲取激光雷達點云數據，設置點云，以預測的系統位姿作為初始值進行點云配準，得到配準后的位姿，作為觀測值，獲取輪式里程計速度值；
[0131]
12、系統觀測方程yk＝gkx
k-1
+ckwk；系統觀測量y為
[0132]
13、計算系統誤差狀態觀測量，系統預測位置，姿態和速度和相應傳感器計算得到的結果之間的差值，得到觀測量y；
[0133]
14、計算系統觀測方程關于誤差狀態的雅可比矩陣：
[0134][0135]
其中，是n系相對于base_link的變換的旋轉部分；vn是速度(n系)；vn×
是vn的反對稱矩陣；系統觀測方程關于噪聲的雅可比矩陣ck為單位矩陣。
[0136]
15、計算卡爾曼增益
[0137]
16、計算校正后協方差矩陣pk＝(i-k
kgk
)pk；
[0138]
17、計算校正后誤差狀態值xk＝xk+kk(y
k-gkxk)；
[0139]
18、使用校正后的誤差狀態值更新名義狀態值；
[0140]
19、重置系統誤差狀態值。
[0141]
本發明室內外環境定位方法通過在室內地圖和室外地圖之間設置切換點，機器人
通過切換點進行室內外切換，實現在室內定位時加載室內地圖，在室外定位時實時加載機器人所在的局部地圖，保證機器人可同時應用于室內和室外。
[0142]
本發明針對室外大場景環境，機器人面臨的激光點云地圖占用內存大，計算效率低的問題，通過預先將室外地圖分割成多個較小的局部地圖，根據機器人所在位置實時加載局部地圖，提高了計算效率，降低了系統計算負載。
[0143]
本發明使用了誤差狀態卡爾曼濾波器進行融合定位，預測部分使用慣性測量單元，觀測部分使用激光配準的位姿數據和輪式里程計的速度，提升了定位的精確性和魯棒性。
[0144]
本發明優選實施例還公開了一種電子設備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，所述處理器執行所述程序時實現上述實施例中所述方法的步驟。
[0145]
本發明優選實施例還公開了一種計算機可讀存儲介質，其上存儲有計算機程序，該程序被處理器執行時實現上述實施例中所述方法的步驟。
[0146]
以上實施例僅是為充分說明本發明而所舉的較佳的實施例，本發明的保護范圍不限于此。本技術領域的技術人員在本發明基礎上所作的等同替代或變換，均在本發明的保護范圍之內。本發明的保護范圍以權利要求書為準。
技術特征：
1.室內外環境定位方法，其特征在于，包括：s10、將室外地圖切割成多個局部地圖，在室內地圖和室外地圖之間設置切換點，機器人通過所述切換點進行室內外切換，以實現在室內定位時加載室內地圖，在室外定位時實時加載機器人所在的局部地圖；s20、使用誤差狀態卡爾曼濾波器融合安裝于機器人上的慣性測量單元、輪式里程計和激光定位系統，對機器人進行定位。2.如權利要求1所述的室內外環境定位方法，其特征在于，在步驟s10中，將室外地圖切割成若干局部地圖，包括：s111、計算室外地圖的x坐標的最大值和最小值，以及y坐標的最大值和最小值；s112、設置切換步長、激光測量范圍和局部地圖邊長；s113、遍歷室外地圖上的點，若該點的x坐標和y坐標都能夠整除所述切換步長，則將該點作為局部地圖的中心點，并將室外地圖切割成若干局部地圖。3.如權利要求2所述的室內外環境定位方法，其特征在于，所述切換步長、激光測量范圍和局部地圖邊長滿足：size≥step+2*m其中，size為局部地圖邊長；step為切換步長；m為激光測量范圍。4.如權利要求1所述的室內外環境定位方法，其特征在于，在室內定位時使用2d激光定位系統，在室外定位時使用3d激光定位系統；在步驟s10中，所述機器人通過所述切換點進行室內外切換，包括：當從室內到達所述切換點時，將2d激光定位系統的當前定位數據傳輸至3d激光定位系統，對3d激光定位系統進行初始化定位；當從室外到達所述切換點時，將3d激光定位系統的當前定位數據傳輸至2d激光定位系統，對2d激光定位系統進行初始化定位。5.如權利要求4所述的室內外環境定位方法，其特征在于，通過以下步驟判斷初始化定位時是否成功，步驟包括：計算當前激光點云根據初始化定位位姿變換之后的點云中每一個點和地圖點云之間的最短歐氏距離的平均值，然后將平均值通過指數函數得到定位的分數，如果分數大于閾值，則初始化定位成功，否則不成功。6.如權利要求1所述的室內外環境定位方法，其特征在于，步驟s20包括：s21、狀態初始化步驟；包括：進行位姿初始化，并進行誤差狀態卡爾曼濾波器初始化；s22、模型預測步驟；包括：利用慣性測量單元測量的角速度和線性加速度積分來預測位姿，然后更新誤差狀態的雅可比矩陣和協方差矩陣；s23、觀測校正步驟；包括：先獲取輪式里程計的速度觀測值，并且根據ndt算法配準當前點云數據和地圖得到位姿，然后將觀測量傳遞到誤差狀態卡爾曼濾波器，計算誤差狀態卡爾曼增益，然后計算誤差狀態值和協方差矩陣，然后將誤差狀態值加到名義狀態值中更新名義狀態值，然后重置誤差狀態值。7.如權利要求6所述的室內外環境定位方法，其特征在于，s22模型預測步驟中，采用如下公式：
式中，p為機器人位置；r為機器人方向的旋轉矩陣；dt為時間間隔；v代表機器人速度；a
m
代表線加速度測量值；代表線加速度偏置；g代表重力加速度；q代表旋轉方向四元數；ω
m
代表角速度測量值；ε代表角速度偏置；下標k、k-1分別代表k、k-1時刻；上標t代表轉置；接著根據系統微分方程：計算系統狀態方程：x
k-1
＝f
k-1
x
k-1
+b
k-1
u
k-1
關于誤差狀態的雅可比矩陣f
k-1
；其中，為系統狀態量的導數；f
t
為系統微分方程狀態雅可比矩陣；e為系統微分方程擾動量；b
t
為系統微分方程擾動雅可比矩陣；f
k-1
為系統狀態方程狀態雅可比矩陣；u為系統狀態方程擾動量，b
k-1
為系統狀態方程擾動雅可比矩陣；之后系統狀態方程狀態使用雅可比矩陣f
k-1
更新誤差狀態的協方差矩陣，公式如下：其中，p是誤差狀態的協方差矩陣。8.如權利要求6所述的室內外環境定位方法，其特征在于，s23觀測校正步驟中，采用如下公式：y
k
＝g
k
x
k-1
+c
k
w
k
其中，y是誤差觀測量，包括位置誤差、角度誤差和速度誤差、x為誤差狀態量、w為觀測擾動量、g和c分別是誤差觀測量關于誤差狀態和觀測擾動的雅可比矩陣；由卡爾曼濾波可知，矯正方程為：p
k
＝(i-k
k
g
k
)p
k
x
k
＝x
k
+k
k
(y
k-g
k
x
k
)其中，p是誤差狀態的協方差矩陣；r是觀測擾動的協方差矩陣；i是單位矩陣；k為卡爾曼增益；下標k、k-1分別代表k、k-1時刻；上標t代表轉置。9.一種電子設備，包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的計算機程序，其特征在于，所述處理器執行所述程序時實現權利要求1-8中任意一項所述方法的步驟。10.一種計算機可讀存儲介質，其上存儲有計算機程序，其特征在于，該程序被處理器執行時實現權利要求1-8任意一項所述方法的步驟。
技術總結
本發明公開了一種室內外環境定位方法，包括：S10、將室外地圖切割成多個局部地圖，在室內地圖和室外地圖之間設置切換點，機器人通過所述切換點進行室內外切換，以實現在室內定位時加載室內地圖，在室外定位時實時加載機器人所在的局部地圖；S20、使用誤差狀態卡爾曼濾波器融合安裝于機器人上的慣性測量單元、輪式里程計和激光定位系統，對機器人進行定位。本發明可同時應用于室內和室外。根據機器人所在位置實時加載局部地圖，提高了計算效率，降低了系統計算負載。本發明使用了誤差狀態卡爾曼濾波器進行融合定位，預測部分使用慣性測量單元，觀測部分使用激光配準的位姿數據和輪式里程計的速度，提升了定位的精確性和魯棒性。提升了定位的精確性和魯棒性。提升了定位的精確性和魯棒性。
