動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法
【專利摘要】本發(fā)明涉及移動(dòng)機(jī)器人自主導(dǎo)航【技術(shù)領(lǐng)域】。本發(fā)明公開了一種動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法。包括如下步驟：1)利用室內(nèi)環(huán)境下多臺(tái)全局?jǐn)z像機(jī)與機(jī)器人車載激光傳感器實(shí)現(xiàn)人的位置跟蹤；2)根據(jù)采集樣本對(duì)特定室內(nèi)環(huán)境場(chǎng)所下人的運(yùn)動(dòng)模式進(jìn)行訓(xùn)練，并對(duì)人的運(yùn)動(dòng)趨勢(shì)進(jìn)行預(yù)測(cè)；3)根據(jù)人的當(dāng)前位置和預(yù)測(cè)位置，與環(huán)境靜態(tài)障礙柵格地圖相融合，生成導(dǎo)航風(fēng)險(xiǎn)概率地圖；4)采用全局路徑規(guī)劃-局部避障控制層次化結(jié)構(gòu)的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器，實(shí)現(xiàn)機(jī)器人導(dǎo)航行為控制，通過控制確保機(jī)器人在與人共處的復(fù)雜動(dòng)態(tài)環(huán)境下安全高效的導(dǎo)航行為。
【專利說明】動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及移動(dòng)機(jī)器人導(dǎo)航【技術(shù)領(lǐng)域】，特別是涉及一種多約束動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法。服務(wù)機(jī)器人在與人共處的動(dòng)態(tài)環(huán)境下導(dǎo)航，除了常規(guī)的避障以外還存在多種約束條件相互疊加，而其中由于人的運(yùn)動(dòng)對(duì)機(jī)器人造成的干擾是不完全已知的。本發(fā)明對(duì)這種動(dòng)態(tài)干擾進(jìn)行量化和一定預(yù)測(cè)，并將機(jī)器人的其它多種運(yùn)動(dòng)規(guī)則轉(zhuǎn)換為約束的形式融入可達(dá)代價(jià)導(dǎo)航柵格，通過建立兩層級(jí)聯(lián)的運(yùn)動(dòng)控制器，實(shí)現(xiàn)了多約束條件制約下的機(jī)器人優(yōu)化導(dǎo)航行為控制，從而有助于提高服務(wù)機(jī)器人未來進(jìn)入家庭后的可靠導(dǎo)航作業(yè)問題。
【背景技術(shù)】
[0002]移動(dòng)機(jī)器人的點(diǎn)到點(diǎn)導(dǎo)航，需要依據(jù)某些優(yōu)化準(zhǔn)則(時(shí)間或行程最短)，在其位形空間(ConfigurationSpace, CSpace)中搜索一條合理、完備、最優(yōu)、可實(shí)時(shí)計(jì)算、能適應(yīng)環(huán)境變化的路徑。從機(jī)器人獲取環(huán)境信息的程度可分為三種情況:
[0003](I)環(huán)境信息完全已知，這是基于環(huán)境模型的全局路徑規(guī)劃的前提；
[0004](2)環(huán)境信息完全未知或部分已知，這是基于傳感器信息的局部路徑規(guī)劃所適用的情況；
[0005](3)環(huán)境中存在以可知或可預(yù)知方式移動(dòng)的障礙物(例如人)，此時(shí)傳統(tǒng)的局部規(guī)劃法雖然能保證避障，但是局部性的避障行為難以提高導(dǎo)航的全局效率。
[0006]對(duì)于動(dòng)態(tài)環(huán)境，為了提高動(dòng)態(tài)路徑重規(guī)劃的計(jì)算效率，一般采用基于滾動(dòng)窗口的移動(dòng)機(jī)器人路徑規(guī)劃方法，充分利用機(jī)器人實(shí)時(shí)測(cè)得的局部環(huán)境信息，以滾動(dòng)方式進(jìn)行在線規(guī)劃，對(duì)動(dòng)態(tài)環(huán)境具有良好的適應(yīng)性。Minguez采用了全局/局部規(guī)劃兩層控制結(jié)構(gòu)(參見 “Sensor-basedrobotmotiongenerationinunknown，dynamicandtroublesomescenarios,RoboticsandAutonomousSystems, 2005”)，將 wavefront 路徑規(guī)劃與 ND 反應(yīng)式避障算法組成級(jí)聯(lián)，提高了在存在人的動(dòng)態(tài)環(huán)境下機(jī)器人避碰運(yùn)動(dòng)的路徑尋優(yōu)能力與快速響應(yīng)能力。
[0007]動(dòng)態(tài)環(huán)境下對(duì)運(yùn)動(dòng)物體的預(yù)測(cè)導(dǎo)航(PredictiveNavigation)是提高導(dǎo)航全局效率的新方法。預(yù)測(cè)導(dǎo)航需要恢復(fù)避讓對(duì)象的位置速度等狀態(tài)信息，通過將CSpace擴(kuò)展為完整的狀態(tài)一時(shí)間描述或者把障礙物運(yùn)動(dòng)軌跡直接映射到速度空間進(jìn)行全局運(yùn)動(dòng)規(guī)劃。例如針對(duì)足球機(jī)器人，Gupta (參見 “Real-Timeldentificationand PredictiveControlofFastMobileRobotsusingGlobalVisionSensing, IEEE TransactionsonInstrumentationandMeasurement, 2005”)線性預(yù)測(cè)了直線運(yùn)動(dòng)的障礙物(足球)的運(yùn)動(dòng)速度變化。
[0008]服務(wù)型移動(dòng)機(jī)器人工作在與人共處的動(dòng)態(tài)環(huán)境下。對(duì)運(yùn)動(dòng)人/物體的動(dòng)態(tài)性加以預(yù)測(cè)，是機(jī)器人改善避障導(dǎo)航性能的有效途徑?，F(xiàn)有預(yù)測(cè)避讓導(dǎo)航方法一般建立在動(dòng)態(tài)物體運(yùn)動(dòng)路線已知、速度已知、勻速運(yùn)動(dòng)、或者均加速運(yùn)動(dòng)等各種假設(shè)上。由于人的運(yùn)動(dòng)狀態(tài)不可知，人的運(yùn)動(dòng)預(yù)測(cè)不同于運(yùn)動(dòng)物體的預(yù)測(cè)，假設(shè)其運(yùn)動(dòng)路線與具體運(yùn)動(dòng)速度已知在實(shí)際應(yīng)用中是不現(xiàn)實(shí)的。近年來國外研究在針對(duì)人的機(jī)器人預(yù)測(cè)導(dǎo)航方面出現(xiàn)了一些有益的嘗試，Bennewitz (參見“UsingEMto LearnMotionBehaviorsofPersonswithMobiIeRobots, IEEE/RSJInternational ConferenceonIntelIigentRobotsandSystems, 2002，，)提出的方法建模并預(yù)測(cè)了人的運(yùn)動(dòng)路線模式，卻沒有考慮人沿著某路線行走的速度變化對(duì)預(yù)測(cè)人機(jī)相遇沖突帶來的影響。
[0009]經(jīng)專利檢索查新，王耀南等人申請(qǐng)了中國發(fā)明專利第200910044273.5號(hào)，名稱為“未知環(huán)境下移動(dòng)機(jī)器人導(dǎo)航安全的方法”。該發(fā)明公開了一種借助柵格地圖和模糊神經(jīng)網(wǎng)絡(luò)控制器來確保移動(dòng)機(jī)器人在未知環(huán)境下安全探索導(dǎo)航的方法。但是該方法沒有考慮環(huán)境中的動(dòng)態(tài)物體及人對(duì)機(jī)器人在柵格地圖中導(dǎo)航所造成的影響，其自主導(dǎo)航系統(tǒng)也僅采用了機(jī)器人車載測(cè)距傳感器來感知環(huán)境障礙物，而車載測(cè)距傳感器在探測(cè)范圍上具有一定局限性。
[0010]梁華為等人申請(qǐng)了中國發(fā)明專利第200610096976.9號(hào)，名稱為“一種機(jī)器人導(dǎo)航
定位系統(tǒng)及導(dǎo)航定位方法”。該發(fā)明公開了一種機(jī)器人導(dǎo)航定位系統(tǒng)及導(dǎo)航定位方法。系統(tǒng)包括應(yīng)用于機(jī)器人導(dǎo)航定位系統(tǒng)的無線傳感器網(wǎng)絡(luò)和機(jī)器人，無線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)由傳感單元、處理單元、定位單元、無線模塊和電源模塊組成；導(dǎo)航定位是通過布撒于監(jiān)控區(qū)域內(nèi)的無線傳感器網(wǎng)絡(luò)節(jié)點(diǎn)輔助機(jī)器人進(jìn)行環(huán)境建模、定位、路徑規(guī)劃和導(dǎo)航。雖然該方法采用了分布式感知技術(shù)來輔助機(jī)器人導(dǎo)航，但也沒有考慮通過對(duì)人的跟蹤和運(yùn)動(dòng)預(yù)測(cè)來指導(dǎo)機(jī)器人對(duì)其合理避讓。
[0011]賈慶軒等人申請(qǐng)了中國發(fā)明專利第201210355341.1號(hào)，名稱為“預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障方法”。該方法考慮了障礙物隨機(jī)運(yùn)動(dòng)的情況下，預(yù)選擇最小距離指標(biāo)冗余機(jī)器人動(dòng)態(tài)避障路徑規(guī)劃，提高了動(dòng)態(tài)避障的規(guī)劃效率，滿足實(shí)時(shí)性的要求。該方法適用于一般隨機(jī)運(yùn)動(dòng)的障礙物情況，但是對(duì)于家庭室內(nèi)環(huán)境下機(jī)器人導(dǎo)航，該方法不適合用于對(duì)人的運(yùn)動(dòng)進(jìn)行跟蹤和預(yù)測(cè)。此外，一般現(xiàn)有方法大多采用避障路徑重規(guī)劃方式來機(jī)械地避讓，而沒有考慮采取繞行、減速、等待等其它避讓策略，往往會(huì)導(dǎo)致機(jī)器人徘徊運(yùn)動(dòng)，降低了機(jī)器人導(dǎo)航作業(yè)的效率。
[0012]根據(jù)以上調(diào)研，雖然機(jī)器人避障和導(dǎo)航控制已有較為廣泛的研究，目前尚未特別針對(duì)與人共存機(jī)器人(Human-symbioticrobots)的導(dǎo)航控制問題建立相應(yīng)的技術(shù)方法。服務(wù)機(jī)器人在與人共處的動(dòng)態(tài)環(huán)境下導(dǎo)航，除了常規(guī)的避障以外還存在多種約束條件相互疊力口，其中由于人的運(yùn)動(dòng)對(duì)機(jī)器人造成的干擾是不完全已知的，而目前已有的技術(shù)方案大多沒有考慮這些因素，這制約了服務(wù)機(jī)器人的安全可靠導(dǎo)航性能。

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

