【技術(shù)實(shí)現(xiàn)步驟摘要】
本專利技術(shù)涉及一種可應(yīng)用于無(wú)人駕駛和工業(yè)機(jī)器人行業(yè)的基于激光雷達(dá)的重定位方法，屬于人工智能與物流倉(cāng)儲(chǔ)領(lǐng)域。
技術(shù)介紹
1、定位是建圖技術(shù)中的重要組成部分，在無(wú)人駕駛車輛、工業(yè)機(jī)器人、無(wú)人機(jī)等行業(yè)中具有關(guān)鍵應(yīng)用。如在物流倉(cāng)儲(chǔ)與快遞分揀行業(yè)使用的agv，其運(yùn)行的作業(yè)環(huán)境可以是已經(jīng)預(yù)先建立有激光點(diǎn)云地圖的區(qū)域，當(dāng)agv首次進(jìn)入或者在此區(qū)域中再次開(kāi)機(jī)時(shí)需要進(jìn)行重定位。重定位技術(shù)的作用是計(jì)算基于此點(diǎn)云地圖坐標(biāo)系的準(zhǔn)確坐標(biāo)位置及姿態(tài)，為其自主移動(dòng)運(yùn)行提供導(dǎo)航所需要的位姿初始數(shù)據(jù)。
2、如公開(kāi)號(hào)為cn118452743a的在先中國(guó)專利申請(qǐng)，其方案的實(shí)施對(duì)于環(huán)境參照物有著較為嚴(yán)格的要求，在無(wú)參照物的環(huán)境中此方案所提出重定位方法則會(huì)失敗。此外，參照物預(yù)先建立在地圖中，如果參照物發(fā)生移動(dòng)，依靠此參照物進(jìn)行重定位則會(huì)定位失敗。
3、又如公開(kāi)號(hào)為cn118466576a的在先中國(guó)專利申請(qǐng)，其方案要求有rtk參與，不僅設(shè)備成本較高且在隧道、地下車庫(kù)或者有高樓遮擋衛(wèi)星信號(hào)的場(chǎng)景下，重定位將會(huì)失敗或者出錯(cuò)。
4、綜上，現(xiàn)有技術(shù)的重定位方案均對(duì)環(huán)境與技術(shù)條件有著較強(qiáng)的依賴性，從而導(dǎo)致在部分場(chǎng)景下重定位失敗或出錯(cuò)，適用性與準(zhǔn)確性均難以滿足現(xiàn)有使用要求。有鑒于此，特提出本專利申請(qǐng)。
技術(shù)實(shí)現(xiàn)思路
1、本申請(qǐng)所述基于激光雷達(dá)的重定位方法，在于解決上述現(xiàn)有技術(shù)存在的問(wèn)題而提出僅使用設(shè)備自身配置的激光雷達(dá)，在不依賴外部環(huán)境參照物和gps信號(hào)的條件下準(zhǔn)確進(jìn)行重定位，以期克服在使用場(chǎng)景和依賴
2、為實(shí)現(xiàn)上述設(shè)計(jì)目的，所述基于激光雷達(dá)的重定位方法包括下述實(shí)施步驟：
3、步驟一)、建圖；
4、在運(yùn)行區(qū)域使用激光進(jìn)行掃描以構(gòu)建點(diǎn)云地圖；
5、步驟二)、重定位；
6、基于步驟一)構(gòu)建的點(diǎn)云地圖，判斷上一次定位前的狀態(tài)；
7、若是定位正常，則執(zhí)行下述步驟，
8、2.1.1)、讀取定位數(shù)據(jù)；
9、2.1.2)、與點(diǎn)云地圖配準(zhǔn)并計(jì)算得分；
10、當(dāng)前幀使用讀取到的位姿與點(diǎn)云地圖計(jì)算配準(zhǔn)得分，使用gicp位姿校驗(yàn)算法進(jìn)行位姿校驗(yàn)；得分足夠則重新定位成功，分?jǐn)?shù)不足則按下述定位異常流程執(zhí)行；
11、若是定位異常，則執(zhí)行下述描述子重定位過(guò)程，
12、2.2.1)、計(jì)算激光當(dāng)前幀的描述子；
13、2.2.2)、依次遍歷建圖中關(guān)鍵幀的描述子與當(dāng)前幀的描述子計(jì)算匹配值，匹配值結(jié)果即對(duì)應(yīng)關(guān)鍵幀與當(dāng)前幀的匹配度，匹配度最大者即為計(jì)算得到的候選幀；
14、2.2.3)、從建圖的位姿文件中獲取候選幀序號(hào)對(duì)應(yīng)的位姿；
15、2.2.4)、以上述對(duì)應(yīng)的位姿為中心點(diǎn)，使用64叉樹(shù)遍歷搜索的方法計(jì)算樹(shù)節(jié)點(diǎn)中的位姿，使用搜索到的位姿計(jì)算當(dāng)前幀點(diǎn)云中的每個(gè)點(diǎn)與地圖點(diǎn)的歐式距離，歐式距離的平均值最小的節(jié)點(diǎn)位姿即為匹配程度最高的位姿數(shù)據(jù)；
16、2.2.5)、計(jì)算得到的位姿結(jié)果數(shù)據(jù)使用前述的gicp位姿校驗(yàn)算法進(jìn)行進(jìn)一步的校驗(yàn)，如果校驗(yàn)通過(guò)，則進(jìn)行下一步的定位，如果校驗(yàn)沒(méi)有通過(guò)，取激光雷達(dá)傳感器中的下一幀激光數(shù)據(jù)，對(duì)步驟二)重新計(jì)算；
17、步驟三)、定位；
18、使用步驟二)重定位計(jì)算得到的位姿作為初始位姿，從激光雷達(dá)傳感器持續(xù)接收激光數(shù)據(jù)進(jìn)行連續(xù)的位姿計(jì)算，并對(duì)計(jì)算得到的位姿狀態(tài)進(jìn)行判斷；
19、3.1)、連續(xù)位姿計(jì)算；
20、對(duì)點(diǎn)云地圖構(gòu)建kd-tree，將從激光雷達(dá)接收到的最新激光數(shù)據(jù)做為最新的當(dāng)前幀，對(duì)當(dāng)前幀中的每一個(gè)點(diǎn)在地圖kd-tree中查找最近5個(gè)點(diǎn)，對(duì)查找到的5個(gè)點(diǎn)進(jìn)行平面擬合，計(jì)算點(diǎn)與擬合到平面的距離，將當(dāng)前幀點(diǎn)云中所有點(diǎn)與平面的距離作為優(yōu)化項(xiàng)，旋轉(zhuǎn)矩陣r和位移t作為優(yōu)化變量，使用高斯-牛頓優(yōu)化算法計(jì)算獲取最優(yōu)旋轉(zhuǎn)矩陣和位移的結(jié)果；
21、3.2)、基于位姿計(jì)算結(jié)果判斷定位狀態(tài)；
22、使用當(dāng)前幀和匹配到地圖中的點(diǎn)云數(shù)據(jù)計(jì)算微分熵、聯(lián)合熵和分離熵，判斷聯(lián)合熵和分離熵的差值，差值在閾值范圍內(nèi)，判斷定位狀態(tài)正常，若差值大于閾值，則判斷定位狀態(tài)異常；
23、定位狀態(tài)正常則記錄數(shù)據(jù)，為再次重啟設(shè)備進(jìn)行重定位提供計(jì)算數(shù)據(jù)；定位狀態(tài)異常，則不記錄數(shù)據(jù)。
24、進(jìn)一步地，所述的步驟一)，構(gòu)建點(diǎn)云地圖包括保存點(diǎn)云地圖文件、關(guān)鍵幀點(diǎn)云文件、關(guān)鍵幀描述子文件及關(guān)鍵幀對(duì)應(yīng)的位姿軌跡文件。
25、進(jìn)一步地，所述的計(jì)算配準(zhǔn)得分過(guò)程包括下述步驟：
26、獲取當(dāng)前激光雷達(dá)掃描數(shù)據(jù)，對(duì)當(dāng)前激光數(shù)據(jù)使用獲取到的位姿轉(zhuǎn)到地圖坐標(biāo)系中；
27、遍歷當(dāng)前幀每個(gè)點(diǎn)，在地圖中查找與此點(diǎn)距離最近的點(diǎn)，計(jì)算地圖點(diǎn)與此點(diǎn)的歐氏距離，如果此距離小于0.15-0.25米，則此點(diǎn)加入有效點(diǎn)集合；
28、計(jì)算有效點(diǎn)個(gè)數(shù)，若有效點(diǎn)數(shù)量大于所有點(diǎn)數(shù)的(60-80％)，則進(jìn)行下一步位姿校驗(yàn)；否則，直接執(zhí)行下述定位異常流程執(zhí)行。
29、進(jìn)一步地，所述的gicp位姿校驗(yàn)算法包括下述步驟:
30、進(jìn)行初始位姿變換；變換公式為qi＝r＊pi+t，其中，qi為源點(diǎn)經(jīng)過(guò)旋轉(zhuǎn)位移計(jì)算后在目標(biāo)坐標(biāo)系中的三維坐標(biāo)位置；pi為源點(diǎn)在xyz三維坐標(biāo)系中的坐標(biāo)位置，單位為米；r為源坐標(biāo)系到目標(biāo)坐標(biāo)系的旋轉(zhuǎn)矩陣；t為源坐標(biāo)系到目標(biāo)坐標(biāo)系的位移，單位為米；
31、計(jì)算點(diǎn)云協(xié)方差；選擇相鄰的n個(gè)點(diǎn)計(jì)算點(diǎn)云協(xié)方差，協(xié)方差公式如下，
32、其中，pi為當(dāng)前點(diǎn)的xyz三維坐標(biāo)系中的坐標(biāo)位置，單位為米，qj為鄰域點(diǎn)的xyz三維坐標(biāo)系中的坐標(biāo)位置，單位為米；
33、對(duì)目標(biāo)坐標(biāo)系中的點(diǎn)云構(gòu)建kd-tree，對(duì)當(dāng)前幀中的每一個(gè)點(diǎn)p在kd-tree中進(jìn)行距離搜索，搜索到的距離最小的點(diǎn)q即做為搜索結(jié)果，構(gòu)成源點(diǎn)p、目標(biāo)點(diǎn)q的點(diǎn)云配對(duì)；
34、計(jì)算源點(diǎn)p和目標(biāo)點(diǎn)q之間的歐式距離為誤差值；
35、誤差值計(jì)算公式為，d(pi,qi)＝(pi,qi)t(∑p+∑q)-1(pi-qi)；其中，∑p、∑q分別為源點(diǎn)p和目標(biāo)點(diǎn)q的協(xié)方差矩陣；
36、將所有點(diǎn)計(jì)算得到的距離誤差作為優(yōu)化項(xiàng)，旋轉(zhuǎn)矩陣r和位移t作為優(yōu)化變量，使用高斯-牛頓優(yōu)化算法計(jì)算獲取最優(yōu)旋轉(zhuǎn)矩陣和位移的結(jié)果。
37、進(jìn)一步地，所述的步驟2.2.1)包括下述過(guò)程：
38、以激光雷達(dá)原點(diǎn)為中心，將周圍區(qū)域以極坐標(biāo)形式徑向20至30等分，周向60至80等分，設(shè)定激光雷達(dá)點(diǎn)云最大距離lmax為80至100米；
39、計(jì)算徑向圈i∈{1,2,...,nr}和周向圈j∈{1,2,...,ns}內(nèi)的激光雷達(dá)點(diǎn)云集合ρij∈φ{(diào)ρij}的高度最大值；
40、根據(jù)ρij生成的nr×ns矩陣即為當(dāng)前幀描述子。
41、進(jìn)一步地，所述的步驟2.2.2)，計(jì)算匹配值公式如下：
42、
43、其中，iq和ic分別為關(guān)本文檔來(lái)自技高網(wǎng)...
【技術(shù)保護(hù)點(diǎn)】
1.一種基于激光雷達(dá)的重定位方法，其特征在于：包括下述步驟，
2.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的步驟一)，構(gòu)建點(diǎn)云地圖包括保存點(diǎn)云地圖文件、關(guān)鍵幀點(diǎn)云文件、關(guān)鍵幀描述子文件及關(guān)鍵幀對(duì)應(yīng)的位姿軌跡文件。
3.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的計(jì)算配準(zhǔn)得分過(guò)程包括下述步驟，
4.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的gicp位姿校驗(yàn)算法包括下述步驟，
5.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的步驟2.2.1)包括下述過(guò)程，
6.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的步驟2.2.2)，計(jì)算匹配值公式如下，
7.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的64叉樹(shù)遍歷搜索包括下述步驟，
8.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的步驟3.2)包括下述執(zhí)行步驟,
【技術(shù)特征摘要】
1.一種基于激光雷達(dá)的重定位方法，其特征在于：包括下述步驟，
2.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的步驟一)，構(gòu)建點(diǎn)云地圖包括保存點(diǎn)云地圖文件、關(guān)鍵幀點(diǎn)云文件、關(guān)鍵幀描述子文件及關(guān)鍵幀對(duì)應(yīng)的位姿軌跡文件。
3.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的計(jì)算配準(zhǔn)得分過(guò)程包括下述步驟，
4.根據(jù)權(quán)利要求1所述的基于激光雷達(dá)的重定位方法，其特征在于：所述的gicp位姿校驗(yàn)算法包括下...
【專利技術(shù)屬性】
技術(shù)研發(fā)人員：焦江磊，李文龍，虞家斌，
申請(qǐng)(專利權(quán))人：青島螞蟻機(jī)器人有限責(zé)任公司，
類型：發(fā)明
國(guó)別省市：
還沒(méi)有人留言評(píng)論。發(fā)表了對(duì)其他瀏覽者有用的留言會(huì)獲得科技券。