本發(fā)明屬于移動(dòng)機(jī)器人自主導(dǎo)航,尤其涉及一種基于自適應(yīng)kmeans算法的機(jī)器人未知環(huán)境探索方法。
背景技術(shù):
1、近年來,隨著硬件設(shè)備的不斷完善,移動(dòng)機(jī)器人廣泛應(yīng)用于工業(yè)、醫(yī)療、農(nóng)業(yè)、服務(wù)業(yè)等各領(lǐng)域。自主探索與導(dǎo)航是移動(dòng)機(jī)器人研究領(lǐng)域中的一個(gè)重要分支,它涉及到機(jī)器人在未知環(huán)境中的探索。機(jī)器人未知環(huán)境探索旨在引導(dǎo)機(jī)器人進(jìn)入未知空間,目前在機(jī)器人探索領(lǐng)域的研究成果中,基于rrt(rapidly?exploring?random?tree,快速探索隨機(jī)樹)的邊界探索算法是最為廣泛的應(yīng)用方法。該探索策略由三個(gè)模塊組成:邊界檢測(cè)器模塊、濾波模塊、機(jī)器人任務(wù)分配器模塊。邊界檢測(cè)器模塊使用rrt算法檢測(cè)邊界點(diǎn),并將檢測(cè)到的大量邊界點(diǎn)傳遞給過濾模塊。濾波模塊對(duì)邊界點(diǎn)進(jìn)行聚類,使用mean?shift聚類算法,過濾模塊負(fù)責(zé)刪除無效和過時(shí)的邊界點(diǎn)。最后,任務(wù)分配模塊通過信息增量計(jì)算選擇最優(yōu)的邊界點(diǎn),將其分配給移動(dòng)機(jī)器人進(jìn)行探索。
2、基于rrt算法的邊界檢測(cè)器模塊通過構(gòu)建探索樹來識(shí)別邊界點(diǎn)。邊界點(diǎn)定義為:如果一個(gè)點(diǎn)位于地圖的未知區(qū)域,并且由rrt樹生長過程中到達(dá),則該點(diǎn)被認(rèn)為是邊界點(diǎn)。這樣的定義有助于機(jī)器人更有效地探索未知環(huán)境并識(shí)別其邊界。檢測(cè)到邊界點(diǎn)會(huì)被過濾模塊處理,過濾模塊中會(huì)使用到聚類算法,傳統(tǒng)的探索中采用mean?shift聚類算法。mean?shift算法是一種基于密度的非參數(shù)聚類算法,其核心思想在于假設(shè)不同簇的數(shù)據(jù)集具有不同的概率密度分布,找到任一樣本點(diǎn)密度增大的最快方向,樣本密度高的區(qū)域?qū)?yīng)分布的最大值,這些樣本點(diǎn)最終會(huì)收斂到局部密度的最大值,且收斂到相同局部最大值的點(diǎn)被認(rèn)為是同一簇類的成員,mean?shift算法能夠處理任意形狀的聚類問題,廣泛應(yīng)用于圖像處理和分析以及計(jì)算機(jī)視覺技術(shù),如圖像去噪、圖像分割、運(yùn)動(dòng)跟蹤等。mean?shift算法計(jì)算量不大,缺乏動(dòng)態(tài)更新機(jī)制,由于固定帶寬直徑,當(dāng)目標(biāo)大小發(fā)生變化時(shí),聚類效果會(huì)不夠完整,以及目標(biāo)速度變快,聚類效果較差。
技術(shù)實(shí)現(xiàn)思路
1、有鑒于此,本發(fā)明提供了一種基于自適應(yīng)kmeans算法的機(jī)器人未知環(huán)境探索方法,在移動(dòng)機(jī)器人未知環(huán)境探索中發(fā)現(xiàn),可以考慮將原本的mean?shift算法替換為kmeans算法,kmeans算法是一種常用的聚類算法,用于將數(shù)據(jù)點(diǎn)劃分成k個(gè)簇。kmeans算法的基本思想是通過不斷迭代的方式,將簇內(nèi)所有數(shù)據(jù)點(diǎn)的均值更新為質(zhì)心,并將簇中所有數(shù)據(jù)點(diǎn)的均值作為新的質(zhì)心,直到達(dá)到收斂條件,kmeans算法具有簡(jiǎn)單易懂、計(jì)算效率高和易于實(shí)現(xiàn)等優(yōu)點(diǎn),具體采用以下技術(shù)方案來實(shí)現(xiàn)。
2、本發(fā)明提供了一種基于自適應(yīng)kmeans算法的機(jī)器人未知環(huán)境探索方法,包括以下步驟:
3、獲取機(jī)器人在未知環(huán)境中移動(dòng)的未知區(qū)域,并基于rrt算法對(duì)所述未知區(qū)域進(jìn)行識(shí)別得到邊界點(diǎn);
4、采用kmeans算法對(duì)所述邊界點(diǎn)進(jìn)行聚類處理,并基于信息增量算法對(duì)聚類處理后的邊界點(diǎn)進(jìn)行計(jì)算以確定目標(biāo)點(diǎn);
5、基于teb路徑規(guī)劃算法引導(dǎo)所述機(jī)器人到達(dá)所述目標(biāo)點(diǎn),并采用gmapping算法進(jìn)行地圖構(gòu)建以完成機(jī)器人未知環(huán)境探索。
6、作為上述技術(shù)方案的優(yōu)選,基于rrt算法對(duì)所述未知區(qū)域進(jìn)行識(shí)別得到邊界點(diǎn),包括:
7、應(yīng)用rrt算法進(jìn)行未知環(huán)境探索時(shí),將機(jī)器人的當(dāng)前位置設(shè)定為探索樹的根節(jié)點(diǎn),rrt算法在探索樹中生成隨機(jī)點(diǎn),并確定與隨機(jī)點(diǎn)最近的臨近點(diǎn);
8、設(shè)定步長為,并計(jì)算出隨機(jī)點(diǎn)和臨近點(diǎn)之間的斜率m:
9、?(1)
10、式中,表示隨機(jī)點(diǎn)的y坐標(biāo),表示臨近點(diǎn)的y坐標(biāo),表示隨機(jī)點(diǎn)的x坐標(biāo),表示隨機(jī)點(diǎn)的x坐標(biāo);
11、當(dāng)隨機(jī)點(diǎn)與臨近點(diǎn)不重合時(shí),根據(jù)所述步長、隨機(jī)點(diǎn)與臨近點(diǎn)的位置、斜率m確定新節(jié)點(diǎn)的坐標(biāo)位置,其中,沿著從臨近點(diǎn)到隨機(jī)點(diǎn)的方向以步長參數(shù)為距離確定新節(jié)點(diǎn)的坐標(biāo)位置,對(duì)應(yīng)的表達(dá)式為:
12、?(2)
13、?(3)
14、式中,s用于確定兩點(diǎn)之間的關(guān)系,若大于,s結(jié)果為正;若小于,s結(jié)果為負(fù);若等于,s結(jié)果為零,為新節(jié)點(diǎn)的x坐標(biāo),為新節(jié)點(diǎn)的y坐標(biāo);
15、當(dāng)隨機(jī)點(diǎn)與臨近點(diǎn)重合時(shí),將重新計(jì)算新節(jié)點(diǎn)的位置,對(duì)應(yīng)的表達(dá)式為:
16、?(4)
17、 (5)
18、式中,和為隨機(jī)點(diǎn)與臨近點(diǎn)重合時(shí)新節(jié)點(diǎn)的坐標(biāo)。
19、作為上述技術(shù)方案的優(yōu)選,通過計(jì)算兩點(diǎn)之間的歐幾里得距離和動(dòng)態(tài)調(diào)整步長以適應(yīng)不同區(qū)域的探索需求,預(yù)設(shè)已知向量和向量并計(jì)算歐幾里得距離的表達(dá)式為:
20、?(6)
21、根據(jù)得到的新節(jié)點(diǎn)判斷新節(jié)點(diǎn)和緊鄰點(diǎn)之間的連線是否穿過障礙物或未探索區(qū)域,以評(píng)估新節(jié)點(diǎn)對(duì)應(yīng)的路徑;
22、若檢測(cè)到新節(jié)點(diǎn)與緊鄰點(diǎn)之間的連線穿過未知區(qū)域,則將新節(jié)點(diǎn)標(biāo)記為邊界點(diǎn);
23、若否,則rrt算法將允許探索樹的生長,并將生長的路徑記錄在所述探索樹中。
24、作為上述技術(shù)方案的優(yōu)選,對(duì)探索到的邊界點(diǎn)進(jìn)行管理,包括:
25、清除舊邊界點(diǎn):在每次探索前,清除上一次采樣中已經(jīng)被探索的邊界點(diǎn);
26、聚類中心點(diǎn)評(píng)估:采用kmeans聚類算法獲取邊界點(diǎn)的聚類中心點(diǎn),并判斷所述聚類中心點(diǎn)是否已經(jīng)被探索;
27、若是,將所述聚類中心點(diǎn)從所述聚類中心點(diǎn)對(duì)應(yīng)的聚類中心點(diǎn)集合移除;
28、其中,對(duì)所有探索到的邊界點(diǎn)進(jìn)行kmeans聚類算法處理,kmeans聚類算法處理包括:選擇k個(gè)質(zhì)心并初始化質(zhì)心,計(jì)算每個(gè)邊界點(diǎn)與所有質(zhì)心之間的距離,并將每個(gè)邊界點(diǎn)歸入最近的質(zhì)心所代表的簇;對(duì)每個(gè)形成的簇重新計(jì)算質(zhì)心,即取簇內(nèi)所有樣本點(diǎn)的均值作為新的質(zhì)心位置;
29、簇內(nèi)樣本點(diǎn)的相似度采用所有邊界點(diǎn)到質(zhì)心的距離之和進(jìn)行評(píng)估,距離之和越小,表明簇內(nèi)樣本點(diǎn)的相似度越高,差異越?。?/p>
30、預(yù)設(shè)是第k個(gè)聚類上的第i個(gè)點(diǎn),是第k個(gè)簇的質(zhì)心,表示樣本點(diǎn)到質(zhì)心的距離,對(duì)應(yīng)的距離度量表達(dá)式為:
31、?(7)
32、式中,m為聚類個(gè)數(shù),k為聚類的類別數(shù);
33、根據(jù)歐幾里得距離將一個(gè)簇內(nèi)所有點(diǎn)到質(zhì)心的距離平方和的表達(dá)式為:
34、?(8)
35、式中,n為第k個(gè)簇內(nèi)數(shù)據(jù)點(diǎn)的數(shù)量;
36、kmeans算法基于最小平方差誤差原則用于在最小化每個(gè)簇內(nèi)所有點(diǎn)到質(zhì)心的距離平方和,對(duì)應(yīng)的平方差誤差可表示為:
37、?(9)
38、式中,是第i個(gè)簇,是第k個(gè)簇的質(zhì)心,是簇中的第i個(gè)數(shù)據(jù)點(diǎn),d表示歐式距離,表示簇中所有點(diǎn)的均值:
39、??(10)
40、式中,是k簇內(nèi)點(diǎn)的數(shù)量,通過計(jì)算每個(gè)簇內(nèi)所有點(diǎn)到質(zhì)心的距離平方和,kmeans算法不斷迭代,更新質(zhì)心的位置,以最小化整個(gè)數(shù)據(jù)集的總平方差誤差。
41、作為上述技術(shù)方案的優(yōu)選,采用kmeans算法對(duì)所述邊界點(diǎn)進(jìn)行聚類處理,包括:
42、獲取邊界點(diǎn)的聚類中心點(diǎn),在代價(jià)地圖上評(píng)估所述聚類中心點(diǎn)并進(jìn)行目標(biāo)點(diǎn)分配,其中,根據(jù)預(yù)設(shè)評(píng)價(jià)標(biāo)準(zhǔn)為邊界點(diǎn)設(shè)置優(yōu)先級(jí),并計(jì)算每個(gè)邊界點(diǎn)的信息熵以評(píng)估其探索價(jià)值,計(jì)算標(biāo)準(zhǔn)包括根據(jù)計(jì)算出的信息熵選擇合適的導(dǎo)航目標(biāo)點(diǎn)并安排機(jī)器人對(duì)位置區(qū)域進(jìn)行探索;
43、根據(jù)信息熵確定目標(biāo)導(dǎo)航的導(dǎo)航目標(biāo)點(diǎn),信息熵是信息量的期望值以描述不確定性:
44、?(11)
45、式中,s為樣本空間,是i發(fā)生的概率;
46、通過對(duì)邊界點(diǎn)的收益公式選擇導(dǎo)航目標(biāo)點(diǎn):
47、 ?(12)
48、式中,為給定的前沿點(diǎn),r表示前沿點(diǎn)的收益, i表示前沿點(diǎn)的信息增量,為機(jī)器人的當(dāng)前所在位置,是用戶給定的參數(shù),n為機(jī)器人導(dǎo)航到前沿點(diǎn)的導(dǎo)航成本,為滯后增益用于確定給定的前沿點(diǎn)是否在距離機(jī)器人的當(dāng)前位置一定半徑之外,若是,的值等于1;反之,的值等于,其中,大于1;通過設(shè)置大于1,機(jī)器人更傾向于探索其附近的前沿點(diǎn),對(duì)應(yīng)的表達(dá)式為:
49、??(13)
50、式中,是以當(dāng)前機(jī)器人為半徑的值。
51、作為上述技術(shù)方案的優(yōu)選,基于teb路徑規(guī)劃算法引導(dǎo)所述機(jī)器人到達(dá)所述目標(biāo)點(diǎn),還包括:
52、根據(jù)前沿點(diǎn)確定最優(yōu)的目標(biāo)點(diǎn),將關(guān)鍵目標(biāo)點(diǎn)作為teb算法在局部路徑規(guī)劃中的導(dǎo)航目標(biāo)點(diǎn),對(duì)應(yīng)的機(jī)器人位姿表達(dá)式為:
53、?(14)
54、式中,、和為機(jī)器人map坐標(biāo)系中的位置和姿態(tài),為時(shí)間維度和控制參數(shù)維度上的狀態(tài)空間,表示坐標(biāo)系中的位置都為正實(shí)數(shù),表示所有可能得狀態(tài)情況,表示笛卡爾積;
55、機(jī)器人在map坐標(biāo)系中的位置和姿態(tài)作為構(gòu)型,在空間中構(gòu)型可排序?yàn)椋?/p>
56、?(15)
57、其中,兩個(gè)構(gòu)型之間存在一個(gè)時(shí)間間隔,時(shí)間間隔定義為,表示機(jī)器人的兩個(gè)構(gòu)型需要的時(shí)間,記錄時(shí)間的序列的表達(dá)式為:
58、?(16)
59、將構(gòu)型和時(shí)間序列合并表示為:
60、?(17)
61、通過加權(quán)多目標(biāo)優(yōu)化獲取最優(yōu)的路徑點(diǎn),即最優(yōu)的q,對(duì)應(yīng)的表達(dá)式為:
62、?(18)
63、?(19)
64、式中,b為arrangement排序的一個(gè)子集,用于評(píng)估和優(yōu)化得到最優(yōu)解;為最優(yōu)結(jié)果;為考慮的各種約束的目標(biāo)函數(shù),為各個(gè)目標(biāo)函數(shù)的分量的加權(quán)和;為目標(biāo)函數(shù)數(shù)量。
65、作為上述技術(shù)方案的優(yōu)選,teb算法的第一個(gè)約束目標(biāo)函數(shù)為跟隨路徑和避障約束,避障約束包括跟隨已知的全局規(guī)劃路和避障,構(gòu)型序列與全局路徑點(diǎn)序列或障礙物的最近距離為,跟隨路徑目標(biāo)以構(gòu)型距全局路徑的允許最大距離作為約束,避障目標(biāo)以構(gòu)型距障礙物的允許最小距離作為約束;
66、根據(jù)路徑約束函數(shù)和障礙約束函數(shù)以懲罰函數(shù),對(duì)應(yīng)的表達(dá)式為:
67、?(20)
68、式中,表示限界值旁的一個(gè)小位移,s表示縮放,n表示多項(xiàng)式階數(shù)。
69、作為上述技術(shù)方案的優(yōu)選,teb算法的第二個(gè)約束目標(biāo)函數(shù)為速度和加速度約束,機(jī)器人運(yùn)動(dòng)的平均線速度和角速度可通過相鄰的、和時(shí)間間隔計(jì)算得到:
70、??(21)
71、??(22)
72、式中,和為兩點(diǎn)之間的距離,為兩點(diǎn)之間的旋轉(zhuǎn)角度;
73、機(jī)器人的平均線加速度以及角速度的計(jì)算表達(dá)式為:
74、?(23)
75、線速度的約束可用懲罰函數(shù)表示為:
76、?(24)
77、結(jié)合移動(dòng)機(jī)器人得到的輪子速度和加速度的上下界確定約束的上下界,其中,表示線速度,表示最大線速度,表示限界值旁的一個(gè)小位移,s表示縮放,n表示多項(xiàng)式階數(shù);
78、機(jī)器人底盤的運(yùn)動(dòng)狀態(tài)由三個(gè)關(guān)鍵變量定義:沿x軸和y軸的線性移動(dòng)和繞軸的旋轉(zhuǎn),指的是繞z軸的旋轉(zhuǎn);將機(jī)器人底盤運(yùn)動(dòng)分解為三個(gè)獨(dú)立變量以機(jī)器人四個(gè)輪子為幾何中心:表示x軸平動(dòng),表示x軸平動(dòng),表示軸的自轉(zhuǎn)速度,得出輪子的軸心速度為:
79、?(25)
80、式中,為幾何中心指向輪子軸心的矢量,為輪子軸心的運(yùn)動(dòng)速度矢量,為輪子軸心沿垂直的方向的速度矢量,且計(jì)算出輪子軸心運(yùn)動(dòng)速度矢量的x、y軸的速度分量為:
81、?(26)
82、?(27)
83、根據(jù)輪子軸心的速度可求出輥?zhàn)拥乃俣?,分解為沿著輥?zhàn)臃较虻乃俣群痛怪庇谳佔(zhàn)臃较虻乃俣?,其中,垂直方向可以忽略,?duì)應(yīng)的表達(dá)式為:
84、?(28)
85、其中,為沿輥?zhàn)臃较虻膯挝皇噶浚瑥亩?jì)算輪子的速度v:
86、?(29)。
87、作為上述技術(shù)方案的優(yōu)選,teb算法的第三個(gè)約束目標(biāo)函數(shù)為運(yùn)動(dòng)學(xué)約束,機(jī)器人以若干弧段組成平滑的軌跡運(yùn)動(dòng),初始與運(yùn)動(dòng)方向、的夾角應(yīng)與結(jié)束構(gòu)型對(duì)應(yīng)的夾角相等,若為機(jī)器人在第i段弧段相對(duì)于世界坐標(biāo)系的絕對(duì)姿態(tài),則:
88、?(30)
89、其中,運(yùn)動(dòng)方向的位移向量表示為:
90、?(31)
91、式中,和分別表示路徑上的兩點(diǎn)坐標(biāo)的位移,相應(yīng)的目標(biāo)函數(shù):
92、?(32)
93、式中,和分別表示路徑上的兩點(diǎn)坐標(biāo),公式(32)通過兩向量相加,表示路徑上兩個(gè)點(diǎn)的方向;乘以位移向量、,表示路徑上兩點(diǎn)之間的距離和方向;為向量的平方范數(shù),表示路徑的代價(jià)函數(shù)。
94、作為上述技術(shù)方案的優(yōu)選,teb算法的第四個(gè)約束目標(biāo)函數(shù)為最快路徑約束,用最小化時(shí)間間隔序列的二次方表示:
95、?(33)
96、進(jìn)行teb路徑規(guī)劃時(shí),同步運(yùn)用gmapping算法來創(chuàng)建未知環(huán)境的地圖,算法通過將二維空間劃分成多個(gè)獨(dú)立的柵格單元來構(gòu)建柵格地圖,每個(gè)柵格單元都與兩個(gè)概率值相關(guān)聯(lián):表示柵格單元為可通行空間的概率,表示障礙物所占據(jù)的概率;兩個(gè)概率值相加總和為1,柵格但愿的狀態(tài)可通過兩個(gè)概率值的比率來確定,比率較高時(shí),表明柵格單元可能是障礙物;
97、當(dāng)?shù)貓D中的一個(gè)特定點(diǎn)接收到一個(gè)新的測(cè)量值z(mì)后,需要相應(yīng)地更新狀態(tài);預(yù)設(shè)在新測(cè)量值到來之前,特定點(diǎn)的狀態(tài)已知,將其更新為新的估計(jì)狀態(tài):
98、?(34)
99、式中,表示狀態(tài)s的對(duì)數(shù)幾率,表示在給定條件z下,狀態(tài)s為1的概率;表示在給定條件z下,狀態(tài)s為0的概率;
100、由貝葉斯公式計(jì)算可得:
101、 ?(35)
102、且對(duì)公式(35)兩邊進(jìn)行取對(duì)數(shù)計(jì)算:
103、?(36)
104、在沒有任何測(cè)量值的初始狀態(tài)下,一個(gè)點(diǎn)的初始狀態(tài)為0,在公式(34)中進(jìn)行計(jì)算,比值為測(cè)量值的模型;
105、實(shí)際上測(cè)量值的模型包括兩種:
106、?(37)
107、式中,兩種測(cè)量值都遵循固定模型,使得每次獲取新的測(cè)量值時(shí),可通過簡(jiǎn)單的算術(shù)加減操作來更新地圖上點(diǎn)的狀態(tài);
108、在構(gòu)建柵格地圖時(shí),機(jī)器人將真實(shí)世界的位置轉(zhuǎn)換為地圖上的柵格坐標(biāo),地圖中的每個(gè)柵格單元的長度表示為r,表示地圖的分辨率;對(duì)于二維柵格地圖,任何點(diǎn)的位置可用以對(duì)坐標(biāo)來確定,坐標(biāo)值表示相對(duì)于地圖原點(diǎn)的水平和垂直距離,其中,每個(gè)柵格位置坐標(biāo)通過表示:
109、?(38)
110、式中,x和y表示真實(shí)世界中的坐標(biāo),i和j為柵格地圖中的坐標(biāo),r為柵格一格的長度,表示分辨率,則和;
111、預(yù)設(shè)機(jī)器人的位姿為,可得到地圖構(gòu)建過程中障礙物的位置:
112、?(39)
113、?(40)
114、其中,d為測(cè)量得到的距離,為激光線與機(jī)器人位姿角的夾角,得到兩個(gè)坐標(biāo)后計(jì)算出兩點(diǎn)在柵格位置和,利用bresenham算法計(jì)算非障礙物格點(diǎn)的集合,更新柵格地圖;
115、預(yù)設(shè)直線上第i個(gè)像素點(diǎn)坐標(biāo),直線斜率為1且,直線上的下一個(gè)像素點(diǎn)的可能位置是或,在處,直線上點(diǎn)的y值是,直線上點(diǎn)離像素點(diǎn)和像素點(diǎn)的距離分別是和:
116、?(41)
117、?(42)
118、式中,b為直線方程的斜率;
119、和的距離差為:
120、?(43)
121、利用距離差可選擇下一個(gè)像素點(diǎn),當(dāng)距離差為正時(shí),,說明直線上理論點(diǎn)像素較近,下一個(gè)像素點(diǎn)?。划?dāng)距離差為負(fù)時(shí),,說明直線上理論點(diǎn)離像素較近,則下一個(gè)像素點(diǎn)取;當(dāng)距離差為零時(shí),取作為下一個(gè)像素點(diǎn)。
122、本發(fā)明提供了一種基于自適應(yīng)kmeans算法的機(jī)器人未知環(huán)境探索方法,通過獲取機(jī)器人在未知環(huán)境中移動(dòng)的未知區(qū)域,并基于rrt算法對(duì)所述未知區(qū)域進(jìn)行識(shí)別得到邊界點(diǎn),采用kmeans算法對(duì)所述邊界點(diǎn)進(jìn)行聚類處理,并基于信息增量算法對(duì)聚類處理后的邊界點(diǎn)進(jìn)行計(jì)算以確定目標(biāo)點(diǎn),基于teb路徑規(guī)劃算法引導(dǎo)所述機(jī)器人到達(dá)所述目標(biāo)點(diǎn),并采用gmapping算法進(jìn)行地圖構(gòu)建以完成機(jī)器人未知環(huán)境探索,可以根據(jù)地圖采集到的邊界點(diǎn)動(dòng)態(tài)調(diào)整聚類中心的數(shù)量,根據(jù)數(shù)據(jù)的特性和分布情況,智能地決定最佳的聚類中心數(shù)目,從而提高聚類的效果和精確度,基于kmeans聚類算法對(duì)邊界點(diǎn)進(jìn)行聚類,并通過信息增量算法對(duì)目標(biāo)點(diǎn)進(jìn)行優(yōu)化,以提高聚類的準(zhǔn)確性和效率。