買專利賣專利找龍圖騰，真高效！ 查專利查商標(biāo)用IPTOP,全免費(fèi)！專利年費(fèi)監(jiān)控用IP管家,真方便！

龍圖騰網(wǎng)獲悉西北工業(yè)大學(xué)申請的專利基于GPS引導(dǎo)式粒子濾波的機(jī)器人多傳感器融合定位方法獲國家發(fā)明授權(quán)專利權(quán)，本發(fā)明授權(quán)專利權(quán)由國家知識(shí)產(chǎn)權(quán)局授予，授權(quán)公告號(hào)為：CN115900708B 。

龍圖騰網(wǎng)通過國家知識(shí)產(chǎn)權(quán)局官網(wǎng)在2025-08-15發(fā)布的發(fā)明授權(quán)授權(quán)公告中獲悉：該發(fā)明授權(quán)的專利申請?zhí)?專利號(hào)為：202211263413.X，技術(shù)領(lǐng)域涉及：G01C21/20；該發(fā)明授權(quán)基于GPS引導(dǎo)式粒子濾波的機(jī)器人多傳感器融合定位方法是由唐煒;劉恩博;王增輝;吳佳樂;張仁遠(yuǎn)設(shè)計(jì)研發(fā)完成，并于2022-10-14向國家知識(shí)產(chǎn)權(quán)局提交的專利申請。

本基于GPS引導(dǎo)式粒子濾波的機(jī)器人多傳感器融合定位方法在說明書摘要公布了：本發(fā)明涉及一種基于GPS引導(dǎo)式粒子濾波的機(jī)器人多傳感器融合定位方法，在3D點(diǎn)云地圖中基于GPS導(dǎo)引式粒子濾波，實(shí)現(xiàn)差分GPS、激光雷達(dá)和IMU的融合。判斷有效GPS粒子數(shù)量的方式?jīng)Q定是否在GPS量測值附近添加少量新粒子，降低定位方法對GPS信號(hào)的依賴程度，GPS僅在信號(hào)好時(shí)發(fā)揮引導(dǎo)粒子運(yùn)動(dòng)方向的作用。利用激光雷達(dá)或IMU的量測數(shù)據(jù)計(jì)算粒子權(quán)值，而不采用與GPS加權(quán)的方式，避免了GPS生成的粒子會(huì)在GPS量測下得到較高的權(quán)值。當(dāng)GPS定位數(shù)據(jù)不可用時(shí)，加入的GPS粒子在激光量測下將得到較低的權(quán)值，同時(shí)為了避免添加GPS粒子帶來的計(jì)算量的問題，在重采樣步驟前對粒子進(jìn)行隨機(jī)降采樣，確保粒子數(shù)量始終維持在設(shè)定范圍內(nèi)，保證了定位系統(tǒng)的實(shí)時(shí)性。

