[發(fā)明專利]一種室內(nèi)移動(dòng)定位與制圖方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201611121736.X
• 申請(qǐng)日: 2016-12-08
• 公開（公告）號(hào): CN106767820B
• 公開（公告）日: 2017-11-14
• 發(fā)明（設(shè)計(jì)）人: 李德仁;張橋;張文俊;孫凱
• 申請(qǐng)（專利權(quán)）人: 立得空間信息技術(shù)股份有限公司
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20;G01C21/32
• 代理公司: 武漢科皓知識(shí)產(chǎn)權(quán)代理事務(wù)所(特殊普通合伙)42222
• 代理人: 魏波

1.一種室內(nèi)移動(dòng)定位與制圖方法，其特征在于，包括以下步驟：

步驟1：激光點(diǎn)云數(shù)據(jù)獲取；

步驟2：點(diǎn)云數(shù)據(jù)預(yù)處理；

步驟3：將輸入的極坐標(biāo)系下的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到笛卡爾坐標(biāo)系下；

步驟4：點(diǎn)云插值及下采樣；

步驟5：粒子濾波預(yù)測姿態(tài)；

步驟6：子地圖生成；具體實(shí)現(xiàn)包括以下子步驟：

步驟6.1：設(shè)第一個(gè)落入地圖中的點(diǎn)云位置為地圖的起始位置start；

步驟6.2：當(dāng)有一個(gè)新的點(diǎn)云newPoint落入地圖中時(shí)，由于點(diǎn)云的連續(xù)性，其位置一定在start的8-鄰域中，則在該位置申請(qǐng)一小塊內(nèi)存，用于存儲(chǔ)newPoint的8-鄰域的點(diǎn)，n小塊內(nèi)存組成的地圖即為子地圖miniMap，且任意兩個(gè)miniMap需要有重疊；

步驟6.3：重復(fù)步驟6.2，生成最終地圖finalMap，且finalMap是不規(guī)則地圖的外接矩形，占用內(nèi)存最小；

步驟7：虛擬標(biāo)靶及局部捆集約束計(jì)算；

首先計(jì)算兩個(gè)子地圖相交部分概率最大的幾個(gè)特征點(diǎn)，將這些特征點(diǎn)設(shè)置為虛擬標(biāo)靶，其次對(duì)每一個(gè)虛擬標(biāo)靶分別與這兩個(gè)子地圖包含的點(diǎn)云的軌跡計(jì)算約束，也就是在某個(gè)軌跡坐標(biāo)系中計(jì)算標(biāo)靶的姿態(tài)，即為局部捆集約束；

步驟8：全局標(biāo)靶添加及約束計(jì)算；

步驟9：利用圖優(yōu)化進(jìn)行全局優(yōu)化；

步驟10：根據(jù)步驟9得到的優(yōu)化結(jié)果，更新軌跡和子地圖數(shù)據(jù)；

步驟11：將每一幀點(diǎn)云數(shù)據(jù)按照軌跡姿態(tài)進(jìn)行累加，生成全局地圖；

步驟12：輸出地圖。

2.根據(jù)權(quán)利要求1所述的室內(nèi)移動(dòng)定位與制圖方法，其特征在于：步驟1中，首先激光水平固定放置，標(biāo)定不同激光的相對(duì)位置，然后同步獲取水平方向和豎直方向激光數(shù)據(jù)。

3.根據(jù)權(quán)利要求1所述的室內(nèi)移動(dòng)定位與制圖方法，其特征在于：步驟2中，設(shè)定閾值，剔除距離大于閥值的點(diǎn)云數(shù)據(jù)，以保證輸入點(diǎn)云數(shù)據(jù)的精度。

4.根據(jù)權(quán)利要求1所述的室內(nèi)移動(dòng)定位與制圖方法，其特征在于：步驟4的具體實(shí)現(xiàn)包括以下子步驟：

步驟4.1：對(duì)于第i-1，i，i+1個(gè)點(diǎn)云數(shù)據(jù)d[i-1]，d[i]和d[i+1]，根據(jù)步驟3得到對(duì)應(yīng)的位置坐標(biāo)為(x_i-1,y_i-1)，(x_i,y_i)和(x_i+1,y_i+1)；

步驟4.2：判斷i-1與i之間是否產(chǎn)生了突變；如果min(d_i-1,i,d_i,i+1)<d_i-1,i+1，則認(rèn)為點(diǎn)云是連續(xù)的，未發(fā)生突變；則對(duì)這兩點(diǎn)點(diǎn)云之間進(jìn)行插值，插值個(gè)數(shù)為其中d_i-1,i表示i-1與i之間的距離，d_i,i+1表示i與i+1之間的距離，d_i-1,i+1表示i-1與i+1之間的距離；

步驟4.3：對(duì)插值后的點(diǎn)云，如果有某個(gè)(x_i,y_i)＝(x_j,y_j)，則丟掉其中一個(gè)點(diǎn)云，進(jìn)而達(dá)到下采樣的結(jié)果。

5.根據(jù)權(quán)利要求1所述的室內(nèi)移動(dòng)定位與制圖方法，其特征在于：步驟5的具體實(shí)現(xiàn)包括以下子步驟：

步驟5.1：將當(dāng)前幀點(diǎn)云數(shù)據(jù)變換到候選位置所對(duì)應(yīng)的姿態(tài)下；

步驟5.2：根據(jù)已生成的地圖，統(tǒng)計(jì)每個(gè)點(diǎn)云數(shù)據(jù)是障礙物的概率，對(duì)概率求和；

步驟5.3：概率最大的位置則認(rèn)為是當(dāng)前的最優(yōu)位置，也是下一個(gè)候選位置；

步驟5.4：重復(fù)步驟5.1-步驟5.3，直到最優(yōu)位置不再變化或變化范圍小于預(yù)設(shè)閥值，則結(jié)束。

6.根據(jù)權(quán)利要求1所述的室內(nèi)移動(dòng)定位與制圖方法，其特征在于，步驟8的具體實(shí)現(xiàn)包括以下子步驟：

步驟8.1：設(shè)點(diǎn)A為子地圖miniMap_i的一個(gè)特征點(diǎn)，點(diǎn)B為子地圖miniMap_j的對(duì)應(yīng)于A的特征點(diǎn)，則(A,B)記為子地圖i和j的一對(duì)全局標(biāo)靶；

步驟8.2：分別計(jì)算A與子地圖miniMap_j，B與子地圖miniMap_i中所有軌跡的對(duì)應(yīng)關(guān)系，形成全局約束。

7.根據(jù)權(quán)利要求1所述的室內(nèi)移動(dòng)定位與制圖方法，其特征在于：步驟9中，在點(diǎn)云逐幀匹配的過程中，誤差被逐漸累積，根據(jù)步驟7和步驟8中計(jì)算的約束，利用圖優(yōu)化算法解決大規(guī)模地圖生成過程中的累積誤差問題。