前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的動(dòng)態(tài)編隊(duì)控制方法
【專利摘要】本發(fā)明提出一種前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的動(dòng)態(tài)編隊(duì)控制方法，多移動(dòng)機(jī)器人在自主編隊(duì)運(yùn)動(dòng)中，領(lǐng)航機(jī)器人負(fù)責(zé)自主導(dǎo)航、目標(biāo)搜索和主動(dòng)避障等任務(wù)，其它機(jī)器人作為跟隨者與領(lǐng)航機(jī)器人一起自主組成既定的幾何隊(duì)形，并在運(yùn)動(dòng)過(guò)程中保持該隊(duì)形。跟隨機(jī)器人的運(yùn)動(dòng)控制器采用基于反步法和鏈?zhǔn)较到y(tǒng)的混合控制算法。當(dāng)領(lǐng)航機(jī)器人的位姿參數(shù)發(fā)生變化時(shí)，跟隨機(jī)器人實(shí)時(shí)檢測(cè)其與領(lǐng)航機(jī)器人之間的距離d和夾角并將自身狀態(tài)向量與期望隊(duì)形的狀態(tài)向量進(jìn)行比較，獲得跟隨機(jī)器人實(shí)際位姿與理想位姿之間的誤差，再依據(jù)該位姿誤差求得各跟隨機(jī)器人對(duì)應(yīng)的運(yùn)動(dòng)控制器，實(shí)現(xiàn)對(duì)跟隨機(jī)器人的速度和角速度的有效控制，最終達(dá)到多移動(dòng)機(jī)器人的編隊(duì)成形和編隊(duì)保持的目的。
【專利說(shuō)明】
前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的動(dòng)態(tài)編隊(duì)控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于移動(dòng)機(jī)器人技術(shù)領(lǐng)域，具體涉及多個(gè)自主車式移動(dòng)機(jī)器人的動(dòng)態(tài)編隊(duì) 控制方法?；?前輪導(dǎo)向與驅(qū)動(dòng)一體化"的車式移動(dòng)機(jī)器人驅(qū)動(dòng)新模式，提出一種適用于 領(lǐng)航跟隨編隊(duì)的機(jī)器人運(yùn)動(dòng)控制方法，為多移動(dòng)機(jī)器人在聯(lián)合偵查、群體盯梢、協(xié)作救援、 合作搬運(yùn)等領(lǐng)域的工程應(yīng)用提供一種解決方法。
【背景技術(shù)】
[0002] 移動(dòng)機(jī)器人作為一種自主、智能的運(yùn)動(dòng)平臺(tái)和運(yùn)輸載體，尤其適用于野外偵查、目 標(biāo)搜索、排雷搶險(xiǎn)、空間探測(cè)等危險(xiǎn)、污染、極端等場(chǎng)合。傳統(tǒng)的單移動(dòng)機(jī)器人存在結(jié)構(gòu)復(fù) 雜、造價(jià)昂貴、作業(yè)范圍受限等弊端，而近年來(lái)出現(xiàn)的多移動(dòng)機(jī)器人技術(shù)則具有群體協(xié)作、 高魯棒性、單體造價(jià)低等獨(dú)特優(yōu)勢(shì)。
[0003] 移動(dòng)機(jī)器人動(dòng)態(tài)編隊(duì)是多移動(dòng)機(jī)器人技術(shù)領(lǐng)域的一個(gè)重要分支，在聯(lián)合偵查、群 體盯梢、協(xié)作救援、合作搬運(yùn)、傳感網(wǎng)絡(luò)等軍事和民用領(lǐng)域具有廣闊的應(yīng)用背景。由于受傳 感器感知范圍、單體作業(yè)范圍、設(shè)備搭載能力、通訊距離等因素制約，在執(zhí)行大范圍、長(zhǎng)距 離、動(dòng)態(tài)復(fù)雜等環(huán)境下的野外作業(yè)任務(wù)時(shí)，亟需多個(gè)移動(dòng)機(jī)器人協(xié)作構(gòu)成一個(gè)較為精準(zhǔn)的 編隊(duì)或圖形以完成特定的目標(biāo)任務(wù)，并在執(zhí)行作業(yè)任務(wù)、目標(biāo)跟蹤、躲避障礙物時(shí)盡可能保 持編隊(duì)隊(duì)形不變。因此，如何形成編隊(duì)、如何保持編隊(duì)隊(duì)形是移動(dòng)機(jī)器人動(dòng)態(tài)編隊(duì)的兩個(gè)核 心問(wèn)題。
[0004] 目前，移動(dòng)機(jī)器人動(dòng)態(tài)編隊(duì)包含三種方法:基于行為法(Behavior-based)、虛擬結(jié) 構(gòu)法(Virtual-structure)和領(lǐng)航跟隨法(Leader-follower)?；谛袨榈木庩?duì)控制方法無(wú) 法準(zhǔn)確描述系統(tǒng)整體行為、難以進(jìn)行數(shù)學(xué)分析，且系統(tǒng)穩(wěn)定性無(wú)法保證，而虛擬結(jié)構(gòu)法存在 系統(tǒng)靈活性差的缺陷。領(lǐng)航跟隨法以其易于保持隊(duì)形、數(shù)學(xué)分析簡(jiǎn)單等優(yōu)勢(shì)，廣泛應(yīng)用于多 移動(dòng)機(jī)器人編隊(duì)、無(wú)人機(jī)編隊(duì)、艦船編隊(duì)等領(lǐng)域。
[0005] 領(lǐng)航跟隨法在執(zhí)行編隊(duì)任務(wù)時(shí)，由領(lǐng)航機(jī)器人(Leader)負(fù)責(zé)目標(biāo)跟蹤、軌跡生成、 主動(dòng)避障等任務(wù)，而跟隨機(jī)器人(follower)主要任務(wù)是跟隨領(lǐng)航機(jī)器人的運(yùn)動(dòng)軌跡，并同 時(shí)保持預(yù)先設(shè)定的編隊(duì)隊(duì)形。因此，跟隨機(jī)器人的驅(qū)動(dòng)方式、運(yùn)動(dòng)控制器設(shè)計(jì)則成為領(lǐng)航跟 隨法形成編隊(duì)并保持隊(duì)形的關(guān)鍵所在。由于跟隨機(jī)器人在編隊(duì)運(yùn)動(dòng)中處于被動(dòng)跟隨的角 色，現(xiàn)有基于"前輪導(dǎo)向與后輪驅(qū)動(dòng)"的車式移動(dòng)機(jī)器人在跟隨運(yùn)動(dòng)時(shí)，有可能因?yàn)榍拜嗈D(zhuǎn) 向角過(guò)大而致使前輪卡死，從而導(dǎo)致跟隨機(jī)器人運(yùn)動(dòng)失效、整個(gè)編隊(duì)隊(duì)形失真等問(wèn)題。

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

