機(jī)器人的導(dǎo)航方法及裝置的制造方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于機(jī)器人技術(shù)領(lǐng)域，尤其涉及機(jī)器人的導(dǎo)航方法及裝置。
【背景技術(shù)】
[0002] 自上世紀(jì)60年代可移動(dòng)的機(jī)器在人在斯坦福大學(xué)出現(xiàn)以來(lái)，隨著科學(xué)技術(shù)的發(fā) 展，可移動(dòng)的機(jī)器人的應(yīng)用領(lǐng)域也發(fā)揮來(lái)越廣泛，從工業(yè)延伸至家庭、服務(wù)、娛樂(lè)、軍事等不 同的領(lǐng)域?？梢苿?dòng)的機(jī)器人的運(yùn)行是以導(dǎo)航為基礎(chǔ)的，需要機(jī)器人了解其所處環(huán)境的地圖 以及定位自身的位置，傳統(tǒng)的室內(nèi)可移動(dòng)的機(jī)器人通常采用激光設(shè)備導(dǎo)航，激光設(shè)備雖然 導(dǎo)航精度高，但其動(dòng)輒上萬(wàn)的價(jià)格帶來(lái)了移動(dòng)機(jī)器人過(guò)高的制造成本。

【發(fā)明內(nèi)容】