[0013]技術(shù)問題:針對(duì)與人共處動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航，本發(fā)明提出了一種多約束動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法。
[0014]技術(shù)方案:多約束動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，所述方法包括:
[0015]利用室內(nèi)環(huán)境下多臺(tái)全局?jǐn)z像機(jī)與機(jī)器人車載激光傳感器組成的分布式傳感器網(wǎng)絡(luò)，實(shí)現(xiàn)全局地圖中動(dòng)態(tài)人的定位與跟蹤；
[0016]通過采集人的長期運(yùn)動(dòng)軌跡學(xué)習(xí)上述室內(nèi)環(huán)境下人的運(yùn)動(dòng)模式；而機(jī)器人在導(dǎo)航過程中，通過對(duì)人運(yùn)動(dòng)的長期運(yùn)動(dòng)規(guī)律預(yù)測(cè)和短期運(yùn)動(dòng)速度、方向預(yù)測(cè)相結(jié)合，對(duì)人的時(shí)空運(yùn)動(dòng)趨勢(shì)進(jìn)行估計(jì)；
[0017]根據(jù)人的當(dāng)前位置和上述估計(jì)得到的預(yù)測(cè)位置，與環(huán)境靜態(tài)障礙柵格地圖相融合，生成導(dǎo)航風(fēng)險(xiǎn)概率地圖；
[0018]采用全局路徑規(guī)劃-局部避障控制層次化結(jié)構(gòu)的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器，實(shí)現(xiàn)機(jī)器人導(dǎo)航行為控制。通過控制并最終確保機(jī)器人在與人共處的復(fù)雜動(dòng)態(tài)環(huán)境下安全高效的導(dǎo)航行為。
[0019]其中所述的人的定位與跟蹤，是在室內(nèi)環(huán)境下采用多臺(tái)全局?jǐn)z像機(jī)與機(jī)器人車載的單臺(tái)激光掃描器實(shí)現(xiàn)人的位置跟蹤，通過標(biāo)定全局?jǐn)z像機(jī)與全局柵格地圖的坐標(biāo)轉(zhuǎn)換關(guān)系，獲取柵格地圖中人的全局位姿及運(yùn)動(dòng)速度。
[0020]其中所述的對(duì)人的時(shí)空運(yùn)動(dòng)趨勢(shì)進(jìn)行估計(jì)，步驟如下:
[0021]I)首先在人的定位與跟蹤基礎(chǔ)上進(jìn)行人的運(yùn)動(dòng)軌跡模式訓(xùn)練和預(yù)測(cè):長期采集的行人位置數(shù)據(jù)，采用EM算法學(xué)習(xí)其運(yùn)動(dòng)模式(軌跡)，對(duì)每種運(yùn)動(dòng)軌跡模式在x-y平面中用混合高斯分布加以建模，在實(shí)時(shí)對(duì)人運(yùn)動(dòng)位置進(jìn)行跟蹤的基礎(chǔ)上，依據(jù)人的運(yùn)動(dòng)歷史估計(jì)出當(dāng)前運(yùn)動(dòng)遵循軌跡模式的概率；
[0022]2)其次進(jìn)行人的短期運(yùn)動(dòng)預(yù)測(cè):假設(shè)人的運(yùn)動(dòng)速度和方向角每過一定時(shí)刻速度和方向角變化一次，且在一定范圍內(nèi)變化，各個(gè)時(shí)刻上的速度和方向角分別為相應(yīng)范圍內(nèi)的隨機(jī)變量且相互獨(dú)立，對(duì)人沿著該預(yù)測(cè)路徑前進(jìn)的短期運(yùn)動(dòng)速度和方向角進(jìn)行預(yù)測(cè)。
[0023]生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括對(duì)運(yùn)動(dòng)人的預(yù)測(cè)避讓可達(dá)代價(jià)的導(dǎo)航約束條件，即當(dāng)人機(jī)運(yùn)動(dòng)可能在狹窄通道內(nèi)出現(xiàn)沖突時(shí)，通過預(yù)測(cè)人的運(yùn)動(dòng)位置來估計(jì)人的時(shí)空可達(dá)代價(jià)，并以降低人機(jī)運(yùn)動(dòng)沖突或阻塞風(fēng)險(xiǎn)為目標(biāo)，控制機(jī)器人提前繞行或減速避讓。
[0024]生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括帶裕量的機(jī)器人四周安全范圍可達(dá)代價(jià)的導(dǎo)航約束條件，即在機(jī)器人坐標(biāo)位置的四周，用高斯分布的點(diǎn)云表示該點(diǎn)處障礙對(duì)機(jī)器人安全威脅概率。
[0025]生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括受建筑物遮擋或警戒區(qū)域繞行可達(dá)代價(jià)的導(dǎo)航約束條件，即控制機(jī)器人緩慢地走出建筑物遮擋范圍。
[0026]生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括遵守行駛靠向習(xí)慣可達(dá)代價(jià)的導(dǎo)航約束條件，即將靠向準(zhǔn)則引入機(jī)器人運(yùn)動(dòng)規(guī)劃，確保機(jī)器人在與人相遇時(shí)雙方遵守相同的靠向規(guī)則，達(dá)到最優(yōu)的導(dǎo)航效率。
[0027]將環(huán)境中已知靜態(tài)障礙物采用柵格占有地圖描述，與按上述四條新的導(dǎo)航約束條件計(jì)算所得的可達(dá)代價(jià)相融合，更新導(dǎo)航風(fēng)險(xiǎn)概率地圖。
[0028]其中所述的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器包括全局規(guī)劃器和局部反應(yīng)式壁障控制器兩個(gè)子模塊:
[0029]I)全局規(guī)劃器在導(dǎo)航風(fēng)險(xiǎn)概率地圖上進(jìn)行全局路徑規(guī)劃，得到全局最優(yōu)的路徑點(diǎn)及每個(gè)點(diǎn)上的運(yùn)動(dòng)方向。
[0030]2)局部反應(yīng)式避障控制器確保機(jī)器人在線運(yùn)動(dòng)時(shí)，以當(dāng)前點(diǎn)的目標(biāo)朝向?yàn)閰⒖?，根?jù)以0.1cm為分辨率的動(dòng)態(tài)窗口中激光傳感器實(shí)際獲取的障礙物情況，計(jì)算避障方向并求出平滑變化的控制量，包括平移速度u和旋轉(zhuǎn)速度ω。
[0031]采用運(yùn)動(dòng)規(guī)劃-避障控制層次化結(jié)構(gòu)的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器，實(shí)現(xiàn)機(jī)器人在與人共處的復(fù)雜動(dòng)態(tài)環(huán)境下安全高效的導(dǎo)航行為。
[0032]本發(fā)明采用上述技術(shù)方案，具有以下有益效果:
[0033]1、當(dāng)機(jī)器人與人位于兩個(gè)相對(duì)獨(dú)立的空間下、使用機(jī)器人車載傳感器無法獲知人的位置時(shí)，利用分布式感知進(jìn)行人的定位與跟蹤，并對(duì)人的全局運(yùn)動(dòng)進(jìn)行預(yù)測(cè)，從而確保機(jī)器人對(duì)人的運(yùn)動(dòng)具有一定預(yù)見性。
[0034]2、建立了一種融合多約束的導(dǎo)航方法，其中對(duì)人動(dòng)態(tài)避讓利用了特定環(huán)境下人的運(yùn)動(dòng)路徑具有一定規(guī)律的特性，預(yù)測(cè)避讓的效果相比傳統(tǒng)地機(jī)械式避障方法，能在確保機(jī)器人自主導(dǎo)航效率的基礎(chǔ)上，提高機(jī)器人導(dǎo)航行為對(duì)人的安全性、預(yù)見性和友好性，有助于解決該類服務(wù)機(jī)器人未來進(jìn)入家庭后的人性化作業(yè)問題。
【專利附圖】

