一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法
【專利摘要】本發(fā)明涉及一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法，屬于差速驅(qū)動(dòng)機(jī)器人的定點(diǎn)跟蹤控制技術(shù)領(lǐng)域，該方法包括：控制器與外界監(jiān)測系統(tǒng)實(shí)時(shí)通信，獲取目標(biāo)點(diǎn)信息；首先，根據(jù)機(jī)器人航向角與目標(biāo)點(diǎn)處目標(biāo)航向角的差值，選擇機(jī)器人自轉(zhuǎn)控制模式；接著，根據(jù)機(jī)器人與目標(biāo)點(diǎn)的相對距離判斷機(jī)器人定點(diǎn)跟蹤控制模式；然后，根據(jù)機(jī)器人位置和航向角及目標(biāo)點(diǎn)位置和目標(biāo)點(diǎn)處目標(biāo)航向角，計(jì)算車輪行駛速度并由驅(qū)動(dòng)電機(jī)執(zhí)行；若機(jī)器人到達(dá)最終目標(biāo)點(diǎn)，則控制結(jié)束，否則跟蹤一下目標(biāo)點(diǎn)。本方法不需要規(guī)劃路徑，設(shè)計(jì)的車速控制律與時(shí)間因素?zé)o關(guān)，便于驅(qū)動(dòng)電機(jī)實(shí)現(xiàn)計(jì)算出的車輪行駛速度，易實(shí)現(xiàn)機(jī)器人跟蹤單個(gè)目標(biāo)點(diǎn)和連續(xù)依次跟蹤多個(gè)目標(biāo)點(diǎn)。
【專利說明】
一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于差速驅(qū)動(dòng)機(jī)器人的定點(diǎn)跟蹤控制技術(shù)領(lǐng)域，特別涉及適用于兩輪、三 輪、四輪、六輪等基于差速驅(qū)動(dòng)的輪式機(jī)器人和輪式走行裝置的定點(diǎn)跟蹤控制。
【背景技術(shù)】
[0002] 差速驅(qū)動(dòng)機(jī)器人是通過2個(gè)驅(qū)動(dòng)電機(jī)控制兩側(cè)車輪行駛速度，實(shí)現(xiàn)直線或曲線運(yùn) 動(dòng)的輪式機(jī)器人，因結(jié)構(gòu)簡單、傳動(dòng)效率高和控制靈活被廣泛使用。隨著輪式機(jī)器人的智能 化程度不斷提高，這種智能機(jī)器人已被用在多個(gè)領(lǐng)域，如生產(chǎn)車間貨物運(yùn)輸、危險(xiǎn)環(huán)境勘 察、室內(nèi)清潔等。
[0003] 機(jī)器人的物品運(yùn)輸、室內(nèi)清潔等任務(wù)，可通過控制機(jī)器人跟蹤單個(gè)目標(biāo)點(diǎn)或依次 跟蹤多個(gè)目標(biāo)點(diǎn)最終到達(dá)指定目的地來完成。移動(dòng)機(jī)器人定點(diǎn)跟蹤控制問題是機(jī)器人位置 控制的基本問題，是機(jī)器人運(yùn)動(dòng)控制研究課題之一，目前機(jī)器人定點(diǎn)跟蹤控制方法有誤差 比例法、路徑規(guī)劃與路徑跟蹤法、軌跡規(guī)劃與軌跡跟蹤法。誤差比例控制法簡單地將機(jī)器人 與目標(biāo)點(diǎn)的位置偏差和航向角偏差乘以一個(gè)比例系數(shù)，來計(jì)算驅(qū)動(dòng)輪行駛速度，不適用于 差速驅(qū)動(dòng)機(jī)器人這種強(qiáng)非線性運(yùn)動(dòng)系統(tǒng)。通過規(guī)劃機(jī)器人起點(diǎn)到目標(biāo)點(diǎn)的路徑然后采用路 徑跟蹤實(shí)現(xiàn)機(jī)器人定點(diǎn)跟蹤的方法，需要設(shè)計(jì)機(jī)器人行駛路徑和路徑跟蹤控制方法。軌跡 規(guī)劃與軌跡跟蹤法通過設(shè)計(jì)輪速隨時(shí)間變化函數(shù)，實(shí)現(xiàn)機(jī)器人定點(diǎn)跟蹤的方法，對車輪速 度控制精度要求較高，當(dāng)驅(qū)動(dòng)電機(jī)出現(xiàn)速度控制誤差時(shí)，導(dǎo)致機(jī)器人不能到達(dá)目標(biāo)點(diǎn)，導(dǎo)致 定點(diǎn)跟蹤失敗。
[0004] 本發(fā)明適用的已有輪式機(jī)器人包括，圖1所示的三輪機(jī)器人、圖2所示的四輪機(jī)器 人、圖3所示的六輪機(jī)器人等基于兩輪差速驅(qū)動(dòng)行駛的多輪機(jī)器人。
[0005] 圖1中的三輪機(jī)器人包括:機(jī)器人底盤11、左驅(qū)動(dòng)輪12、右驅(qū)動(dòng)輪13、左輪驅(qū)動(dòng)電機(jī) 14、右輪驅(qū)動(dòng)電機(jī)15、控制器16、電源設(shè)備17、萬向輪18,其中左輪驅(qū)動(dòng)電機(jī)14包括轉(zhuǎn)子141 和定子142,右輪驅(qū)動(dòng)電機(jī)15又包括轉(zhuǎn)子151和定子152。圖1中：左驅(qū)動(dòng)輪12與機(jī)器人底盤11 通過左輪驅(qū)動(dòng)電機(jī)14連接，左輪驅(qū)動(dòng)電機(jī)14的轉(zhuǎn)子141與驅(qū)動(dòng)輪12固接，左輪驅(qū)動(dòng)電機(jī)14的 定子142固接于機(jī)器人底盤11;右驅(qū)動(dòng)輪13與機(jī)器人底盤11通過右輪驅(qū)動(dòng)電機(jī)15連接，右輪 驅(qū)動(dòng)電機(jī)15的轉(zhuǎn)子151與右驅(qū)動(dòng)輪13固接，右輪驅(qū)動(dòng)電機(jī)15的定子152固接于機(jī)器人底盤 11;控制器16固定在機(jī)器人底盤11上，與外界監(jiān)測系統(tǒng)進(jìn)行通信，實(shí)時(shí)獲取目標(biāo)點(diǎn)位置及目 標(biāo)點(diǎn)處目標(biāo)航向角信息與機(jī)器人位置及航向角信息，根據(jù)目標(biāo)點(diǎn)位置及目標(biāo)點(diǎn)處目標(biāo)航向 角與機(jī)器人的位置及航向角計(jì)算左驅(qū)動(dòng)輪12和右驅(qū)動(dòng)輪13的運(yùn)動(dòng)速度，并將速度指令發(fā)送 給左輪驅(qū)動(dòng)電機(jī)14和右輪驅(qū)動(dòng)電機(jī)15，然后左輪驅(qū)動(dòng)電機(jī)14和右輪驅(qū)動(dòng)電機(jī)15執(zhí)行接收到 的速度指令；電源設(shè)備17固定在機(jī)器人底盤11上，給左輪驅(qū)動(dòng)電機(jī)14和右輪驅(qū)動(dòng)電機(jī)15及 控制器16供電。萬向輪18固定在機(jī)器人底盤11的縱向?qū)ΨQ線上，可在底盤11所在平面內(nèi)轉(zhuǎn) 動(dòng)，用于支撐機(jī)器人重量。
[0006] 圖2中的三輪機(jī)器人包括:機(jī)器人底盤21、左驅(qū)動(dòng)輪22、右驅(qū)動(dòng)輪23、左輪驅(qū)動(dòng)電機(jī) 24、右輪驅(qū)動(dòng)電機(jī)25、控制器26、電源設(shè)備27、萬向輪281、萬向輪282，其中左輪驅(qū)動(dòng)電機(jī)24 包括轉(zhuǎn)子241和定子242,右輪驅(qū)動(dòng)電機(jī)25包括轉(zhuǎn)子251和定子252。圖2中：左驅(qū)動(dòng)輪22與機(jī) 器人底盤21通過左輪驅(qū)動(dòng)電機(jī)24連接，左輪驅(qū)動(dòng)電機(jī)24的轉(zhuǎn)子241與左驅(qū)動(dòng)輪22固接，左輪 驅(qū)動(dòng)電機(jī)24的定子242固接于機(jī)器人底盤21;右驅(qū)動(dòng)輪23與機(jī)器人底盤21通過右輪驅(qū)動(dòng)電 機(jī)25連接，右輪驅(qū)動(dòng)電機(jī)25的轉(zhuǎn)子251與右驅(qū)動(dòng)輪23固接，右輪驅(qū)動(dòng)電機(jī)25的定子252固接 于機(jī)器人底盤21;控制器26固定在機(jī)器人底盤21上，與外界監(jiān)測系統(tǒng)進(jìn)行通信，實(shí)時(shí)獲取目 標(biāo)點(diǎn)位置和目標(biāo)點(diǎn)處目標(biāo)航向角信息與機(jī)器人位置及航向角信息，根據(jù)目標(biāo)點(diǎn)位置及目標(biāo) 點(diǎn)處目標(biāo)航向角與機(jī)器人的位置及航向角，計(jì)算左驅(qū)動(dòng)輪22和右驅(qū)動(dòng)輪23運(yùn)動(dòng)速度，并將 速度指令發(fā)送給左輪驅(qū)動(dòng)電機(jī)24和右輪驅(qū)動(dòng)電機(jī)25,然后左輪驅(qū)動(dòng)電機(jī)24和右輪驅(qū)動(dòng)電機(jī) 25執(zhí)行接收到的速度指令；電源設(shè)備27固定在機(jī)器人底盤21上，給左輪驅(qū)動(dòng)電機(jī)24和右輪 驅(qū)動(dòng)電機(jī)25及控制器26供電。萬向輪281和萬向輪282固定在機(jī)器人底盤21的對稱線上，且 分別布置底盤21的前端和后端，萬向輪281和萬向輪282可在底盤21所在平面內(nèi)轉(zhuǎn)動(dòng)，用于 支撐機(jī)器人重量。
[0007]圖3中的三輪機(jī)器人包括:機(jī)器人底盤31、左驅(qū)動(dòng)輪32、右驅(qū)動(dòng)輪33、左輪驅(qū)動(dòng)電機(jī) 34、右輪驅(qū)動(dòng)電機(jī)35、控制器36、電源設(shè)備37、萬向輪381、萬向輪382、萬向輪383、萬向輪 384,其中左輪驅(qū)動(dòng)電機(jī)34包括轉(zhuǎn)子341和定子342,右輪驅(qū)動(dòng)點(diǎn)機(jī)35包括轉(zhuǎn)子351和定子 352。圖3中：左驅(qū)動(dòng)輪32與機(jī)器人底盤31通過左輪驅(qū)動(dòng)電機(jī)34連接，左輪驅(qū)動(dòng)電機(jī)34的轉(zhuǎn)子 341與左驅(qū)動(dòng)輪32固接，左輪驅(qū)動(dòng)電機(jī)34的定子342固接于機(jī)器人底盤31;右驅(qū)動(dòng)輪33與機(jī) 器人底盤31通過驅(qū)動(dòng)電機(jī)35連接，右輪驅(qū)動(dòng)電機(jī)35的轉(zhuǎn)子351與右驅(qū)動(dòng)輪33固接，右輪驅(qū)動(dòng) 電機(jī)35的定子352固接于機(jī)器人底盤31;控制器36固定在機(jī)器人底盤31上，與外界監(jiān)測系統(tǒng) 進(jìn)行通信獲取目標(biāo)點(diǎn)位置及目標(biāo)點(diǎn)處目標(biāo)航向角信息與機(jī)器人位置及航向角信息，根據(jù)目 標(biāo)點(diǎn)位置及目標(biāo)點(diǎn)處目標(biāo)航向角與機(jī)器人位置及航向角，計(jì)算左驅(qū)動(dòng)輪32和右驅(qū)動(dòng)輪23運(yùn) 動(dòng)速度，并將速度指令發(fā)送給左輪驅(qū)動(dòng)電機(jī)34和右輪驅(qū)動(dòng)電機(jī)35,然后左輪驅(qū)動(dòng)電機(jī)34和 右輪驅(qū)動(dòng)電機(jī)35執(zhí)行接收到的速度指令；電源設(shè)備37固定在機(jī)器人底盤31上，給左輪驅(qū)動(dòng) 電機(jī)34和右輪驅(qū)動(dòng)電機(jī)35及控制器36供電。萬向輪381、萬向輪382、萬向輪383、萬向輪384 均固定在機(jī)器人底盤31上，分別布置在底盤31的四個(gè)端點(diǎn)處，用于支撐機(jī)器人重量，全部萬 向輪可在底盤31所在平面內(nèi)轉(zhuǎn)動(dòng)。
[0008]圖1所示的萬向輪18,圖2所示的萬向輪281、萬向輪282,圖3中所示萬向輪381、萬 向輪382、萬向輪383、萬向輪384只啟平衡載荷作用，對機(jī)器人的運(yùn)動(dòng)和定點(diǎn)跟蹤控制不產(chǎn) 生影響，因此將圖1、圖2、圖3所示機(jī)器人簡化為圖4所示兩輪機(jī)器人模型。
[0009]圖4所示兩輪機(jī)器人結(jié)構(gòu)包括:機(jī)器人底盤41、左驅(qū)動(dòng)輪42、右驅(qū)動(dòng)輪43、左輪驅(qū)動(dòng) 電機(jī)44、右輪驅(qū)動(dòng)電機(jī)45,控制器46,電源設(shè)備47,其中左輪驅(qū)動(dòng)電機(jī)44包括轉(zhuǎn)子441和定子 442，右輪驅(qū)動(dòng)電機(jī)45包括轉(zhuǎn)子451和定子452。圖4中：左驅(qū)動(dòng)輪42與機(jī)器人底盤41通過左輪 驅(qū)動(dòng)電機(jī)44連接，左輪驅(qū)動(dòng)電機(jī)44的轉(zhuǎn)子441與驅(qū)動(dòng)輪42固接，左輪驅(qū)動(dòng)電機(jī)44的定子442 與機(jī)器人底盤41固接;右驅(qū)動(dòng)輪43與機(jī)器人底盤41通過右輪驅(qū)動(dòng)電機(jī)45連接，右輪驅(qū)動(dòng)電 機(jī)45的轉(zhuǎn)子451與右驅(qū)動(dòng)輪43固接，右輪驅(qū)動(dòng)電機(jī)45的定子與機(jī)器人底盤41固接;控制器46 固定在機(jī)器人底盤41上，與外界監(jiān)測系統(tǒng)進(jìn)行通信，實(shí)時(shí)獲取目標(biāo)點(diǎn)信息和機(jī)器人的位置 和航向角信息，根據(jù)獲得的目標(biāo)點(diǎn)位置及目標(biāo)點(diǎn)處目標(biāo)航向角信息與機(jī)器人的位置及航向 角，計(jì)算左驅(qū)動(dòng)輪42和右驅(qū)動(dòng)輪43運(yùn)動(dòng)速度，并將速度指令發(fā)送給左輪驅(qū)動(dòng)電機(jī)44和右輪 驅(qū)動(dòng)電機(jī)45,然后左輪驅(qū)動(dòng)電機(jī)44和右輪驅(qū)動(dòng)電機(jī)45執(zhí)行接收到的速度指令；電源設(shè)備47 固定在機(jī)器人底盤41上，給左輪驅(qū)動(dòng)電機(jī)44和右輪驅(qū)動(dòng)電機(jī)45及控制器46供電。

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