[0006] 針對(duì)現(xiàn)有"前輪導(dǎo)向與后輪驅(qū)動(dòng)"車式移動(dòng)機(jī)器人存在的前輪易卡死、編隊(duì)隊(duì)形易 失真等問(wèn)題，本發(fā)明基于一種"前輪導(dǎo)向與驅(qū)動(dòng)一體化"的車式移動(dòng)機(jī)器人驅(qū)動(dòng)新模式;在 此新驅(qū)動(dòng)模式基礎(chǔ)上，設(shè)計(jì)了一種基于反步法(Backstepping)和鏈?zhǔn)较到y(tǒng)的跟隨機(jī)器人運(yùn) 動(dòng)控制器，以確保多個(gè)車式移動(dòng)機(jī)器人編隊(duì)運(yùn)動(dòng)的精準(zhǔn)性和穩(wěn)定性，從而為多自主移動(dòng)機(jī) 器人在聯(lián)合偵查、協(xié)作救援、合作搬運(yùn)、空間探索等領(lǐng)域的工程應(yīng)用提供理論支撐與解決思 路。
[0007] 技術(shù)方案
[0008] 本發(fā)明中的車式移動(dòng)機(jī)器人采用"前輪導(dǎo)向與驅(qū)動(dòng)一體化"運(yùn)動(dòng)模式，多個(gè)移動(dòng)機(jī) 器人的動(dòng)態(tài)編隊(duì)控制采用領(lǐng)航跟隨法。多移動(dòng)機(jī)器人在自主編隊(duì)運(yùn)動(dòng)并執(zhí)行任務(wù)過(guò)程中， 領(lǐng)航機(jī)器人負(fù)責(zé)自主導(dǎo)航、目標(biāo)搜索和主動(dòng)避障等任務(wù)，而其它機(jī)器人則作為跟隨者與領(lǐng) 航機(jī)器人一起自主組成既定的幾何隊(duì)形，并在運(yùn)動(dòng)過(guò)程中盡可能保持該隊(duì)形不變。
[0009] 跟隨機(jī)器人的運(yùn)動(dòng)控制器采用基于反步法和鏈?zhǔn)较到y(tǒng)的混合控制算法。當(dāng)領(lǐng)航機(jī) 器人的位姿參數(shù)發(fā)生變化時(shí)，跟隨機(jī)器人實(shí)時(shí)檢測(cè)其與領(lǐng)航機(jī)器人之間的距離d和夾角滬， 并將自身狀態(tài)向量與期望隊(duì)形的狀態(tài)向量進(jìn)行比較，獲得跟隨機(jī)器人實(shí)際位姿與理 想位姿之間的誤差，再依據(jù)該位姿誤差求得各跟隨機(jī)器人對(duì)應(yīng)的運(yùn)動(dòng)控制器，實(shí)現(xiàn)對(duì)跟隨 機(jī)器人的速度和角速度的有效控制，最終達(dá)到多移動(dòng)機(jī)器人的編隊(duì)成形和編隊(duì)保持的目 的。
[0010]本發(fā)明的技術(shù)方案為：
[0011] 所述一種前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的動(dòng)態(tài)編隊(duì)控制方法，其特征在于:包括以下 步驟：
[0012] 步驟1:對(duì)非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人進(jìn)行設(shè)置，確 保非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人都在各自的通訊范圍內(nèi)；
[0013] 步驟2:根據(jù)普法夫約束方程
[0015]建立非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型為：
[0017] 其中x，y，0，φ，l，v，w分別表示前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人在廣義坐標(biāo)下的位姿橫 坐標(biāo)、位姿縱坐標(biāo)、方向角、前輪方向角、車身長(zhǎng)度、速度和角速度；
[0018] 步驟3:將非完整移動(dòng)機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)模型通過(guò)鏈?zhǔn)阶儞Q轉(zhuǎn)換為可控的鏈?zhǔn)?系統(tǒng)：
[0020]同時(shí)控制輸入的變換方程為：
[0023]通過(guò)上述鏈?zhǔn)阶儞Q非完整移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型表示為：
[0025]其中Vl，Wl分別表示第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下的速度和角速度，Q=[ qia， 91,2沖,3沖,4]7為變換后第1個(gè)跟隨機(jī)器人的狀態(tài)量，11=[111, 1，111,2]7為變換后第1個(gè)跟隨機(jī) 器人的控制輸入；
[0026]步驟4:計(jì)算第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下理想位姿
[0028] 其中^^^，^(^分別表示前領(lǐng)航移動(dòng)機(jī)器人在廣義坐標(biāo)下的位姿橫坐標(biāo)"立姿 縱坐標(biāo)、方向角、前輪方向角3^7^91￥，巾 1￥，(1，分別表示第1個(gè)跟隨機(jī)器人在廣義坐標(biāo) 下的理想位姿橫坐標(biāo)、位姿縱坐標(biāo)、方向角、前輪方向角、第i個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人之 間的距離、第i個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人之間的夾角；
[0029] 步驟5:構(gòu)建廣義坐標(biāo)系下跟隨機(jī)器人的誤差模型：
[0030] 實(shí)時(shí)采集各跟隨機(jī)器人的實(shí)際位姿，并與各自對(duì)應(yīng)的期望位姿進(jìn)行比較并形成誤 差模型，設(shè)計(jì)運(yùn)動(dòng)控制器使得誤差趨近于零，實(shí)現(xiàn)跟隨機(jī)器人的實(shí)際位姿趨近于預(yù)期的期 望位姿：
[0032] 其中1^，5^，0^，(})^分別表示第1個(gè)跟隨機(jī)器人的位姿橫坐標(biāo)誤差，位姿縱坐標(biāo)誤 差，方向角誤差，前輪方向角誤差; Xl，yi，，Φ i分別表示第i個(gè)跟隨機(jī)器人的實(shí)際位姿橫坐 標(biāo)，位姿縱坐標(biāo)，方向角，前輪方向角；
[0033] 將上述在廣義坐標(biāo)系下跟隨機(jī)器人的位姿誤差轉(zhuǎn)換到鏈?zhǔn)较到y(tǒng)下的跟隨機(jī)器人 的位姿誤差：
[0037] 其中：啦,1(3，(^2(3，(^3(3，(^4(3分別表示第1個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的位姿誤差； 9 1,1^1>沖,3^1,扣分別表示第1個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的理想位姿參數(shù);11=[1 11山， m,2v]T分別表示第i個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的理想控制輸入；
[0038] 步驟6:設(shè)計(jì)跟隨機(jī)器人的運(yùn)動(dòng)控制器：
[0041 ] 有益效果
[0042] 本發(fā)明采用"前輪導(dǎo)向與驅(qū)動(dòng)一體化"的車式移動(dòng)機(jī)器人驅(qū)動(dòng)模式，有助于解決現(xiàn) 有"前輪導(dǎo)向與后輪驅(qū)動(dòng)"方式存在的車式移動(dòng)機(jī)器人前輪卡死、編隊(duì)隊(duì)形失真等問(wèn)題；另 外，采用反步法(Backstepping)方法和李雅普諾夫(Lyapunouv)穩(wěn)定性理論，所設(shè)計(jì)的跟隨 機(jī)器人運(yùn)動(dòng)控制器具有可控性和穩(wěn)定性;本發(fā)明在車式移動(dòng)機(jī)器人驅(qū)動(dòng)方式改進(jìn)、運(yùn)動(dòng)控 制器設(shè)計(jì)兩方面的解決思路和研究方法，在多移動(dòng)機(jī)器人編隊(duì)控制、無(wú)人機(jī)編隊(duì)飛行、艦船 編隊(duì)航行等應(yīng)用領(lǐng)域具有一定的適用性和推廣性。
[0043] 本發(fā)明的附加方面和優(yōu)點(diǎn)將在下面的描述中部分給出，部分將從下面的描述中變 得明顯，或通過(guò)本發(fā)明的實(shí)踐了解到。
【附圖說(shuō)明】
[0044] 本發(fā)明的上述和/或附加的方面和優(yōu)點(diǎn)從結(jié)合下面附圖對(duì)實(shí)施例的描述中將變得 明顯和容易理解，其中：
[0045] 圖1.前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人模型；
[0046] 圖2 .Leader-follower編隊(duì)運(yùn)動(dòng)模型（以三角形為例）；
[0047]圖3.編隊(duì)運(yùn)動(dòng)過(guò)程中跟隨機(jī)器人的前輪卡死現(xiàn)象（Φ2ν=Φ3ν=90°);
[0048]圖4.編隊(duì)直線軌跡仿真結(jié)果；
[0049]圖5.編隊(duì)類正弦軌跡仿真結(jié)果；
[0050]圖6.編隊(duì)圓軌跡仿真結(jié)果。
【具體實(shí)施方式】
[0051] 下面詳細(xì)描述本發(fā)明的實(shí)施例，所述實(shí)施例是示例性的，旨在用于解釋本發(fā)明，而 不能理解為對(duì)本發(fā)明的限制。
[0052] 本發(fā)明中的車式移動(dòng)機(jī)器人采用"前輪導(dǎo)向與驅(qū)動(dòng)一體化"運(yùn)動(dòng)模式，多個(gè)移動(dòng)機(jī) 器人的動(dòng)態(tài)編隊(duì)控制采用領(lǐng)航跟隨法。多移動(dòng)機(jī)器人在自主編隊(duì)運(yùn)動(dòng)并執(zhí)行任務(wù)過(guò)程中， 領(lǐng)航機(jī)器人負(fù)責(zé)自主導(dǎo)航、目標(biāo)搜索和主動(dòng)避障等任務(wù)，而其它機(jī)器人則作為跟隨者與領(lǐng) 航機(jī)器人一起自主組成既定的幾何隊(duì)形，并在運(yùn)動(dòng)過(guò)程中盡可能保持該隊(duì)形不變。
[0053]跟隨機(jī)器人的運(yùn)動(dòng)控制器采用基于反步法和鏈?zhǔn)较到y(tǒng)的混合控制算法。當(dāng)領(lǐng)航機(jī) 器人的位姿參數(shù)發(fā)生變化時(shí)，跟隨機(jī)器人實(shí)時(shí)檢測(cè)其與領(lǐng)航機(jī)器人之間的距離d和夾角爐， 并將自身狀態(tài)向量(40與期望隊(duì)形的狀態(tài)向量進(jìn)行比較，獲得跟隨機(jī)器人實(shí)際位姿與理 想位姿之間的誤差，再依據(jù)該位姿誤差求得各跟隨機(jī)器人對(duì)應(yīng)的運(yùn)動(dòng)控制器，實(shí)現(xiàn)對(duì)跟隨 機(jī)器人的速度和角速度的有效控制，最終達(dá)到多移動(dòng)機(jī)器人的編隊(duì)成形和編隊(duì)保持的目 的。
[0054]具體包括以下步驟：
[0055] 步驟1:對(duì)非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人進(jìn)行設(shè)置，機(jī) 器人的初始位置和姿態(tài)在傳感器的通訊范圍隨機(jī)設(shè)置，確保非完整移動(dòng)機(jī)器人系統(tǒng)中的每 個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人都在各自的通訊范圍內(nèi)；機(jī)器人通過(guò)藍(lán)牙模塊啟動(dòng)，利用傳感 器進(jìn)行相互之間的通訊。
[0056] 步驟2:為防止非完整移動(dòng)機(jī)器人系統(tǒng)中每個(gè)移動(dòng)機(jī)器人在運(yùn)動(dòng)過(guò)程中出現(xiàn)打滑 現(xiàn)象，對(duì)其進(jìn)行普法夫(Pfaffian)約束，根據(jù)普法夫約束方程
[0058]建立非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型為：
[0060] 其中x，y，0，Φ，l，v，w分別表示前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人在廣義坐標(biāo)下的位姿橫 坐標(biāo)、位姿縱坐標(biāo)、方向角、前輪方向角、車身長(zhǎng)度、速度和角速度。
[0061] 步驟3:根據(jù)步驟2所建立的基于普法夫(Pfaffian)約束非完整移動(dòng)機(jī)器人系統(tǒng)， 為了便于控制器的設(shè)計(jì)，將非完整移動(dòng)機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)模型通過(guò)鏈?zhǔn)阶儞Q轉(zhuǎn)換為可控 的鏈?zhǔn)较到y(tǒng)：
[0063]同時(shí)控制輸入的變換方程為：
[0066]通過(guò)上述鏈?zhǔn)阶儞Q非完整移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型表示為：
[0068]其中Vl，Wl分別表示第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下的速度和角速度，Q=[ qia， 91,2沖,3沖,4]7為變換后第1個(gè)跟隨機(jī)器人的狀態(tài)量，11=[111, 1，111,2]7為變換后第1個(gè)跟隨機(jī) 器人的控制輸入。
[0069]步驟4:為了完成"前輪導(dǎo)向與驅(qū)動(dòng)一體化"運(yùn)動(dòng)新模式下多移動(dòng)機(jī)器人的編隊(duì)成 形以及編隊(duì)保持的任務(wù)，計(jì)算第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下理想位姿
[0071] 其中^^^，^(^分別表示前領(lǐng)航移動(dòng)機(jī)器人在廣義坐標(biāo)下的位姿橫坐標(biāo)"立姿 縱坐標(biāo)、方向角、前輪方向角;^，7^9^(^，(1，分別表示第1個(gè)跟隨機(jī)器人在廣義坐標(biāo) 下的理想位姿橫坐標(biāo)、位姿縱坐標(biāo)、方向角、前輪方向角、第i個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人之 間的距離、第i個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人之間的夾角。
[0072] 步驟5:構(gòu)建廣義坐標(biāo)系下跟隨機(jī)器人的誤差模型：
[0073] 實(shí)時(shí)采集各跟隨機(jī)器人的實(shí)際位姿，并與各自對(duì)應(yīng)的期望位姿進(jìn)行比較并形成誤 差模型，設(shè)計(jì)運(yùn)動(dòng)控制器使得誤差趨近于零，實(shí)現(xiàn)跟隨機(jī)器人的實(shí)際位姿趨近于預(yù)期的期 望位姿：
[0075]其中xie，yie，0ie，φ ie分別表示第i個(gè)跟隨機(jī)器人的位姿橫坐標(biāo)誤差，位姿縱坐標(biāo)誤 差，方向角誤差，前輪方向角誤差;Xl，yi，，Φ i分別表示第i個(gè)跟隨機(jī)器人的實(shí)際位姿橫坐 標(biāo)，位姿縱坐標(biāo)，方向角，前輪方向角；
[0076]將上述在廣義坐標(biāo)系下跟隨機(jī)器人的位姿誤差轉(zhuǎn)換到鏈?zhǔn)较到y(tǒng)下的跟隨機(jī)器人 的位姿誤差：
[0080] 其中：啦,1(3，(^2(3，(^3(3，(^4(3分別表示第1個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的位姿誤差； 9 1,1^1>沖,3^1,扣分別表示第1個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的理想位姿參數(shù);11=[1 11山， m,2v]T分別表示第i個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的理想控制輸入。
[0081] 步驟6:設(shè)計(jì)跟隨機(jī)器人的運(yùn)動(dòng)控制器：
[0082] 采用反步法(Backstepping)方法和李雅普諾夫(Lyapunouv)穩(wěn)定性理論，通過(guò)遞 歸方式構(gòu)造出李雅普諾夫(Lyapunov)函數(shù)，推導(dǎo)并設(shè)計(jì)出使得整個(gè)系統(tǒng)具有李雅普諾夫 (Lyapunov)穩(wěn)定的跟隨機(jī)器人運(yùn)動(dòng)控制器;可計(jì)算出跟隨機(jī)器人的速度和角速度：
[0085] 利用所求得的閉環(huán)運(yùn)動(dòng)控器(Vl，Wl)來(lái)控制編隊(duì)中第i跟隨機(jī)器人，使得整個(gè)編隊(duì) 系統(tǒng)的動(dòng)態(tài)特性和穩(wěn)態(tài)特性都得到改善，尤其解決了多移動(dòng)機(jī)器人在動(dòng)態(tài)編隊(duì)中，跟隨機(jī) 器人前輪卡死的狀況，使得動(dòng)態(tài)編隊(duì)可以順利進(jìn)行。
[0086] 基于上述原理，本實(shí)施例過(guò)程為：
[0087] 1.針對(duì)單個(gè)車式移動(dòng)機(jī)器人建立運(yùn)動(dòng)學(xué)模型。見圖1。
[0089] 2 ·根據(jù)Leader-f 〇1 lower方法，計(jì)算第i個(gè)跟隨移動(dòng)機(jī)器人的理想位姿，見圖2 〇
[0091] 其中，d代表編隊(duì)中跟隨機(jī)器人與領(lǐng)航機(jī)器人保持的理想距離，識(shí)代表編隊(duì)中跟隨 機(jī)器人與領(lǐng)航機(jī)器人保持的理想角度。
[0092] 3.將運(yùn)動(dòng)學(xué)模型轉(zhuǎn)化成鏈?zhǔn)较到y(tǒng)。通過(guò)鏈?zhǔn)阶儞Q：
[0099] 4.在鏈?zhǔn)较到y(tǒng)下，對(duì)跟隨移動(dòng)機(jī)器人的理想位姿與實(shí)際位姿進(jìn)行分析，見圖2。
[0101 ] 5.利用反步法(backstepping)設(shè)計(jì)第i個(gè)跟隨車式移動(dòng)機(jī)器人運(yùn)動(dòng)控制器。為了 使用(backstepping)，先將上述誤差模型映射為三角形式，如下：
[01 02] yi,l - Qi, 4e-( Qi, Se+Qi, 3v) * Qi,le
[01 03] yi,2 - Qi, 3e-( Qi, 2e'l'Qi, 2v) * Qi,le
[0104] yi,3 = qi,2e
[0105] yi,4 = qi,ie
[0106] 對(duì)上式微分：
[0107] =Wu-^.2(?i.i-wu,.)v.4
[0108] Λ.2 ，.少),3-"他八4
[0109]
[0110]
[0111] ⑴考慮九1子系統(tǒng)，將y1>2視為虛擬控制量，madPyu視為時(shí)變函數(shù)，令又廣仏， 選取李雅普諾夫(Lyapunouv)函數(shù)：
[0113] = Μ/,1,. (?/,1 -?/,1ν ) ν,·,4
[0114] 因?yàn)閥1>2是虛擬控制量，令其期望值為f1;1，并定義誤差變量瓦2=為 12-/,+ |^,,卜觀 察到當(dāng)yi,4=0時(shí)，fi,i(yi,i)=0是系統(tǒng)的穩(wěn)定方程。
[0115] (2)考慮系統(tǒng)，選取李雅普諾夫(Lyapunouv)函數(shù)：
[0118]其中凡2二乃>2 -又2 (知)，將y i, 3視為虛擬控制量，令其期望值為f i, 2，定義 另,3 =乃,3 - Λ2 (乃,1，乃,2 )，對(duì)八2求導(dǎo)，可得到：
[0120] 可知乜,2^,1#,2)=11,1，對(duì)無(wú),3求導(dǎo)，可得：
[0122] (3)考慮(ju,九2，九3)子系統(tǒng)，選取李雅普諾夫(Lyapunouv)函數(shù)，
[0125] 為了使負(fù)定，選取如下控制輸入：
[0126] -kxyui - uiJryLi + u^ - ??；2ν + uUvy. ,
[0127] 其中，1α>0,可得：
[0128] Ui,2 = Ui,2v-2Ui,lvyi,2-kl(yi,l+yi,3)
[0129] (4)考慮(，允.2, )系統(tǒng)，選取李雅普諾夫(Lyapunouv)函數(shù)，
[0131]為使ft 4負(fù)定，選取如下控制輸入：
[0147] 利用上述方法所求得的閉環(huán)運(yùn)動(dòng)控器(Vl Wl)來(lái)控制編隊(duì)中的第i個(gè)跟隨機(jī)器人， 并選取控制參數(shù)(khks)，使得整個(gè)編隊(duì)系統(tǒng)的動(dòng)態(tài)特性和穩(wěn)態(tài)特性都得到改善，尤其解決 了多移動(dòng)機(jī)器人在動(dòng)態(tài)編隊(duì)中，跟隨機(jī)器人前輪卡死的狀況，使得動(dòng)態(tài)編隊(duì)可以順利進(jìn)行。
[0148] 通過(guò)直線、類正弦、圓三類運(yùn)動(dòng)軌跡的三組仿真實(shí)驗(yàn)驗(yàn)證了上述動(dòng)態(tài)編隊(duì)控制方 法的有效性。
[0149] 在直線編隊(duì)仿真實(shí)驗(yàn)中，三個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人，先完成三角形編隊(duì)，然后 保持隊(duì)形做直線運(yùn)動(dòng)。機(jī)器人的初始位姿和編隊(duì)過(guò)程如圖4(a)所示，實(shí)際行運(yùn)動(dòng)軌跡如圖4 (b)所示，運(yùn)動(dòng)過(guò)程中跟隨機(jī)器人的跟隨速度如圖4(c)所示，跟隨機(jī)器人與領(lǐng)航機(jī)器人的距 離誤差le和角度誤差θβ如圖4(d)和圖4(e)。由于跟隨機(jī)器人起始位姿遠(yuǎn)離期望位姿，初始時(shí) 會(huì)有較大誤差，前40s機(jī)器人迅速完成編隊(duì)，40s之后機(jī)器人以較小誤差保持隊(duì)形前進(jìn)。 [0150]在類正弦編隊(duì)仿真實(shí)驗(yàn)中，三個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人，先完成三角形編隊(duì)，然 后保持隊(duì)形做類正弦運(yùn)動(dòng)。機(jī)器人的初始位置和編隊(duì)過(guò)程如圖5(a)所示，實(shí)際行駛編隊(duì)軌 跡如圖5(b)所示，運(yùn)動(dòng)過(guò)程中跟隨機(jī)器人跟隨速度如圖5(c)所示，跟隨機(jī)器人與領(lǐng)航機(jī)器 人的距離誤差l e和角度誤差如圖5(d)和圖5(e)。由于正弦軌跡的周期性，跟隨機(jī)器人的速 度v以及距離誤差le和角度誤差9 6都成周期性變化。跟隨機(jī)器人在前10s迅速完成編隊(duì)，10s 之后以較小誤差保持隊(duì)形前進(jìn)。
[0151]在圓編隊(duì)仿真實(shí)驗(yàn)中，三個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人，先完成三角形編隊(duì)，然后保 持隊(duì)形做圓運(yùn)動(dòng)。機(jī)器人的初始位置和編隊(duì)過(guò)程如圖6(a)所示，實(shí)際行駛編隊(duì)軌跡如圖6 (b)所示，運(yùn)動(dòng)過(guò)程中跟隨機(jī)器人的跟隨速度如圖6(c)所示，跟隨機(jī)器人的距離誤差l e和角 度誤差9e如圖6(d)和圖6(e)。其中在t = 100和250s時(shí)出現(xiàn)奇異點(diǎn)兩個(gè)跟隨機(jī)器人前輪導(dǎo)向 角〇1=(|)2 = 90°使得跟隨機(jī)器人的速度產(chǎn)生突變，由于本發(fā)明采用前輪驅(qū)動(dòng)在該奇異點(diǎn)跟 隨機(jī)器人仍能繼續(xù)跟領(lǐng)航機(jī)器人保持隊(duì)形前進(jìn)。跟隨機(jī)器人在前l(fā)〇s迅速完成編隊(duì)，l〇s之 后以較小誤差保持隊(duì)形行進(jìn)。
[0152] 盡管上面已經(jīng)示出和描述了本發(fā)明的實(shí)施例，可以理解的是，上述實(shí)施例是示例 性的，不能理解為對(duì)本發(fā)明的限制，本領(lǐng)域的普通技術(shù)人員在不脫離本發(fā)明的原理和宗旨 的情況下在本發(fā)明的范圍內(nèi)可以對(duì)上述實(shí)施例進(jìn)行變化、修改、替換和變型。
【主權(quán)項(xiàng)】
1. 一種前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的動(dòng)態(tài)編隊(duì)控制方法，其特征在于:包括W下步驟： 步驟1:對(duì)非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人進(jìn)行設(shè)置，確保非 完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人都在各自的通訊范圍內(nèi)； 步驟2:根據(jù)普法夫約束方程建立非完整移動(dòng)機(jī)器人系統(tǒng)中的每個(gè)前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型為：其中x，y，0, Φ ,l,v，w分別表示前輪驅(qū)動(dòng)車式移動(dòng)機(jī)器人在廣義坐標(biāo)下的位姿橫坐標(biāo)、 位姿縱坐標(biāo)、方向角、前輪方向角、車身長(zhǎng)度、速度和角速度. 步驟3:將非完整移動(dòng)機(jī)器人系統(tǒng)的運(yùn)動(dòng)學(xué)模型通過(guò)鏈?zhǔn)阶儞Q轉(zhuǎn)換為可控的鏈?zhǔn)较到y(tǒng)：其中vi，wi分別表示第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下的速度和角速度，Q=[qi,i，qi,2, 91,3,〇1,4^為變換后第1個(gè)跟隨機(jī)器人的狀態(tài)量，11=[111,1，111,2]%變換后第1個(gè)跟隨機(jī)器人 的控制輸入； 步驟4:計(jì)算第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下理想位姿其中：^1，71，01，(1)1分別表示前領(lǐng)航移動(dòng)機(jī)器人在廣義坐標(biāo)下的位姿橫坐標(biāo)、位姿縱坐 標(biāo)、方向角、前輪方向角;Xlv，ylv，θlv，Φlv，d，巧分別表示第i個(gè)跟隨機(jī)器人在廣義坐標(biāo)下的 理想位姿橫坐標(biāo)、位姿縱坐標(biāo)、方向角、前輪方向角、第i個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人之間的 距離、第i個(gè)跟隨機(jī)器人與領(lǐng)航機(jī)器人之間的夾角； 步驟5:構(gòu)建廣義坐標(biāo)系下跟隨機(jī)器人的誤差模型： 實(shí)時(shí)采集各跟隨機(jī)器人的實(shí)際位姿，并與各自對(duì)應(yīng)的期望位姿進(jìn)行比較并形成誤差模 型，設(shè)計(jì)運(yùn)動(dòng)控制器使得誤差趨近于零，實(shí)現(xiàn)跟隨機(jī)器人的實(shí)際位姿趨近于預(yù)期的期望位 姿：其中Xie，yie，0ie，Φ le分別表示第i個(gè)跟隨機(jī)器人的位姿橫坐標(biāo)誤差，位姿縱坐標(biāo)誤差， 方向角誤差，前輪方向角誤差;^1，71，91，(1)1分別表示第1個(gè)跟隨機(jī)器人的實(shí)際位姿橫坐標(biāo)， 位姿縱坐標(biāo)，方向角，前輪方向角. 將上述在廣義坐標(biāo)系下跟隨機(jī)器人的位姿誤差轉(zhuǎn)換到鏈?zhǔn)较到y(tǒng)下的跟隨機(jī)器人的位 姿誤差：其中：91,16,91,26,91,36,91,46分別表不第1個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的位姿誤差；91,山， 91,2巾沖,3巾沖,如分別表示第1個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的理想位姿參數(shù);11=[化叫，化2乂^ 分別表示第i個(gè)跟隨機(jī)器人在鏈?zhǔn)较到y(tǒng)下的理想控制輸入； 步驟6 :設(shè)計(jì)跟隨機(jī)器人的運(yùn)動(dòng)控制器：
【文檔編號(hào)】G05D1/02GK106094835SQ201610618966
【公開日】2016年11月9日
【申請(qǐng)日】2016年8月1日 公開號(hào)201610618966.0, CN 106094835 A, CN 106094835A, CN 201610618966, CN-A-106094835, CN106094835 A, CN106094835A, CN201610618966, CN201610618966.0
【發(fā)明人】楊宏安, 張婷, 王經(jīng)國(guó), 張亞, 陳豪杰, 白洛玉
【申請(qǐng)人】西北工業(yè)大學(xué)