[0003] 有鑒于此，本發(fā)明實(shí)施例提供了機(jī)器人的導(dǎo)航方法及裝置，以解決現(xiàn)有的機(jī)器人 導(dǎo)航技術(shù)會(huì)導(dǎo)致機(jī)器人制造成本過(guò)高的問(wèn)題。
[0004] 第一方面，提供了一種機(jī)器人的導(dǎo)航方法，所述機(jī)器人中內(nèi)置了體感傳感器，所述 方法包括：
[0005] 將點(diǎn)云數(shù)據(jù)集Pk與所述體感傳感器采集到的上一幀的點(diǎn)云數(shù)據(jù)集X輸入KD樹匹配 器，在X中尋找離Pk最近的對(duì)應(yīng)數(shù)據(jù)點(diǎn)，生成點(diǎn)對(duì)集合Y k = C(Pk，X)，其中，所述k為迭代次數(shù)， 初始化k = 0，Po為所述體感傳感器采集到的當(dāng)前幀的點(diǎn)云數(shù)據(jù)集；
[0006] 將所述點(diǎn)云數(shù)據(jù)集Pk與所述點(diǎn)云數(shù)據(jù)集X進(jìn)行配準(zhǔn)；
[0007] 在未達(dá)到預(yù)設(shè)的終止條件之前，令k = k+l，返回執(zhí)行所述將點(diǎn)云數(shù)據(jù)集Pk與所述 體感傳感器采集到的上一幀的點(diǎn)云數(shù)據(jù)集X輸入KD樹匹配器的操作；
[0008] 若已達(dá)到所述預(yù)設(shè)的終止條件，根據(jù)當(dāng)前的點(diǎn)云數(shù)據(jù)集Pk矯正所述機(jī)器人的狀態(tài) 變化量。
[0009 ]第二方面，提供了一種機(jī)器人的導(dǎo)航裝置，所述機(jī)器人中內(nèi)置了體感傳感器，所述 裝置包括：
[0010] 輸入單元，用于將點(diǎn)云數(shù)據(jù)集Pk與所述體感傳感器采集到的上一幀的點(diǎn)云數(shù)據(jù)集 X輸入KD樹匹配器，在X中尋找離Pk最近的對(duì)應(yīng)數(shù)據(jù)點(diǎn)，生成點(diǎn)對(duì)集合Yk = C(Pk，X)，其中，所 述k為迭代次數(shù)，初始化k = 0，PQ為所述體感傳感器采集到的當(dāng)前幀的點(diǎn)云數(shù)據(jù)集；
[0011] 配準(zhǔn)單元，用于將所述點(diǎn)云數(shù)據(jù)集pk與所述點(diǎn)云數(shù)據(jù)集X進(jìn)行配準(zhǔn)；
[0012] 第一返回單元，用于在未達(dá)到預(yù)設(shè)的終止條件之前，令k = k+l，返回執(zhí)行所述輸入 單元的操作；
[0013] 第一矯正單元，用于若已達(dá)到所述預(yù)設(shè)的終止條件，根據(jù)當(dāng)前的點(diǎn)云數(shù)據(jù)集Pk矯 正所述機(jī)器人的狀態(tài)變化量。
[0014] 在本發(fā)明實(shí)施例中，利用體感傳感器來(lái)替代激光設(shè)備，實(shí)現(xiàn)機(jī)器人導(dǎo)航過(guò)程中的 位姿矯正，大大降低了機(jī)器人的制造成本。
【附圖說(shuō)明】
[0015] 為了更清楚地說(shuō)明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn)有技術(shù)描述 中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些 實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可以根據(jù)這些 附圖獲得其他的附圖。
[0016] 圖1是本發(fā)明實(shí)施例提供的機(jī)器人硬件系統(tǒng)的結(jié)構(gòu)框圖；
[0017] 圖2是本發(fā)明實(shí)施例提供的機(jī)器人軟件系統(tǒng)的結(jié)構(gòu)框圖；
[0018] 圖3是本發(fā)明實(shí)施例提供的機(jī)器人的導(dǎo)航方法的實(shí)現(xiàn)流程圖；
[0019] 圖4是本發(fā)明實(shí)施例提供的機(jī)器人的導(dǎo)航方法的處理模塊示意圖；
[0020] 圖5是本發(fā)明另一實(shí)施例提供的機(jī)器人的導(dǎo)航方法的實(shí)現(xiàn)流程圖；
[0021] 圖6是本發(fā)明實(shí)施例提供的機(jī)器人的導(dǎo)航方法的點(diǎn)集對(duì)齊示意圖；
[0022] 圖7是本發(fā)明實(shí)施例提供的機(jī)器人的導(dǎo)航裝置的結(jié)構(gòu)框圖。
【具體實(shí)施方式】
[0023] 以下描述中，為了說(shuō)明而不是為了限定，提出了諸如特定系統(tǒng)結(jié)構(gòu)、技術(shù)之類的具 體細(xì)節(jié)，以便透切理解本發(fā)明實(shí)施例。然而，本領(lǐng)域的技術(shù)人員應(yīng)當(dāng)清楚，在沒有這些具體 細(xì)節(jié)的其它實(shí)施例中也可以實(shí)現(xiàn)本發(fā)明。在其它情況中，省略對(duì)眾所周知的系統(tǒng)、裝置、電 路以及方法的詳細(xì)說(shuō)明，以免不必要的細(xì)節(jié)妨礙本發(fā)明的描述。
[0024] 圖1示出了本發(fā)明實(shí)施例提供的機(jī)器人硬件系統(tǒng)的結(jié)構(gòu)框圖，為了便于說(shuō)明，僅示 出了與本實(shí)施例相關(guān)的部分。
[0025] 如圖1所示，機(jī)器人的硬件系統(tǒng)主要由以下幾個(gè)部分組成：
[0026] 1、工業(yè)控制計(jì)算機(jī):其上搭載了操作系統(tǒng)，用于對(duì)數(shù)據(jù)進(jìn)行處理和存儲(chǔ)，并為硬件 系統(tǒng)中的其他模塊提供服務(wù)；
[0027] 2、體感傳感器:用于收集機(jī)器人所處環(huán)境的色彩和深度信息；
[0028] 3、運(yùn)動(dòng)傳感器:用于獲取機(jī)器人的運(yùn)動(dòng)信息，包括碼盤、陀螺儀等傳感器；
[0029] 4、驅(qū)動(dòng)控制單元:通過(guò)將驅(qū)動(dòng)信號(hào)輸入底層控制器，以驅(qū)動(dòng)機(jī)器人行走。
[0030] 5、其他模塊:包括顯示模塊，通信模塊及各模塊之間的通信接口。
[0031]圖2示出了本發(fā)明實(shí)施例提供的機(jī)器人軟件系統(tǒng)的結(jié)構(gòu)框圖，為了便于說(shuō)明，僅示 出了與本實(shí)施例相關(guān)的部分。
[0032] 如圖2所示，機(jī)器人的軟件系統(tǒng)由下至上主要由以下幾個(gè)部分組成：
[0033] 1、操作系統(tǒng):其搭載在工業(yè)控制計(jì)算機(jī)之上；
[0034] 2、機(jī)器人驅(qū)動(dòng)程序；
[0035] 3、機(jī)器人操作系統(tǒng)(R0S);
[0036] 4、導(dǎo)航功能包:其安裝在機(jī)器人操作系統(tǒng)之中。
[0037]在本發(fā)明實(shí)施例中，采用工業(yè)控制計(jì)算機(jī)作為主要計(jì)算處理單元，體感傳感器、碼 盤將采集的數(shù)據(jù)發(fā)送到工業(yè)控制計(jì)算機(jī)，通過(guò)計(jì)算構(gòu)建地圖并實(shí)現(xiàn)移動(dòng)機(jī)器人的定位。在 導(dǎo)航的過(guò)程中，移動(dòng)機(jī)器人通過(guò)底層的嵌入式系統(tǒng)控制機(jī)器人驅(qū)動(dòng)行走。
[0038]基于圖1和圖2所示的機(jī)器人的軟硬件結(jié)構(gòu)，接下來(lái)對(duì)本發(fā)明實(shí)施例提供的機(jī)器人 的導(dǎo)航方法進(jìn)行闡述：
[0039] 圖3示出了本發(fā)明實(shí)施例提供的機(jī)器人的導(dǎo)航方法的實(shí)現(xiàn)流程，詳述如下：
[0040] 在S301中，將點(diǎn)云數(shù)據(jù)集Pk與所述體感傳感器采集到的上一幀的點(diǎn)云數(shù)據(jù)集X輸 入KD (K-d imens i ona 1，K維)樹匹配器，在X中尋找離Pk最近的對(duì)應(yīng)數(shù)據(jù)點(diǎn)，生成點(diǎn)對(duì)集合Yk = 以?1<3)，其中，所述1^為迭代次數(shù)，初始化1^ = 0，？()為所述體感傳感器采集到的當(dāng)前幀的點(diǎn)云 數(shù)據(jù)集。
[0041] 體感傳感器采集圖像的速率為30幀/秒，通過(guò)迭代最近點(diǎn)方法，不斷地把體感傳感 器采集到的當(dāng)前幀的點(diǎn)云數(shù)據(jù)與上一幀的點(diǎn)云數(shù)據(jù)匹配，以實(shí)現(xiàn)進(jìn)一步的位姿矯正。如圖4 所示，數(shù)據(jù)點(diǎn)濾波器把點(diǎn)云數(shù)據(jù)作為輸入，并在處理后輸出另一點(diǎn)云數(shù)據(jù)，其處理方式包括 增加描述符、通過(guò)采樣減少數(shù)據(jù)點(diǎn)數(shù)量等，也可以同時(shí)使用多個(gè)數(shù)據(jù)點(diǎn)濾波器，用戶可按需 選擇。
[0042] 進(jìn)一步地，在S301之后，S302之前，所述方法還包括：
[0043] 通過(guò)外層濾波器檢測(cè)所述點(diǎn)對(duì)集合Yk，去除所述點(diǎn)對(duì)集合Yk中不滿足預(yù)設(shè)規(guī)則的 點(diǎn)對(duì)。
[0044] 例如，可以通過(guò)判斷兩個(gè)點(diǎn)之間的距離是否超過(guò)某個(gè)閾值，若超過(guò)則去除這兩個(gè) 點(diǎn)。
[0045]在S302中，將所述點(diǎn)云數(shù)據(jù)集Pk與所述點(diǎn)云數(shù)據(jù)集X進(jìn)行配準(zhǔn)。
[0046]具體地，可以首先計(jì)算矩陣Q( Spy)的特征向量qR= [qo qi q2 q3]，其中，
所述ΣΡΥ是點(diǎn)集Pk和Yk 的協(xié)互方差矩陣，I3是3階單位矩陣，Δ=[Α23 A31 A12]t，
[0047]其次，根據(jù)所述特征向量qR計(jì)算旋轉(zhuǎn)矩陣R和平移向量qT，獲取配準(zhǔn)向量q= [qR q τ ] T，以實(shí)現(xiàn)將所述點(diǎn)云數(shù)據(jù)集P k與所述點(diǎn)云數(shù)據(jù)集X進(jìn)行配準(zhǔn)。其中：
)，qT = yy-R (qR) μρ，所述yy和μΑ別是點(diǎn)集Yk和Pk的質(zhì)心。
[0048]在S303中，在未達(dá)到預(yù)設(shè)的終止條件之前，令k = k+l，返回執(zhí)行所述將點(diǎn)云數(shù)據(jù)集 Pk與所述體感傳感器采集到的上一幀的點(diǎn)云數(shù)據(jù)集X輸入KD樹匹配器的操作。
[0049]在S304中，若已達(dá)到所述預(yù)設(shè)的終止條件，根據(jù)當(dāng)前的點(diǎn)云數(shù)據(jù)集Pk矯正所述機(jī) 器人的狀態(tài)變化量。
[0050]在本發(fā)明實(shí)施例中，所述預(yù)設(shè)的終止條件包括：
[0051 ]所述k