[0010] 本發(fā)明的目的是為克服已有技術(shù)的不足之處，提出一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī) 器人定點(diǎn)跟蹤控制方法，本方法便于驅(qū)動(dòng)電機(jī)實(shí)現(xiàn)計(jì)算出的車輪行駛速度，易實(shí)現(xiàn)機(jī)器人 對單個(gè)定點(diǎn)跟蹤和連續(xù)依次對多個(gè)定點(diǎn)跟蹤。
[0011] 本發(fā)明提出的一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法，其特征在 于，該方法包括以下步驟：
[0012] 1)機(jī)器人的控制器啟動(dòng)，機(jī)器人進(jìn)入定點(diǎn)跟蹤控制狀態(tài)；
[0013] 2)控制器與外界監(jiān)測系統(tǒng)實(shí)時(shí)通信，獲取當(dāng)前目標(biāo)點(diǎn)P的位置和當(dāng)前目標(biāo)點(diǎn)P處目 標(biāo)航向角t的信息，若機(jī)器人航向角9與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角t之差的絕對值小于90° 或大于270°，則進(jìn)入步驟10)，否則進(jìn)入步驟3);
[0014] 3)若機(jī)器人航向角0與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r之差的絕對值大于等于90°且 小于等于180°，則進(jìn)入步驟4);若機(jī)器人航向角0與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0 r之差的絕 對值大于180°且小于等于270°，則進(jìn)入步驟5);
[0015] 4)若機(jī)器人航向角0大于當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r，則進(jìn)入步驟6)，否則，進(jìn)入 步驟7);
[0016] 5)若機(jī)器人航向角0大于當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r，則進(jìn)入步驟8)，否則，進(jìn)入 步驟9);
[0017] 6)控制器控制機(jī)器人順時(shí)針自轉(zhuǎn)，根據(jù)公式（1)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速 度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角9與目標(biāo)點(diǎn)P處目標(biāo)航向角I 之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，當(dāng)機(jī)器人航向角9與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角I之 差小于90°時(shí)，進(jìn)入步驟10);
[0018] i ' (1)
[Vl =ks'V0
[0019] 公式（1)中：vr為機(jī)器人右驅(qū)動(dòng)輪速度，若vr>0，右驅(qū)動(dòng)輪相對機(jī)器人底盤前進(jìn)行 駛，若Vr<0，右驅(qū)動(dòng)輪相對機(jī)器人底盤后退行駛;VI為機(jī)器人左驅(qū)動(dòng)輪速度，若VI >〇，左驅(qū) 動(dòng)輪相對機(jī)器人底盤前進(jìn)行駛，若Vl<〇，左驅(qū)動(dòng)輪相對機(jī)器人底盤后退行駛;V。為用戶設(shè)定 的機(jī)器人運(yùn)動(dòng)速度，V。彡0 ;ks為機(jī)器人速度比例系數(shù)；
[0020] 7)控制器控制機(jī)器人逆時(shí)針自轉(zhuǎn)，根據(jù)公式(2)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速 度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角與目標(biāo)點(diǎn)處目標(biāo)航向角之 差;控制器控制機(jī)器人自轉(zhuǎn)過程中，若當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角I與機(jī)器人航向角9之差 小于90°，進(jìn)入步驟10); \ \
[0021] ； (2)
[vr=-^-v0
[0022] 8)控制器控制機(jī)器人逆時(shí)針自轉(zhuǎn)，根據(jù)公式(2)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪行 駛速度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角9與目標(biāo)點(diǎn)P處目標(biāo)航向 角I之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，當(dāng)機(jī)器人航向角9與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角t 之差大于270°時(shí)，進(jìn)入步驟10);
[0023] 9)控制器控制機(jī)器人順時(shí)針自轉(zhuǎn)，根據(jù)公式（1)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)行駛 速度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角9與目標(biāo)點(diǎn)P處目標(biāo)航向角 I之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，若當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角t與機(jī)器人航向角9之 差大于270°時(shí)，進(jìn)入步驟10);
[0024] 10)計(jì)算在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L， 計(jì)算公式如公式(3)所示，若L<0,則進(jìn)入步驟12);若L>0,則進(jìn)入步驟13);若L = 0且當(dāng)前 目標(biāo)點(diǎn)P與機(jī)器人兩驅(qū)動(dòng)輪輪心連線中點(diǎn)〇不重合，則進(jìn)入步驟11 )，若L = 0且當(dāng)前目標(biāo)點(diǎn)P 與機(jī)器人兩驅(qū)動(dòng)輪輪心連線中點(diǎn)〇重合，則進(jìn)入步驟15);
[0025] L= (x-Xr)cos9r+(y-yr)sin9r (3)
[0026] 公式(3)中，L為在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的 距離，x為機(jī)器人的X軸坐標(biāo)，y為機(jī)器人的Y軸坐標(biāo)， Xr為當(dāng)前目標(biāo)點(diǎn)P的X軸坐標(biāo)，yr為當(dāng)前目 標(biāo)點(diǎn)P的Y軸坐標(biāo)，為當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角；
[0027] 11)控制器采用公式(4)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪的行駛速度，控制機(jī)器人 沿當(dāng)前目標(biāo)航向角9直線前進(jìn)行駛一段距離S，S>0,然后返回步驟10);
[0_ 。⑷
[Vl^ks-Vo
[0029] 12)當(dāng)步驟(10)中采用公式(3)計(jì)算的L<0時(shí)，控制器采用前進(jìn)式定點(diǎn)跟蹤控制方 法，即機(jī)器人前進(jìn)行駛跟蹤目標(biāo)點(diǎn)的方法，根據(jù)公式(5)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速 度，控制機(jī)器人跟蹤當(dāng)前目標(biāo)點(diǎn)P;同時(shí)，控制器采用公式(3)，實(shí)時(shí)計(jì)算在當(dāng)前目標(biāo)點(diǎn)P處目 標(biāo)航向角I方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，若L<0,則繼續(xù)執(zhí)行步驟12)，若L多0， 則進(jìn)入步驟14);
[0031]公式（5)中：D為機(jī)器人左驅(qū)動(dòng)輪與右驅(qū)動(dòng)輪的輪心距離；0是機(jī)器人航向角；1^為 位置偏差增益，ki>0，k2為航向角偏差增益，k2>0;
[0032] 13)當(dāng)步驟（10)中采用公式(3)計(jì)算的L>0時(shí)，控制器采用后退式定點(diǎn)跟蹤控制方 法，即機(jī)器人后退行駛跟蹤目標(biāo)點(diǎn)的方法，根據(jù)公式(6)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)行駛 速度，控制機(jī)器人跟蹤當(dāng)前目標(biāo)點(diǎn)P;同時(shí)，控制器采用公式(3)，實(shí)時(shí)計(jì)算當(dāng)前目標(biāo)點(diǎn)P處目 標(biāo)航向角I方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，若L>0,則繼續(xù)執(zhí)行步驟13)，若LS0， 則進(jìn)入步驟14);
[0034] 14)若機(jī)器人到達(dá)當(dāng)前目標(biāo)點(diǎn)P，則進(jìn)入步驟15)，否則，返回步驟10);
[0035] 15)若當(dāng)前目標(biāo)點(diǎn)P為最終目標(biāo)點(diǎn)，則進(jìn)入步驟17)，否則，進(jìn)入步驟16);
[0036] 16)選取下一目標(biāo)點(diǎn)為新的當(dāng)前目標(biāo)點(diǎn)，重新返回步驟2);
[0037] 17)機(jī)器人到達(dá)最終目標(biāo)點(diǎn)，定點(diǎn)跟蹤控制結(jié)束。
[0038]本發(fā)明的特點(diǎn)及有益效果：
[0039] 該方法在非時(shí)間區(qū)域中，設(shè)計(jì)用于差速機(jī)器人定點(diǎn)跟蹤的輪速控制律，設(shè)計(jì)出的 車輪行駛速度與時(shí)間因素?zé)o關(guān)，擺脫了時(shí)間對速度的約束，便于驅(qū)動(dòng)電機(jī)實(shí)現(xiàn)計(jì)算出的車 輪行駛速度;定點(diǎn)跟蹤過程中，不需要規(guī)劃機(jī)器人運(yùn)動(dòng)路徑，便于實(shí)現(xiàn)機(jī)器人單個(gè)定點(diǎn)跟蹤 和連續(xù)依次對多個(gè)定點(diǎn)跟蹤。
【附圖說明】
[0040] 圖1是三輪機(jī)器人結(jié)構(gòu)示意圖。
[0041 ]圖2是四輪機(jī)器人機(jī)構(gòu)示意圖。
[0042]圖3是六輪機(jī)器人結(jié)構(gòu)示意圖。
[0043] 圖4是結(jié)構(gòu)簡化后的兩輪機(jī)器人結(jié)構(gòu)示意圖。
[0044] 圖5是本發(fā)明的兩輪機(jī)器人定點(diǎn)跟蹤控制方法實(shí)施流程示意圖。
[0045] 圖6是本實(shí)施例中前進(jìn)式定點(diǎn)跟蹤示意圖。
[0046] 圖7是本實(shí)施例中后退式定點(diǎn)跟蹤示意圖。
[0047] 圖8是本實(shí)施例機(jī)器人多定點(diǎn)連續(xù)跟蹤效果示意圖。
【具體實(shí)施方式】
[0048] 本發(fā)明提出的一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法，下面結(jié)合附 圖及具體實(shí)施例進(jìn)一步說明如下：
[0049] 本發(fā)明提出的一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法實(shí)施例，用于 兩輪機(jī)器人的定點(diǎn)跟蹤控制。該兩輪機(jī)器人定點(diǎn)跟蹤控制方法的實(shí)施流程如圖5所示，包括 以下步驟：
[0050] 1)機(jī)器人的控制器啟動(dòng)，機(jī)器人進(jìn)入定點(diǎn)跟蹤控制狀態(tài)；
[0051 ] 2)控制器與外界監(jiān)測系統(tǒng)實(shí)時(shí)通信，獲取當(dāng)前目標(biāo)點(diǎn)P的位置和當(dāng)前目標(biāo)點(diǎn)P處目 標(biāo)航向角t的信息，若機(jī)器人航向角9與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角t之差的絕對值小于90° 或大于270°，則進(jìn)入步驟10)，否則進(jìn)入步驟3);
[0052] 3)若機(jī)器人航向角0與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r之差的絕對值大于等于90°且 小于等于180°，則進(jìn)入步驟4);若機(jī)器人航向角0與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0 r之差的絕 對值大于180°且小于等于270°，則進(jìn)入步驟5);
[0053] 4)若機(jī)器人航向角0大于當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r，則進(jìn)入步驟6)，否則，進(jìn)入 步驟7);
[0054] 5)若機(jī)器人航向角0大于當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r，則進(jìn)入步驟8)，否則，進(jìn)入 步驟9);
[0055] 6)控制器控制機(jī)器人順時(shí)針自轉(zhuǎn)，根據(jù)公式（1)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速 度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角9與目標(biāo)點(diǎn)P處目標(biāo)航向角I 之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，當(dāng)機(jī)器人航向角9與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角I之 差小于90°時(shí)，進(jìn)入步驟10); fv,. =-A?v0
[0056] , ⑴
[0057] 公式（1)中：Vr為機(jī)器人右驅(qū)動(dòng)輪速度，若Vr> 0，右驅(qū)動(dòng)輪相對機(jī)器人底盤前進(jìn)行 駛，若vr<〇，右驅(qū)動(dòng)輪相對機(jī)器人底盤后退行駛;vi為機(jī)器人左驅(qū)動(dòng)輪速度，若vi>〇，左驅(qū) 動(dòng)輪相對機(jī)器人底盤前進(jìn)行駛，若vi<〇，左驅(qū)動(dòng)輪相對機(jī)器人底盤后退行駛;V。為用戶設(shè)定 的機(jī)器人運(yùn)動(dòng)速度，V。多0 ; ks為機(jī)器人速度比例系數(shù)，建議ks的取值范圍為(0，1 ];
[0058] 7)控制器控制機(jī)器人逆時(shí)針自轉(zhuǎn)，根據(jù)公式(2)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速 度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角與目標(biāo)點(diǎn)處目標(biāo)航向角之 差;控制器控制機(jī)器人自轉(zhuǎn)過程中，若當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角I與機(jī)器人航向角9之差 小于90°時(shí)，進(jìn)入步驟10);
[_卜:" ⑵
[0060] 公式(2)中各符號定義與公式（1)相同；
[0061] 8)控制器控制機(jī)器人逆時(shí)針自轉(zhuǎn)，根據(jù)公式(2)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪行 駛速度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角9與目標(biāo)點(diǎn)P處目標(biāo)航向 角I之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，當(dāng)機(jī)器人航向角9與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角t 之差大于270°時(shí)，進(jìn)入步驟10);
[0062] 9)控制器控制機(jī)器人順時(shí)針自轉(zhuǎn)，根據(jù)公式（1)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)行駛 速度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角9與目標(biāo)點(diǎn)P處目標(biāo)航向角 I之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，若當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角t與機(jī)器人航向角9之 差大于270°時(shí)，進(jìn)入步驟10);
[0063] 10)計(jì)算在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L， 計(jì)算公式如公式(3)所示，若L<0,則進(jìn)入步驟12);若L>0,則進(jìn)入步驟13);若L = 0且當(dāng)前 目標(biāo)點(diǎn)P與機(jī)器人兩驅(qū)動(dòng)輪輪心連線中點(diǎn)〇不重合(如圖6中，當(dāng)前目標(biāo)點(diǎn)P69在左驅(qū)動(dòng)輪63 和右驅(qū)動(dòng)輪64的輪心連線上，但當(dāng)前目標(biāo)點(diǎn)P69與兩輪心連線中點(diǎn)〇不重合的情況；如圖7 中，當(dāng)前目標(biāo)點(diǎn)P79在左驅(qū)動(dòng)輪73和右驅(qū)動(dòng)輪74的輪心連線上，但當(dāng)前目標(biāo)點(diǎn)P79與兩輪心 連線中點(diǎn)〇不重合的情況），則進(jìn)入步驟11)，若L = 0且當(dāng)前目標(biāo)點(diǎn)P與機(jī)器人兩驅(qū)動(dòng)輪輪心 連線中點(diǎn)〇重合(如圖6中，當(dāng)前目標(biāo)點(diǎn)P69在左驅(qū)動(dòng)輪63和右驅(qū)動(dòng)輪64的輪心連線上，且與 兩輪心連線中點(diǎn)〇重合的情況；圖7中，當(dāng)前目標(biāo)點(diǎn)P79在左驅(qū)動(dòng)輪73和右驅(qū)動(dòng)輪74的輪心連 線上，且與兩輪心連線中點(diǎn)〇重合的情況），則進(jìn)入步驟15);
[0064] L= (x-Xr)cos9r+(y-yr)sin9r (3)
[0065] 公式(3)中，L為在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的 距離，x為機(jī)器人的X軸坐標(biāo)，y為機(jī)器人的Y軸坐標(biāo)， Xr為當(dāng)前目標(biāo)點(diǎn)P的X軸坐標(biāo)，yr為當(dāng)前目 標(biāo)點(diǎn)P的Y軸坐標(biāo)，為當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角；
[0066] 11)控制器采用公式(4)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪的行駛速度，控制機(jī)器人 沿當(dāng)前目標(biāo)航向角9直線前進(jìn)行駛一段距離S，S>0,（建議S彡0.1米），然后返回步驟10); \vr -ks-V〇.
[0067] / (4)
[0068] 公式(4)中各符號定義同公式(1)、（2);
[0069] 12)當(dāng)步驟(10)中采用公式(3)計(jì)算的L<0時(shí)，控制器采用前進(jìn)式定點(diǎn)跟蹤控制方 法（即機(jī)器人前進(jìn)行駛跟蹤目標(biāo)點(diǎn)的方法），根據(jù)公式(5)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速 度，控制機(jī)器人跟蹤當(dāng)前目標(biāo)點(diǎn)P;同時(shí)，控制器采用公式(3)，實(shí)時(shí)計(jì)算在當(dāng)前目標(biāo)點(diǎn)P處目 標(biāo)航向角I方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，若L<0,則繼續(xù)執(zhí)行步驟12)，若L多0， 則進(jìn)入步驟14);
[0071] 公式(5)中：D為機(jī)器人左驅(qū)動(dòng)輪與右驅(qū)動(dòng)輪的輪心距離；0是機(jī)器人航向角;1^為 位置偏差增益為航向角偏差增益，k2>0;其他符號定義與公式（1)、（2)、（3)、（4) 相同；
[0072] 本實(shí)施例步驟12)中，機(jī)器人控制器與外界監(jiān)測系統(tǒng)實(shí)時(shí)通信，實(shí)時(shí)獲取當(dāng)前目標(biāo) 點(diǎn)信息及機(jī)器人位置和航向角信息，并采用公式(5)計(jì)算驅(qū)動(dòng)輪的行駛速度，并將速度指令 信息發(fā)送給車輪驅(qū)動(dòng)電機(jī)，驅(qū)動(dòng)電機(jī)執(zhí)行接收到的速度指令。
[0073]前進(jìn)式定點(diǎn)跟蹤如圖6所示。圖6中：〇點(diǎn)為機(jī)器人62左驅(qū)動(dòng)輪63與右驅(qū)動(dòng)輪64的輪 心連線中點(diǎn)；x、y分別為〇點(diǎn)在坐標(biāo)系61的X軸坐標(biāo)和Y軸坐標(biāo);0是機(jī)器人航向角，為沿逆時(shí) 針方向上，坐標(biāo)系X軸到機(jī)器人62行駛方向的夾角；x r、yr分別為當(dāng)前目標(biāo)點(diǎn)P點(diǎn)69在坐標(biāo)系 61中的X軸坐標(biāo)、Y軸坐標(biāo);0r為當(dāng)前目標(biāo)P點(diǎn)69的航向角；
[0074]公式(5)中，^為控制器67計(jì)算出的左驅(qū)動(dòng)輪63行駛速度，控制器67將該速度指令 發(fā)送給左輪驅(qū)動(dòng)電機(jī)65,左輪驅(qū)動(dòng)電機(jī)65執(zhí)行該速度指令;vr為控制器67計(jì)算出的右驅(qū)動(dòng) 輪64行駛速度，控制器67將該速度指令發(fā)送給右輪驅(qū)動(dòng)電機(jī)66,右輪驅(qū)動(dòng)電機(jī)66執(zhí)行該速 度指令;v。為使用者設(shè)定的機(jī)器人62的行駛速度，即機(jī)器人62〇點(diǎn)的運(yùn)動(dòng)速度，〇點(diǎn)為機(jī)器人 62左驅(qū)動(dòng)輪63與右驅(qū)動(dòng)輪64的輪心連線中心;D為機(jī)器人62的左驅(qū)動(dòng)輪63與右驅(qū)動(dòng)輪64的 輪心距離;x為機(jī)器人62〇點(diǎn)的X軸坐標(biāo);y為機(jī)器人62〇點(diǎn)的Y軸坐標(biāo)；0是機(jī)器人航向角，為逆 時(shí)針方向上，坐標(biāo)系61的X軸到機(jī)器人62行駛方向的夾角;P點(diǎn)69為當(dāng)前目標(biāo)點(diǎn)；x r是P點(diǎn)69 的X軸坐標(biāo);yr為P點(diǎn)69的Y軸坐標(biāo)；9r為當(dāng)前目標(biāo)點(diǎn)P點(diǎn)69的航向角;ki為距離偏差增益，ki> 0，根據(jù)經(jīng)驗(yàn)建議h的取值范圍為[0.1，30]; k2為航向角偏差增益，k2>0，根據(jù)經(jīng)驗(yàn)建議k2的 取值范圍為[0.1，50]。
[0075] 13)當(dāng)步驟（10)中采用公式(3)計(jì)算的L>0時(shí)，控制器采用后退式定點(diǎn)跟蹤控制方 法，即機(jī)器人后退行駛跟蹤目標(biāo)點(diǎn)的方法，，根據(jù)公式(6)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)行駛 速度，控制機(jī)器人跟蹤當(dāng)前目標(biāo)點(diǎn)P;同時(shí)，控制器采用公式(3)，實(shí)時(shí)計(jì)算當(dāng)前目標(biāo)點(diǎn)P處目 標(biāo)航向角L方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，若L>0,則繼續(xù)執(zhí)行步驟13)，若LS0， 則進(jìn)入步驟14);
[0077] 公式(6)中各符號定義與公式(1)、（2)、（3)、（4)、（5)相同；
[0078] 本實(shí)施例步驟13)中，機(jī)器人控制器與外界監(jiān)測系統(tǒng)實(shí)時(shí)通信，實(shí)時(shí)獲取當(dāng)前目標(biāo) 點(diǎn)位置及目標(biāo)點(diǎn)處目標(biāo)航向角信息與機(jī)器人位置和航向角信息，并采用公式(6)計(jì)算驅(qū)動(dòng) 輪的行駛速度，并將速度指令信息發(fā)送給車輪驅(qū)動(dòng)電機(jī)，驅(qū)動(dòng)電機(jī)執(zhí)行接收到的速度指令。 [0079]后退式定點(diǎn)跟蹤如圖7所示。圖7中 ：〇點(diǎn)為機(jī)器人72左驅(qū)動(dòng)輪73與右驅(qū)動(dòng)輪74的輪 心連線中點(diǎn)；x、y分別為〇點(diǎn)在坐標(biāo)系71的X軸坐標(biāo)和Y軸坐標(biāo);0是機(jī)器人航向角，為沿逆時(shí) 針方向上，坐標(biāo)系X軸到機(jī)器人72行駛方向的夾角; Xr、yr分別為當(dāng)前目標(biāo)點(diǎn)P點(diǎn)79在坐標(biāo)系 71中的X軸坐標(biāo)、Y軸坐標(biāo);0r為當(dāng)前目標(biāo)P點(diǎn)79的航向角；
[0080] 公式(6)中，^為控制器77計(jì)算出的左驅(qū)動(dòng)輪73行駛速度，控制器77將該速度指令 發(fā)送給左輪驅(qū)動(dòng)電機(jī)75,左輪驅(qū)動(dòng)電機(jī)75執(zhí)行該速度指令;v r為控制器77計(jì)算出的右驅(qū)動(dòng) 輪74行駛速度，控制器77將該速度指令發(fā)送給右輪驅(qū)動(dòng)電機(jī)76,右輪驅(qū)動(dòng)電機(jī)76執(zhí)行該速 度指令;v。為使用者設(shè)定的機(jī)器人72的行駛速度，即機(jī)器人72〇點(diǎn)的運(yùn)動(dòng)速度，〇點(diǎn)為機(jī)器人 72左驅(qū)動(dòng)輪73與右驅(qū)動(dòng)輪74的輪心連線中心;D為機(jī)器人72的左驅(qū)動(dòng)輪73與右驅(qū)動(dòng)輪74的 輪心距離;x為機(jī)器人72〇點(diǎn)的X軸坐標(biāo);y為機(jī)器人72〇點(diǎn)的Y軸坐標(biāo);0是機(jī)器人航向角，為逆 時(shí)針方向上，坐標(biāo)系71的X軸到機(jī)器人72行駛方向的夾角;P點(diǎn)79為當(dāng)前目標(biāo)點(diǎn)； Xr是P點(diǎn)79 的X軸坐標(biāo);yr為P點(diǎn)79的Y軸坐標(biāo);0r為當(dāng)前目標(biāo)點(diǎn)P點(diǎn)79的航向角;h為位置偏差增益， 0，根據(jù)經(jīng)驗(yàn)建議h的取值范圍為[0.1，30]; k2為航向角偏差增益，k2>0，根據(jù)經(jīng)驗(yàn)建議k2的 取值范圍為[0.1，50]。
[0081] 14)若機(jī)器人到達(dá)當(dāng)前目標(biāo)點(diǎn)P，則進(jìn)入步驟15)，否則，返回步驟10);
[0082] 15)若當(dāng)前目標(biāo)點(diǎn)P為最終目標(biāo)點(diǎn)，則進(jìn)入步驟17)，否則，進(jìn)入步驟16);
[0083] 16)選取下一目標(biāo)點(diǎn)為新的當(dāng)前目標(biāo)點(diǎn)，重新返回步驟2);
[0084] 17)機(jī)器人到達(dá)最終目標(biāo)點(diǎn)，定點(diǎn)跟蹤控制結(jié)束。
[0085] 上述方法同樣可用于三輪、四輪、六輪機(jī)器人定點(diǎn)跟蹤控制。用于如圖1所示三輪 機(jī)器人時(shí)，控制器16只需采用上述方法，計(jì)算左驅(qū)動(dòng)輪12和右驅(qū)動(dòng)輪13的行駛速度，并將速 度指令分別發(fā)送給左輪驅(qū)動(dòng)電機(jī)14和右輪驅(qū)動(dòng)電機(jī)15,左輪驅(qū)動(dòng)電機(jī)14與右輪驅(qū)動(dòng)電機(jī)15 執(zhí)行接收到的速度指令，完成該三輪機(jī)器人的定點(diǎn)跟蹤任務(wù)，對萬向輪18不做任何控制。用 于如圖2所示的四輪機(jī)器人，控制器26只需采用上述方法，計(jì)算左驅(qū)動(dòng)輪22和右驅(qū)動(dòng)輪23的 行駛速度，并將速度指令分別發(fā)送給左輪驅(qū)動(dòng)電機(jī)24和右輪驅(qū)動(dòng)電機(jī)25,左輪驅(qū)動(dòng)電機(jī)24 與右輪驅(qū)動(dòng)電機(jī)25執(zhí)行接收到的速度指令，完成該四輪機(jī)器人的定點(diǎn)跟蹤任務(wù)，對萬向輪 281和萬向輪282不做任何控制。用于如圖3中所示的六輪機(jī)器人，控制器36只需采用上述流 程，計(jì)算左驅(qū)動(dòng)輪32和右驅(qū)動(dòng)輪33的行駛速度，并將速度指令分別發(fā)給左輪驅(qū)動(dòng)電機(jī)34和 右輪驅(qū)動(dòng)電機(jī)35,左輪驅(qū)動(dòng)電機(jī)34和右輪驅(qū)動(dòng)電機(jī)35執(zhí)行接收的速度指令，完成六輪機(jī)器 人的定點(diǎn)跟蹤任務(wù)，對萬向輪381、萬向輪382、萬向輪383及萬向輪384不做任何控制。
[0086] 選擇任意一條路線，選取路線上的一系列點(diǎn)，采用上述定點(diǎn)跟蹤控制方法進(jìn)行跟 蹤控制，定點(diǎn)跟蹤仿真結(jié)果如圖8所示。圖8中，控制器87控制機(jī)器人82沿建筑物80圍成的道 路81到達(dá)目的地88;圖8中的五角星為控制器87通過與外界監(jiān)測系統(tǒng)通信獲取的道路81中 心線上的多個(gè)點(diǎn)，控制器87通過控制機(jī)器人82依次跟蹤這些標(biāo)示點(diǎn)最終到達(dá)目的地88。在 跟蹤圖8所示五角星時(shí)，控制器87根據(jù)上述定點(diǎn)跟蹤控制方法計(jì)算左驅(qū)動(dòng)輪83和右驅(qū)動(dòng)輪 84的行駛速度，并由左輪驅(qū)動(dòng)電機(jī)85和右輪驅(qū)動(dòng)電機(jī)86執(zhí)行計(jì)算出的速度，控制機(jī)器人依 次跟蹤圖8五角星標(biāo)示的定點(diǎn)，最終到達(dá)目標(biāo)點(diǎn)88,整個(gè)過程中機(jī)器人82的實(shí)際運(yùn)動(dòng)軌跡如 圖8中實(shí)線89所示。由仿真結(jié)果圖8可知，本發(fā)明提出的差速機(jī)器人定點(diǎn)跟蹤方法，可以實(shí)現(xiàn) 單個(gè)定點(diǎn)跟蹤或多個(gè)定點(diǎn)連續(xù)跟蹤，使機(jī)器人最終到達(dá)目標(biāo)地。
【主權(quán)項(xiàng)】
1. 一種非時(shí)間參考的差速驅(qū)動(dòng)機(jī)器人定點(diǎn)跟蹤控制方法，該方法包括以下步驟： 1) 機(jī)器人的控制器啟動(dòng)，機(jī)器人進(jìn)入定點(diǎn)跟蹤控制狀態(tài)； 2) 控制器與外界監(jiān)測系統(tǒng)實(shí)時(shí)通信，獲取當(dāng)前目標(biāo)點(diǎn)P的位置和當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航 向角的信息，若機(jī)器人航向角Θ與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0 r之差的絕對值小于90°或大 于270°，則進(jìn)入步驟10)，否則進(jìn)入步驟3); 3) 若機(jī)器人航向角Θ與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角差的絕對值大于等于90°且小于 等于180°，則進(jìn)入步驟4);若機(jī)器人航向角Θ與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r之差的絕對值 大于180°且小于等于270°，則進(jìn)入步驟5); 4) 若機(jī)器人航向角Θ大于當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r，則進(jìn)入步驟6)，否則，進(jìn)入步驟 7)； 5) 若機(jī)器人航向角Θ大于當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r，則進(jìn)入步驟8)，否則，進(jìn)入步驟 9)； 6) 控制器控制機(jī)器人順時(shí)針自轉(zhuǎn)，根據(jù)公式（1)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速度， 同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角Θ與目標(biāo)點(diǎn)P處目標(biāo)航向角1之 差;控制器控制機(jī)器人自轉(zhuǎn)過程中，當(dāng)機(jī)器人航向角Θ與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角θ τ之差 小于90°時(shí)，進(jìn)入步驟10); ⑴ I V/ ^ks-Vo 公式（1 )中：Vr為機(jī)器人右驅(qū)動(dòng)輪速度，若Vr > 〇，右驅(qū)動(dòng)輪相對機(jī)器人底盤前進(jìn)行駛，若 Vr <0，右驅(qū)動(dòng)輪相對機(jī)器人底盤后退行駛;Vl為機(jī)器人左驅(qū)動(dòng)輪速度，若Vl >0，左驅(qū)動(dòng)輪相 對機(jī)器人底盤前進(jìn)行駛，若Vl<〇，左驅(qū)動(dòng)輪相對機(jī)器人底盤后退行駛;V。為用戶設(shè)定的機(jī)器 人運(yùn)動(dòng)速度，V。多〇;k s為機(jī)器人速度比例系數(shù)； 7) 控制器控制機(jī)器人逆時(shí)針自轉(zhuǎn)，根據(jù)公式(2)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速度， 同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角與目標(biāo)點(diǎn)處目標(biāo)航向角之差;控 制器控制機(jī)器人自轉(zhuǎn)過程中，若當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0 1與機(jī)器人航向角Θ之差小于 90°，進(jìn)入步驟10); \ν··-κ·ν0 ⑵ Iv/ 二-夂 _ vO 8) 控制器控制機(jī)器人逆時(shí)針自轉(zhuǎn)，根據(jù)公式(2)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪行駛速 度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角Θ與目標(biāo)點(diǎn)P處目標(biāo)航向角0 r 之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，當(dāng)機(jī)器人航向角Θ與當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角 差大于270°時(shí)，進(jìn)入步驟10); 9) 控制器控制機(jī)器人順時(shí)針自轉(zhuǎn)，根據(jù)公式（1)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)行駛速 度，同時(shí)，實(shí)時(shí)與外界監(jiān)測系統(tǒng)通信，獲取并計(jì)算機(jī)器人航向角Θ與目標(biāo)點(diǎn)P處目標(biāo)航向角0 r 之差;控制器控制機(jī)器人自轉(zhuǎn)過程中，若當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角91與機(jī)器人航向角Θ之 差大于270°，進(jìn)入步驟10); 10) 計(jì)算在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角0r方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，計(jì)算公 式如公式(3)所示，若L<0,則進(jìn)入步驟12);若L>0,則進(jìn)入步驟13);若L = O且當(dāng)前目標(biāo)點(diǎn)P 與機(jī)器人兩驅(qū)動(dòng)輪輪心連線中點(diǎn)0不重合，則進(jìn)入步驟11)，若L=O且當(dāng)前目標(biāo)點(diǎn)P與機(jī)器人 兩驅(qū)動(dòng)輪輪心連線中點(diǎn)〇重合，則進(jìn)入步驟15); (3) 公式(3)中，L為在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角θτ方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離，X 為機(jī)器人的X軸坐標(biāo)，y為機(jī)器人的Y軸坐標(biāo)，Xr為當(dāng)前目標(biāo)點(diǎn)P的X軸坐標(biāo)，yr為當(dāng)前目標(biāo)點(diǎn)P 的γ軸坐標(biāo)，Or為當(dāng)前目標(biāo)點(diǎn)P處目標(biāo)航向角； 11) 控制器采用公式(4)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪的行駛速度，控制機(jī)器人沿當(dāng) 前目標(biāo)航向角Θ直線前進(jìn)行駛一段距離S，S>0,然后返回步驟10);12) 當(dāng)步驟（10)中采用公式(3)計(jì)算的L<0時(shí)，控制器采用前進(jìn)式定點(diǎn)跟蹤控制方法， 即機(jī)器人前進(jìn)行駛跟蹤目標(biāo)點(diǎn)的方法，根據(jù)公式(5)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)輪速度， 控制機(jī)器人跟蹤當(dāng)前目標(biāo)點(diǎn)P;同時(shí)，控制器采用公式(3)，實(shí)時(shí)計(jì)算在當(dāng)前目標(biāo)點(diǎn)P處目標(biāo) 航向角方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，若L<0,則繼續(xù)執(zhí)行步驟12)，若L^OJlJ 進(jìn)入步驟14);公式(5)中:D為機(jī)器人左驅(qū)動(dòng)輪與右驅(qū)動(dòng)輪的輪心距離;Θ是機(jī)器人航向角;1^為位置偏 差增益，ki>0，k2為航向角偏差增益，k2>0; 13) 當(dāng)步驟（10)中采用公式(3)計(jì)算的L>0時(shí)，控制器采用后退式定點(diǎn)跟蹤控制方法， 即機(jī)器人后退行駛跟蹤目標(biāo)點(diǎn)的方法，根據(jù)公式（6)計(jì)算機(jī)器人左驅(qū)動(dòng)輪、右驅(qū)動(dòng)行駛速 度，控制機(jī)器人跟蹤當(dāng)前目標(biāo)點(diǎn)P;同時(shí)，控制器采用公式(3)，實(shí)時(shí)計(jì)算當(dāng)前目標(biāo)點(diǎn)P處目標(biāo) 航向角方向上，機(jī)器人與當(dāng)前目標(biāo)點(diǎn)P的距離L，若L>0,則繼續(xù)執(zhí)行步驟13)，若LSOJlJ 進(jìn)入步驟14);14) 若機(jī)器人到達(dá)當(dāng)前目標(biāo)點(diǎn)P，則進(jìn)入步驟15)，否則，返回步驟10); 15) 若當(dāng)前目標(biāo)點(diǎn)P為最終目標(biāo)點(diǎn)，則進(jìn)入步驟17)，否則，進(jìn)入步驟16); 16) 選取下一目標(biāo)點(diǎn)為新的當(dāng)前目標(biāo)點(diǎn)，重新返回步驟2); 17) 機(jī)器人到達(dá)最終目標(biāo)點(diǎn)，定點(diǎn)跟蹤控制結(jié)束。
【文檔編號】G05D1/02GK105892459SQ201610204701
【公開日】2016年8月24日
【申請日】2016年4月1日
【發(fā)明人】李紅, 王文軍, 李克強(qiáng)
【申請人】清華大學(xué)