本發(fā)明授權(quán)基于GPS引導(dǎo)式粒子濾波的機(jī)器人多傳感器融合定位方法在權(quán)利要求書中公布了：1.一種基于GPS引導(dǎo)式粒子濾波的機(jī)器人多傳感器融合定位方法，其特征在于步驟如下： 步驟1：對傳感器數(shù)據(jù)進(jìn)行預(yù)處理，即將IMU數(shù)據(jù)進(jìn)行平滑濾波，將差分GPS獲取的經(jīng)緯度坐標(biāo)以及偏航角轉(zhuǎn)化為UTM坐標(biāo)系下； 步驟2：遙控機(jī)器人運(yùn)動(dòng)，記錄起點(diǎn)處GPS的定位信息構(gòu)造GPS里程計(jì)，并在起點(diǎn)處建立全局點(diǎn)云地圖坐標(biāo)系Map系與GPS東北天坐標(biāo)系gps系；將預(yù)處理后的IMU數(shù)據(jù)、激光雷達(dá)數(shù)據(jù)和GPS里程計(jì)數(shù)據(jù)作為3DSLAM算法的輸入，輸出為全局點(diǎn)云地圖，并保存全局點(diǎn)云地圖為pcd格式，此地圖是3D點(diǎn)云地圖，無需進(jìn)行2D投影； 步驟3：計(jì)算機(jī)器人在GPS東北天坐標(biāo)系gps系下的定位數(shù)據(jù): 式中，為k時(shí)刻GPS在UTM坐標(biāo)系下的定位數(shù)據(jù)； 將機(jī)器人在GPS東北天坐標(biāo)系gps系中的位置信息變換到全局點(diǎn)云地圖坐標(biāo)系Map系下： 計(jì)算GPS數(shù)據(jù)變換到全局點(diǎn)云地圖坐標(biāo)系下的定位數(shù)據(jù)，并發(fā)布該定位數(shù)據(jù)，后續(xù)步驟用到的GPS定位數(shù)據(jù)均為該數(shù)據(jù)： 步驟4：加載構(gòu)建好的全局點(diǎn)云地圖，利用激光雷達(dá)、IMU和GPS的定位數(shù)據(jù)基于粒子濾波框架實(shí)現(xiàn)融合定位，具體步驟如下： 步驟1初始化粒子 步驟2對每個(gè)粒子的狀態(tài)進(jìn)行預(yù)測 步驟3通過獲取的激光雷達(dá)數(shù)據(jù)與IMU數(shù)據(jù)計(jì)算每個(gè)粒子的權(quán)值 對于激光雷達(dá)數(shù)據(jù)，利用體素降采樣過濾點(diǎn)云數(shù)據(jù)，將過濾后的點(diǎn)云數(shù)據(jù)利用正態(tài)分布變換法NDT與3D點(diǎn)云地圖中每個(gè)粒子位姿處的點(diǎn)云匹配，并利用得分函數(shù)h1·計(jì)算一次迭代配準(zhǔn)的得分，計(jì)算粒子權(quán)值： 其中，為上一時(shí)刻粒子權(quán)值，為測量概率，zlaser指當(dāng)前時(shí)刻經(jīng)過體素降采樣后的激光雷達(dá)數(shù)據(jù)，h1·指激光雷達(dá)數(shù)據(jù)通過NDT一次迭代配準(zhǔn)的得分函數(shù)，指粒子狀態(tài)的預(yù)測值； 對于IMU數(shù)據(jù)，計(jì)算粒子權(quán)值： 其中，為上一時(shí)刻粒子權(quán)值，為測量概率，指粒子狀態(tài)的預(yù)測值，zimu是當(dāng)前時(shí)刻的IMU的量測數(shù)據(jù)，h2·是IMU的觀測方程，Qk+1是IMU觀測協(xié)方差矩陣； 所述IMU的觀測方程h2·： 其中，δk+1是IMU的觀測噪聲，δk+1的協(xié)方差為Qk+1，Qk+1用于衡量當(dāng)前時(shí)刻IMU測量誤差的大小； 步驟4對每個(gè)粒子的權(quán)值進(jìn)行歸一化，獲得每個(gè)粒子歸一化后的權(quán)值 步驟5對粒子進(jìn)行重采樣，復(fù)制權(quán)值較高的粒子，去除權(quán)值較低的粒子，獲取機(jī)器人位置的真實(shí)概率分布； 步驟6發(fā)布當(dāng)前時(shí)刻機(jī)器人的定位結(jié)果Φk+1： 步驟7添加GPS粒子，執(zhí)行完轉(zhuǎn)步驟2進(jìn)行下一次預(yù)測，直至結(jié)束； 經(jīng)過重采樣后的粒子在GPS量測下的權(quán)值 其中g(shù)·為GPS的觀測方程： zk+1指當(dāng)前時(shí)刻GPS的量測數(shù)據(jù)，Σk+1指GPS測量噪聲的協(xié)方差矩陣，指當(dāng)前時(shí)刻第i個(gè)粒子的狀態(tài)； 定義在GPS量測數(shù)據(jù)下，權(quán)值的粒子稱之有效GPS粒子，計(jì)算有效GPS粒子數(shù)占總粒子數(shù)的百分比Ng： 判斷：當(dāng)有效GPS粒子數(shù)占比Ng小于閾值Nth時(shí)，向粒子群中添加m個(gè)粒子； 當(dāng)有效GPS粒子數(shù)占比Ng大于閾值Nth時(shí)，目前粒子的位姿與GPS的定位數(shù)據(jù)接近，不需要添加新粒子，返回2進(jìn)行下一步預(yù)測； 所述m個(gè)粒子服從Nμk+1,Σk+1的正態(tài)分布，μk+1＝xk+1,yk+1,θk+1和Σk+1分別是當(dāng)前時(shí)刻GPS定位結(jié)果和定位協(xié)方差矩陣，從正態(tài)分布Nμk+1,Σk+1中隨機(jī)生成該m個(gè)粒子： m＝[Nth-Ng·n]， 其中：[]為取整函數(shù)，n為粒子群中粒子總數(shù)。

如需購買、轉(zhuǎn)讓、實(shí)施、許可或投資類似專利技術(shù)，可聯(lián)系本專利的申請人或?qū)＠麢?quán)人西北工業(yè)大學(xué)，其通訊地址為：710072 陜西省西安市友誼西路；或者聯(lián)系龍圖騰網(wǎng)官方客服，聯(lián)系龍圖騰網(wǎng)可撥打電話0551-65771310或微信搜索“龍圖騰網(wǎng)”。