【附圖說明】
[0035]圖1為本發(fā)明實(shí)施實(shí)例的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航流程；
[0036]圖2為本發(fā)明實(shí)施實(shí)例的分布式傳感器下人的定位與跟蹤，采用包括全局?jǐn)z像機(jī)和機(jī)器人車載激光傳感器組成的分布式傳感器網(wǎng)絡(luò)；
[0037]圖3為本發(fā)明實(shí)施實(shí)例的人的運(yùn)動(dòng)軌跡模式學(xué)習(xí)，Ca)為人的運(yùn)動(dòng)軌跡樣本，(b)為訓(xùn)練得到的幾種典型軌跡模式；
[0038]圖4為本發(fā)明實(shí)施實(shí)例的人的短期運(yùn)動(dòng)方向(a)與速度預(yù)測(cè)(b)示意圖；
[0039]圖5是本發(fā)明實(shí)施實(shí)例中人的運(yùn)動(dòng)模式預(yù)測(cè)，(a) (b) (C)對(duì)應(yīng)了人運(yùn)動(dòng)過程中的三個(gè)位置情況，上半圖是攝像機(jī)對(duì)人的跟蹤，下半圖是柵格地圖中人的位置及運(yùn)動(dòng)趨勢(shì)預(yù)測(cè)，其中字母表示地點(diǎn)路標(biāo)，軌跡旁的數(shù)字表示人沿著該路徑模式的概率；
[0040]圖6是本發(fā)明實(shí)施實(shí)例中，障礙物及行人四周膨脹安全區(qū)域；
[0041]圖7為本發(fā)明實(shí)施實(shí)例中，室內(nèi)環(huán)境下的可達(dá)受遮擋區(qū)域；
[0042]圖8為本發(fā)明實(shí)施實(shí)例中，以走廊環(huán)境為例，說明右向行駛規(guī)則相應(yīng)的代價(jià)柵格；
[0043]圖9為本發(fā)明實(shí)施實(shí)例中，導(dǎo)航風(fēng)險(xiǎn)概率地圖的更新與導(dǎo)航控制器結(jié)構(gòu)；
[0044]圖10在一個(gè)較為狹窄、含多個(gè)通道的實(shí)例房間環(huán)境中，本發(fā)明提出的導(dǎo)航結(jié)果與傳統(tǒng)的重規(guī)劃式避障導(dǎo)航控制方式結(jié)果對(duì)比。
[0045]圖11是機(jī)器人靠近人的導(dǎo)航行為實(shí)例，(a)場(chǎng)景示意；(b)三條路徑的選擇?！揪唧w實(shí)施方式】
[0046]下面結(jié)合具體實(shí)施例，進(jìn)一步闡明本發(fā)明，應(yīng)理解這些實(shí)施例僅用于說明本發(fā)明而不用于限制本發(fā)明的范圍，在閱讀了本發(fā)明之后，本領(lǐng)域技術(shù)人員對(duì)本發(fā)明的各種等價(jià)形式的修改均落于本申請(qǐng)所附權(quán)利要求所限定的范圍。
[0047]本發(fā)明實(shí)施實(shí)例的方法步驟是:
[0048]利用室內(nèi)環(huán)境下多臺(tái)全局?jǐn)z像機(jī)與機(jī)器人車載激光傳感器組成的分布式傳感器網(wǎng)絡(luò)，實(shí)現(xiàn)全局地圖中動(dòng)態(tài)人的位置跟蹤；
[0049]通過采集人的長期運(yùn)動(dòng)軌跡學(xué)習(xí)特定室內(nèi)環(huán)境下人的運(yùn)動(dòng)模式；而機(jī)器人在導(dǎo)航過程中，通過對(duì)人運(yùn)動(dòng)的長期運(yùn)動(dòng)規(guī)律預(yù)測(cè)和短期運(yùn)動(dòng)速度、方向預(yù)測(cè)相結(jié)合，對(duì)人的時(shí)空運(yùn)動(dòng)趨勢(shì)進(jìn)行估計(jì)；
[0050]根據(jù)人的當(dāng)前位置和預(yù)測(cè)位置，將包含對(duì)人動(dòng)態(tài)避讓在內(nèi)的多種機(jī)器人導(dǎo)航規(guī)則轉(zhuǎn)換為導(dǎo)航約束條件，與環(huán)境靜態(tài)障礙柵格地圖相融合，生成導(dǎo)航風(fēng)險(xiǎn)概率地圖；
[0051]采用全局路徑規(guī)劃-局部避障控制層次化結(jié)構(gòu)的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器，實(shí)現(xiàn)機(jī)器人導(dǎo)航行為控制，并最終確保機(jī)器人在與人共處的復(fù)雜動(dòng)態(tài)環(huán)境下安全高效的導(dǎo)航行為。
[0052]具體實(shí)現(xiàn)方式為:
[0053]如圖1所示為本實(shí)施例的機(jī)器人預(yù)測(cè)導(dǎo)航流程步驟?？傮w分為訓(xùn)練和運(yùn)行兩個(gè)階段:在訓(xùn)練階段，首先系統(tǒng)利用分布式攝像機(jī)與機(jī)器人車載激光組成的分布式傳感器網(wǎng)絡(luò)進(jìn)行人的定位和跟蹤(Si)，將定位結(jié)果保存在機(jī)器人柵格地圖坐標(biāo)系下，同時(shí)采集并保存人的位置數(shù)據(jù)；然后判斷是否采集了足夠的樣本并完成了訓(xùn)練(S2);如果未完成訓(xùn)練，則進(jìn)行人的運(yùn)動(dòng)軌跡模式樣本訓(xùn)練(S3)，訓(xùn)練后返回SI繼續(xù)執(zhí)行，否則進(jìn)入運(yùn)行階段。在運(yùn)行階段，首先對(duì)人的長期運(yùn)動(dòng)模式和短期運(yùn)動(dòng)進(jìn)行預(yù)測(cè)(S4)，然后進(jìn)行融合多約束的導(dǎo)航風(fēng)險(xiǎn)概率地圖更新(S5);接著通過層次化的導(dǎo)航運(yùn)動(dòng)控制器(S6)，向機(jī)器人底輪輸出運(yùn)動(dòng)控制量(S7)，機(jī)器人利 用車載定位系統(tǒng)判斷自身位置，直到其到達(dá)目的地(S8)。
[0054]人機(jī)共處運(yùn)動(dòng)情況下，人的未知?jiǎng)討B(tài)性是機(jī)器人自定位系統(tǒng)的主要干擾，恢復(fù)人的位置信息有利于提高機(jī)器人定位的準(zhǔn)確性；而機(jī)器人車載激光測(cè)距器對(duì)人的檢測(cè)又極易受到混雜背景中的干擾。為此，在實(shí)施例子中首先采用同時(shí)定位與人的跟蹤(SLAP)算法獲取人的位置信息，從而為人的運(yùn)動(dòng)估計(jì)與預(yù)測(cè)等后續(xù)工作提供依據(jù)。在實(shí)施例子中，采用如圖2所示的全局?jǐn)z像機(jī)和機(jī)器人激光傳感器組成的分布式傳感器網(wǎng)絡(luò)，將各個(gè)傳感器數(shù)據(jù)通過局域網(wǎng)傳送到中心服務(wù)器，在中心服務(wù)器上利用融合分布式多源感知的Rao-Blackwellized粒子濾波算法(RBPF)進(jìn)行同時(shí)定位與人的跟蹤，即同時(shí)更新人_機(jī)狀態(tài)的后驗(yàn)分布估計(jì)，獲得機(jī)器人全局位姿rk= (xr, yr, Θ)及運(yùn)動(dòng)速度(\，~)、人的全局位置hk = (xh, yh, Θ h)及運(yùn)動(dòng)速度(vh, wh)。其中Xr和yr是機(jī)器人在全局x-y地圖中的位置坐標(biāo)，是機(jī)器人姿態(tài)角，Vr和Wr分別是機(jī)器人運(yùn)動(dòng)線速度和角速度。Xh和Yh是人在全局X-y地圖中的位置坐標(biāo)，Gh是人的即時(shí)運(yùn)動(dòng)方向與地圖坐標(biāo)系X軸正方向的夾角。而下標(biāo)k表示第k個(gè)時(shí)刻。
[0055]在實(shí)施例子中，機(jī)器人車載的測(cè)距激光傳感器獲得的數(shù)據(jù)是激光在測(cè)距高度35cm平面上掃描得到的環(huán)境中障礙物上各個(gè)點(diǎn)相對(duì)于移動(dòng)機(jī)器人的距離和角度，在O。~180°范圍內(nèi)每1°分辨率獲得一個(gè)激光束數(shù)據(jù)，共計(jì)181個(gè)激光束。環(huán)境中的分布式全局?jǐn)z像機(jī)為CXD攝像頭，采集RGB圖像分辨率320*240 ;在實(shí)時(shí)例子中，在約80平米的室內(nèi)環(huán)境下共布置了 5臺(tái)不同視角的全局?jǐn)z像機(jī)。
[0056]人的運(yùn)動(dòng)軌跡模式分析與運(yùn)動(dòng)預(yù)測(cè)的實(shí)施步驟如下:
[0057]I)首先在人的定位與跟蹤基礎(chǔ)上進(jìn)行人的運(yùn)動(dòng)軌跡模式訓(xùn)練和預(yù)測(cè)。利用長期采集的行人位置數(shù)據(jù)，采用EM算法學(xué)習(xí)其運(yùn)動(dòng)軌跡模式(軌跡)共M種，在實(shí)施實(shí)例環(huán)境中軌跡模式的類型數(shù)目M—般滿足4 SMS 20。這M種運(yùn)動(dòng)軌跡模式的集合記為ΨΗΨρ...，ΨΜ}。每種運(yùn)動(dòng)軌跡模式Wm在x-y平面中用Lm個(gè)分量的高斯分布鏈匕,=(之,4)1來描述，式是第I個(gè)混合分量，4是混合系數(shù)。圖3 (a)為在某實(shí)驗(yàn)環(huán)境下
采集到的行人運(yùn)動(dòng)軌跡實(shí)例。經(jīng)過模型訓(xùn)練，共得到{匕}|1,共M種模型，PpattonW1Jhk)為
依據(jù)人的運(yùn)動(dòng)歷史觀測(cè)估計(jì)其當(dāng)前運(yùn)動(dòng)遵循某一類模式Ψπ的概率。圖3 (b)是以這些歷史軌跡數(shù)據(jù)為樣本訓(xùn)練得到軌跡模式(僅顯示了其中四種)，每個(gè)軌跡模型用一系列高斯分布表示，圖中的橢圓即代表x-y平面中的一個(gè)高斯分布?？梢娫谠撌覂?nèi)環(huán)境下，人的長期運(yùn)動(dòng)規(guī)律與室內(nèi)場(chǎng)所語義上下文有一定聯(lián)系。人的運(yùn)動(dòng)軌跡呈現(xiàn)為重要地點(diǎn)(例如飲水機(jī)、桌子、沙發(fā)、打印機(jī)等)之間的聯(lián)通路線。
[0058]在實(shí)時(shí)對(duì)人運(yùn)動(dòng)位置進(jìn)行跟蹤的基礎(chǔ)上，依據(jù)人的運(yùn)動(dòng)歷史估計(jì)出當(dāng)前運(yùn)動(dòng)遵循軌跡模式的概率。假設(shè)人的平面位置hk，其屬于給定某個(gè)軌跡模式ψπ中第I個(gè)高斯分布的概率記為/KW,:,),按式(I)計(jì)算為:
【權(quán)利要求】
1.動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于，所述方法包括: 利用室內(nèi)環(huán)境下多臺(tái)全局?jǐn)z像機(jī)與機(jī)器人車載激光傳感器組成的分布式傳感器網(wǎng)絡(luò)，實(shí)現(xiàn)全局地圖中動(dòng)態(tài)人的定位與跟蹤； 通過采集人的長期運(yùn)動(dòng)軌跡學(xué)習(xí)上述室內(nèi)場(chǎng)所環(huán)境下人的運(yùn)動(dòng)模式；而機(jī)器人在導(dǎo)航過程中，通過對(duì)人運(yùn)動(dòng)的長期運(yùn)動(dòng)規(guī)律預(yù)測(cè)和短期運(yùn)動(dòng)速度、方向預(yù)測(cè)相結(jié)合，對(duì)人的時(shí)空運(yùn)動(dòng)趨勢(shì)進(jìn)行估計(jì)； 根據(jù)人的當(dāng)前位置和上述估計(jì)得到的預(yù)測(cè)位置，與環(huán)境靜態(tài)障礙柵格地圖相融合，生成導(dǎo)航風(fēng)險(xiǎn)概率地圖； 采用全局路徑規(guī)劃與局部避障控制層次化結(jié)構(gòu)的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器，實(shí)現(xiàn)機(jī)器人導(dǎo)航行為控制。
2.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:所述的人的定位與跟蹤，是在室內(nèi)環(huán)境下采用多臺(tái)全局?jǐn)z像機(jī)與機(jī)器人車載的單臺(tái)激光掃描器實(shí)現(xiàn)人的位置跟蹤，通過標(biāo)定全局?jǐn)z像機(jī)與全局柵格地圖的坐標(biāo)轉(zhuǎn)換關(guān)系，獲取柵格地圖中人的全局位姿及運(yùn)動(dòng)速度。
3.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:所述的對(duì)人的時(shí)空運(yùn)動(dòng)趨勢(shì)進(jìn)行估計(jì)，步驟如下: 1)首先在人的定位與跟蹤基礎(chǔ)上進(jìn)行人的運(yùn)動(dòng)軌跡模式訓(xùn)練和預(yù)測(cè):長期采集的行人位置數(shù)據(jù)，采用EM算法學(xué)習(xí)其運(yùn)動(dòng)模式類型，對(duì)每種類型的運(yùn)動(dòng)模式在x-y平面中用混合高斯分布加以建模，在實(shí)時(shí)對(duì)人運(yùn)動(dòng)位置進(jìn)行跟蹤的基礎(chǔ)上，依據(jù)人的運(yùn)動(dòng)歷史估計(jì)出當(dāng)前運(yùn)動(dòng)遵循軌跡模式的概率； 2)其次進(jìn)行人的短期運(yùn)動(dòng)預(yù)測(cè):`假設(shè)人的運(yùn)動(dòng)速度和方向角每過一定時(shí)刻速度和方向角變化一次，且在一定范圍內(nèi)變化，各個(gè)時(shí)刻上的速度和方向角分別為相應(yīng)范圍內(nèi)的隨機(jī)變量且相互獨(dú)立，對(duì)人沿著該預(yù)測(cè)路徑前進(jìn)的短期運(yùn)動(dòng)速度和方向角進(jìn)行預(yù)測(cè)。
4.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括對(duì)運(yùn)動(dòng)人預(yù)測(cè)避讓可達(dá)代價(jià)的導(dǎo)航約束條件，即當(dāng)人機(jī)運(yùn)動(dòng)可能在狹窄通道內(nèi)出現(xiàn)沖突時(shí)，通過預(yù)測(cè)人的運(yùn)動(dòng)位置來估計(jì)人的時(shí)空可達(dá)代價(jià)，并以降低人機(jī)運(yùn)動(dòng)沖突或阻塞風(fēng)險(xiǎn)為目標(biāo)，控制機(jī)器人提前繞行或減速避讓。
5.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括帶裕量的機(jī)器人四周安全范圍可達(dá)代價(jià)的導(dǎo)航約束條件，即在機(jī)器人坐標(biāo)位置的四周，用高斯分布的點(diǎn)云表示該點(diǎn)處障礙對(duì)機(jī)器人安全威脅概率。
6.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括受建筑物遮擋或警戒區(qū)域繞行可達(dá)代價(jià)的導(dǎo)航約束條件，即控制機(jī)器人緩慢地繞行走出建筑物遮擋范圍。
7.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:生成所述導(dǎo)航風(fēng)險(xiǎn)概率地圖還包括遵守行駛靠向習(xí)慣可達(dá)代價(jià)的導(dǎo)航約束條件，即將靠向準(zhǔn)則引入機(jī)器人運(yùn)動(dòng)規(guī)劃，確保機(jī)器人在與人相遇時(shí)雙方遵守相同的靠向規(guī)則，達(dá)到最優(yōu)的導(dǎo)航效率。
8.根據(jù)權(quán)利要求1所述的動(dòng)態(tài)環(huán)境下服務(wù)動(dòng)機(jī)器人導(dǎo)航方法，其特征在于:所述的機(jī)器人導(dǎo)航運(yùn)動(dòng)控制器包括全局規(guī)劃器和局部反應(yīng)式壁障控制器兩個(gè)子模塊:.1)全局規(guī)劃器在所述導(dǎo)航風(fēng)險(xiǎn)概率地圖上，進(jìn)行全局路徑規(guī)劃，得到全局最優(yōu)的路徑點(diǎn)及每個(gè)點(diǎn)上的運(yùn)動(dòng)方向； .2)局部反應(yīng)式避障控制器確保機(jī)器人在線運(yùn)動(dòng)時(shí)，以當(dāng)前點(diǎn)的目標(biāo)朝向?yàn)閰⒖?，根?jù)以0.1cm為分辨率的動(dòng)態(tài)窗口中激光傳感器實(shí)際獲取的障礙物情況，計(jì)算避障方向并求出平滑變化的控制量，包括平移速度u和旋轉(zhuǎn)速度ω。
【文檔編號(hào)】G05D1/02GK103558856SQ201310591604
【公開日】2014年2月5日 申請(qǐng)日期:2013年11月21日 優(yōu)先權(quán)日:2013年11月21日
【發(fā)明者】錢堃, 符張杰, 陳之杰, 楊鴻
申請(qǐng)人:東南大學(xué)