人運(yùn)動(dòng)學(xué)特性，可以得到機(jī)器人到目標(biāo)點(diǎn)的距離偏差r為：
【主權(quán)項(xiàng)】
1.基于目標(biāo)跟蹤控制策略的單移動(dòng)機(jī)器人最優(yōu)巡回控制方法，包括地圖構(gòu)建、任務(wù)描 述、拓?fù)錁?gòu)建、路徑尋優(yōu)、任務(wù)分解、目標(biāo)跟蹤控制和位姿信息反饋7個(gè)步驟，其特征在于： 首先，對(duì)輸入的環(huán)境信息進(jìn)行建模，構(gòu)建加權(quán)切換系統(tǒng)模型，同時(shí)輸入描述任務(wù)需求的線性 時(shí)序任務(wù)公式，并進(jìn)行轉(zhuǎn)換，輸出任務(wù)圖表；其次，將加權(quán)切換系統(tǒng)模型和任務(wù)圖表進(jìn)行融 合，構(gòu)建任務(wù)可行網(wǎng)絡(luò)拓?fù)洌唤又?，采用基于Di jkstra算法的最優(yōu)路徑搜索算法在任務(wù)可 行網(wǎng)絡(luò)拓?fù)渖纤阉鞒鰸M足任務(wù)需求的最優(yōu)路徑，并將任務(wù)可行網(wǎng)絡(luò)拓?fù)渖系淖顑?yōu)路徑映射 回加權(quán)切換系統(tǒng)模型，獲得環(huán)境中滿足任務(wù)需求的最優(yōu)路徑；之后，將實(shí)際環(huán)境中對(duì)應(yīng)最優(yōu) 路徑上的離散坐標(biāo)點(diǎn)存儲(chǔ)在緩存中，并結(jié)合當(dāng)前任務(wù)完成情況，按序讀取緩存中的目標(biāo)點(diǎn)； 然后，采用目標(biāo)跟蹤控制策略，結(jié)合目標(biāo)點(diǎn)坐標(biāo)信息和實(shí)時(shí)反饋的機(jī)器人位置與朝向信息， 根據(jù)機(jī)器人與目標(biāo)點(diǎn)之間的距離和角度的偏差給出控制量，控制機(jī)器人完成最優(yōu)路徑跟 蹤；最后，實(shí)時(shí)采集機(jī)器人當(dāng)前運(yùn)行環(huán)境的圖像信息，通過色塊識(shí)別的方法，采用UDP通信 的方式，反饋回機(jī)器人當(dāng)前的位置坐標(biāo)和朝向信息，具體實(shí)現(xiàn)步驟如下： 步驟1，地圖構(gòu)建 將機(jī)器人當(dāng)前運(yùn)行環(huán)境構(gòu)建為加權(quán)切換系統(tǒng)模型；將移動(dòng)機(jī)器人所在的運(yùn)行環(huán)境劃分 為有限多個(gè)方塊區(qū)域，每個(gè)獨(dú)立的區(qū)域用字母Q表示，其中i e {1，2, 3，...}，Q是區(qū) 域的集合，Qtl表示機(jī)器人的初始位置，機(jī)器人在環(huán)境中的移動(dòng)對(duì)應(yīng)區(qū)域的變化，表示為一； 從一個(gè)區(qū)域到相鄰區(qū)域的移動(dòng)距離或是機(jī)器人移動(dòng)的成本（權(quán)值），表示為ω，移動(dòng)機(jī)器人 在特定區(qū)域所執(zhí)行的任務(wù)表不為 31 iε Π ，其中i e {1，2, 3,...}，Π 是所有任務(wù)的集合； 如此構(gòu)成一個(gè)轉(zhuǎn)換系統(tǒng)模型，用字母T表示：T = (Qjtl，一，Π ，ω); 步驟2,任務(wù)描述 采用線性時(shí)序邏輯語言來描述指定任務(wù)，以線性時(shí)序任務(wù)公式的形式表達(dá)任務(wù)需求， 并將任務(wù)公式轉(zhuǎn)換為任務(wù)圖表；線性時(shí)序邏輯語言產(chǎn)生的表達(dá)式基于切換系統(tǒng)的任務(wù)集 合JiiG Π ，線性時(shí)序邏輯語言的語法包括如下：線性時(shí)序邏輯語言的謂詞包括^ (非）、 V (或）、Λ (與）、□(全局）、?(最終）、U(直到）；用線性時(shí)序邏輯語言表述的任務(wù)需 求用字母Φ表示，Φ是由切換系統(tǒng)模型中的子任務(wù)π ie Π 和謂詞所組成的表達(dá)式：假設(shè) (^和Φ 2為兩個(gè)任務(wù)需求，表示不能執(zhí)行Φ Λ色表示執(zhí)行Φ2同時(shí)避免執(zhí)行Φ i， (^Λ ( □ Φ 2)表示執(zhí)行1和Φ 2并最終執(zhí)行Φ 2,（ Λ ( ?□ Φ2)表示循環(huán)執(zhí) 行1和Φ 2;采用線性時(shí)序邏輯語言表述的移動(dòng)機(jī)器人巡回任務(wù)為： (?□ 0) Λ ( ?□ q!) Λ ( ?□ q2) Λ ( ?□ q3) Λ … 其中，0為機(jī)器人運(yùn)行環(huán)境中需要全局避開的區(qū)域，qie Q為環(huán)境中需要巡回的節(jié) 點(diǎn)；在獲得任務(wù)表達(dá)式后，采用LTL2BA工具箱將任務(wù)表達(dá)式Φ轉(zhuǎn)換成對(duì)應(yīng)的BUchi自動(dòng) 機(jī)，Bilchi自動(dòng)機(jī)是可判斷輸入的路徑序列是否滿足任務(wù)需求的圖表，用字母B表不：B = ΣΒ，δ B，F(xiàn))，其中S是有限狀態(tài)集合，S是初始狀態(tài)集合，ΣB是輸入字母表，δ B 是轉(zhuǎn)換函數(shù)，F(xiàn)gS是最終狀態(tài)集合；Bilchi自動(dòng)機(jī)可以保證路徑滿足任務(wù)需求； 步驟3,拓?fù)錁?gòu)建 將加權(quán)切換系統(tǒng)模型和任務(wù)圖表相融合構(gòu)建任務(wù)可行網(wǎng)絡(luò)拓?fù)?；把輸入的加?quán)切換系 統(tǒng)模型T與任務(wù)圖表B作笛卡爾乘積，得到任務(wù)可行網(wǎng)絡(luò)拓?fù)?，用字母P表示，即P = ΤΧΒ， 其中 T = (Q，Q。，一，Π ，ω)，B = (S，S。，Σ Β，δ Β，F(xiàn))，P = (SP，Sp。，一 Ρ，F(xiàn)p)，Sp= QXS 為 有限狀態(tài)集合，Sptl= QtlXStl為初始狀態(tài)集合，為轉(zhuǎn)換函數(shù)，F(xiàn)p= QXF為最終狀 態(tài)集合；在任務(wù)可行網(wǎng)絡(luò)拓?fù)渖纤穆窂?，既滿足指定的任務(wù)需求，又符合機(jī)器人當(dāng)前 的運(yùn)行環(huán)境； 步驟4,路徑尋優(yōu) 采用基于Di jkstra算法的最優(yōu)路徑搜索算法，根據(jù)線性時(shí)序邏輯任務(wù)公式中要求訪 問節(jié)點(diǎn)的出現(xiàn)順序，在任務(wù)可行網(wǎng)絡(luò)拓?fù)渖纤阉鞒鲎顑?yōu)路徑；然后，將任務(wù)可行網(wǎng)絡(luò)拓?fù)渖?的最優(yōu)路徑映射回加權(quán)切換系統(tǒng)模型上，獲得實(shí)際環(huán)境中的最優(yōu)路徑；其中，搜索所得的最 優(yōu)路徑為一個(gè)機(jī)器人運(yùn)行環(huán)境中最優(yōu)路徑上對(duì)應(yīng)的離散坐標(biāo)點(diǎn)集； 步驟5,任務(wù)分解 將步驟4所得的最優(yōu)路徑存入緩存，并結(jié)合當(dāng)前任務(wù)完成情況（已訪問的點(diǎn)），按序?qū)?離散坐標(biāo)點(diǎn)集中的坐標(biāo)信息發(fā)送給目標(biāo)跟蹤控制器，用以控制機(jī)器人實(shí)現(xiàn)路徑跟蹤，直到 遍歷離散坐標(biāo)點(diǎn)集中的所有點(diǎn)，以確保機(jī)器人完成完整的路徑跟蹤； 步驟6,目標(biāo)跟蹤控制 結(jié)合步驟5給出的目標(biāo)點(diǎn)坐標(biāo)/)和實(shí)時(shí)反饋的機(jī)器人當(dāng)前位置坐標(biāo)（X，y)，根據(jù) 兩輪差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)特性，可以得到機(jī)器人到目標(biāo)點(diǎn)的距離偏差r為： r = yj(x* -xf +(y -y)2 根據(jù)全局?jǐn)z像頭實(shí)時(shí)采集的機(jī)器人上前后兩個(gè)不同顏色的色塊中心點(diǎn)坐標(biāo)信息，換算 得到坐標(biāo)系X軸的正方向按順時(shí)針旋轉(zhuǎn)到機(jī)器人正面朝向所經(jīng)過的角度Θ，從而可以得到 機(jī)器人與目標(biāo)點(diǎn)之間的角度偏差Φ為： 其中，口可表示為：
根據(jù)上述獲得的機(jī)器人與目標(biāo)點(diǎn)之間的距離偏差r和角度偏差Φ，采用具有如下形式 的控制器，獲得機(jī)器人的線速度和轉(zhuǎn)動(dòng)速度 υ = K1Ircos ( Φ) ω = -K1Sin ( Φ) cos ( Φ) -K2 Φ 其中，KdPK2為控制參數(shù)，υ為機(jī)器人向前運(yùn)行的速度，ω表示機(jī)器人轉(zhuǎn)向時(shí)的角速 度；然后，再根據(jù)兩輪差動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)特性得到移動(dòng)機(jī)器人左右兩輪的控制量為：
其中，1^和υ κ分別表示機(jī)器人的左右兩輪速度，d為機(jī)器人左右兩輪之間的距離；在 控制量和υ以勺作用下，移動(dòng)機(jī)器人朝著給定的目標(biāo)點(diǎn)運(yùn)行； 步驟7,位姿信息反饋 在機(jī)器人運(yùn)行過程中，采用全局相機(jī)實(shí)時(shí)采集機(jī)器人當(dāng)前運(yùn)行環(huán)境的圖像信息，通過 圖像處理記錄并顯示機(jī)器人的運(yùn)行軌跡，并以色塊識(shí)別的方法將機(jī)器人當(dāng)前的位置坐標(biāo)信 息和朝向信息通過通信協(xié)議UDP反饋給步驟6中給出的目標(biāo)跟蹤控制器，從而實(shí)現(xiàn)閉環(huán)控 制，其中，機(jī)器人的朝向通過獲取機(jī)器人上兩個(gè)不同顏色的色塊的中心坐標(biāo)來換算得到；此 外，當(dāng)機(jī)器人運(yùn)動(dòng)到當(dāng)前給定的目標(biāo)點(diǎn)時(shí)，重復(fù)執(zhí)行步驟5,給出下一個(gè)目標(biāo)點(diǎn)的坐標(biāo)，實(shí)現(xiàn) 分段目標(biāo)跟蹤，最終達(dá)到路徑跟蹤的效果。
2.根據(jù)權(quán)利要求1所述的基于目標(biāo)跟蹤控制策略的單移動(dòng)機(jī)器人最優(yōu)巡回控制方法， 其特征在于所述的步驟6中，目標(biāo)跟蹤控制器 υ = K1Ircos ( Φ) ω = -K1Sin ( Φ) cos ( Φ) -K2 Φ 中的控制參數(shù)1和K 2必須都大于零，以滿足李雅普諾夫穩(wěn)定性判據(jù)，使得在有限的時(shí) 間內(nèi)，機(jī)器人到目標(biāo)點(diǎn)的距離r和機(jī)器人與目標(biāo)點(diǎn)之間的角度偏差Φ能夠收斂到0，即，使 得目標(biāo)跟蹤控制器能夠控制機(jī)器人向目標(biāo)點(diǎn)運(yùn)動(dòng)；而且，控制參數(shù)&和K 2取值的差值不可 過大，以免KJPK2差值過大時(shí)機(jī)器人會(huì)產(chǎn)生很高的角速度，導(dǎo)致機(jī)器人蛇形前進(jìn)，影響控制 效果。
【專利摘要】一種基于目標(biāo)跟蹤控制策略的單移動(dòng)機(jī)器人最優(yōu)巡回控制方法，包括地圖構(gòu)建、任務(wù)描述、拓?fù)錁?gòu)建、路徑尋優(yōu)、任務(wù)分解、目標(biāo)跟蹤控制和位姿信息反饋7個(gè)步驟。首先，基于線性時(shí)序邏輯理論，將構(gòu)建的加權(quán)切換系統(tǒng)與線性時(shí)序邏輯任務(wù)公式相融合，構(gòu)建任務(wù)可行網(wǎng)絡(luò)拓?fù)?；然后，采用基于Dijkstra算法的路徑尋優(yōu)方法搜索出最優(yōu)路徑，并將環(huán)境中對(duì)應(yīng)最優(yōu)路徑上的離散坐標(biāo)點(diǎn)存儲(chǔ)在緩存中，之后按序發(fā)送給目標(biāo)跟蹤控制器；然后，采用目標(biāo)跟蹤控制方法，根據(jù)機(jī)器人與目標(biāo)點(diǎn)之間距離和角度的偏差，給出控制量控制機(jī)器人向目標(biāo)點(diǎn)運(yùn)行；最后，在機(jī)器人運(yùn)行過程中，采用全局?jǐn)z像頭實(shí)時(shí)獲取機(jī)器人當(dāng)前的位置坐標(biāo)和朝向信息，并反饋給目標(biāo)跟蹤控制器，從而實(shí)現(xiàn)閉環(huán)控制。
【IPC分類】G05D1-02
【公開號(hào)】CN104834309
【申請?zhí)枴緾N201510167059
【發(fā)明人】歐林林, 陳浩, 禹鑫燚, 程誠, 趙千一, 邢雙, 韓紹峰, 李壯, 陳志南
【申請人】浙江工業(yè)大學(xué)
【公開日】2015年8月12日
【申請日】2015年4月10日