專利名稱：用于移動(dòng)單元的軌道跟蹤控制系統(tǒng)與方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種用于移動(dòng)單元的軌道跟蹤控制系統(tǒng)與方法，特別涉及 用于控制機(jī)器人的軌道跟蹤的方法和系統(tǒng)。
背景技術(shù)：
已經(jīng)提出了一種這樣的技術(shù)其控制例如車輛和機(jī)器人等移動(dòng)單元，以便自動(dòng)跟蹤目標(biāo)路徑。例如，存在這樣的技術(shù)其通過進(jìn)行移動(dòng)單元位 置橫向偏離量以及移動(dòng)單元方位角偏離量的反饋控制，進(jìn)行移動(dòng)單元的自動(dòng)軌道跟蹤。然而，當(dāng)僅進(jìn)行這樣的反饋控制時(shí)，移動(dòng)單元對(duì)于曲率變化 的響應(yīng)可能被延遲，因此，移動(dòng)單元難以跟蹤包括具有多種曲率的曲線的 目標(biāo)路徑。為了解決這一問題，JP-A-2002-215239提出了一種技術(shù)，其中，移動(dòng) 單元的操航角基于通過移動(dòng)單元橫向位置偏離與目標(biāo)路徑曲率的前饋控制 設(shè)置的操作量以及通過相對(duì)于目標(biāo)路徑的方位角偏離與移動(dòng)單元橫向位置 偏離的反饋控制設(shè)置的操作量來確定。根據(jù)這種技術(shù)，移動(dòng)單元可準(zhǔn)確跟 蹤具有多種曲率的目標(biāo)路徑。然而，才艮據(jù)JP-A-20020215239所述的這種多艮蹤控制纟支術(shù)，目標(biāo)路徑的 曲率被直接引入控制循環(huán)，因此，移動(dòng)單元的行為直接反映目標(biāo)路徑曲率 的不連續(xù)變化，結(jié)果，移動(dòng)單元的移動(dòng)變得不穩(wěn)定。另外，由于移動(dòng)單元 總是受到前饋控制，當(dāng)移動(dòng)單元大大偏離目標(biāo)路徑時(shí)，前饋控制提供的操 作量作為反饋控制提供的操作量的干擾，結(jié)果，軌道跟蹤的準(zhǔn)確性劣化。發(fā)明內(nèi)容本發(fā)明的目的在于提供一種軌道跟蹤控制系統(tǒng)以及方法，其使得移動(dòng) 單元能夠準(zhǔn)確跟蹤目標(biāo)路徑。本發(fā)明第 一 實(shí)施形態(tài)涉及一種軌道跟蹤控制系統(tǒng)，其控制移動(dòng)單元來跟蹤目標(biāo)路徑。軌道跟蹤控制系統(tǒng)包含曲率計(jì)算部分，其將目標(biāo)路徑近 似為弧并計(jì)算弧的曲率；前饋控制部分，其根據(jù)曲率計(jì)算部分計(jì)算得到的 曲率設(shè)置移動(dòng)單元的前饋控制。曲率計(jì)算部分根據(jù)移動(dòng)單元的移動(dòng)速度計(jì) 算曲率。根據(jù)這種結(jié)構(gòu)，移動(dòng)單元可準(zhǔn)確跟蹤目標(biāo)路徑。在上述軌道多艮蹤控制系統(tǒng)中，例如，曲率計(jì)算部分可根據(jù)移動(dòng)單元的 移動(dòng)速度和控制周期計(jì)算曲率。特別地，根據(jù)控制周期計(jì)算曲率使得移動(dòng) 單元能以適用于移動(dòng)單元實(shí)際操作的控制量單位受到控制，因此，改進(jìn)了 移動(dòng)單元的跟蹤運(yùn)動(dòng)的準(zhǔn)確度。另外，在上述軌道跟蹤控制系統(tǒng)中，曲率計(jì)算部分可包含最近點(diǎn)設(shè) 置部分，其在目標(biāo)路徑上設(shè)置最接近于移動(dòng)單元的點(diǎn)；鄰近點(diǎn)設(shè)置部分， 其在目標(biāo)路徑上設(shè)置在由最近點(diǎn)設(shè)置部分設(shè)置的最近點(diǎn)之前的鄰近點(diǎn)以及 在由最近點(diǎn)設(shè)置部分設(shè)置的最近點(diǎn)之后的鄰近點(diǎn)；弧設(shè)置部分，其設(shè)置通 過由最近點(diǎn)設(shè)置部分設(shè)置的最近點(diǎn)以及由鄰近點(diǎn)設(shè)置部分設(shè)置的鄰近點(diǎn)的 弧。才艮據(jù)此結(jié)構(gòu)，曲率的計(jì)算能夠得到簡(jiǎn)化。在上面介紹的結(jié)構(gòu)中，鄰近點(diǎn)設(shè)置單元可根據(jù)移動(dòng)單元的移動(dòng)速度確 定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離。在這種情況下，移動(dòng)單元可受到控制， 以便以適合于其移動(dòng)速度的方式跟蹤目標(biāo)路徑。另外，鄰近點(diǎn)設(shè)置部分可根據(jù)移動(dòng)單元的移動(dòng)速度和控制周期設(shè)置最 近點(diǎn)與各個(gè)鄰近點(diǎn)之間的距離。在這種情況下，特別地，根據(jù)控制周期確 定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離使得移動(dòng)單元能以適用于移動(dòng)單元實(shí)際 操作的控制量單位受到控制，因此改進(jìn)移動(dòng)單元跟蹤運(yùn)動(dòng)的準(zhǔn)確度。例如，鄰近點(diǎn)設(shè)置部分可根據(jù)VAT確定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距 離L，其中，V為移動(dòng)單元的移動(dòng)速度，AT為移動(dòng)單元的控制周期。另外，當(dāng)移動(dòng)單元相對(duì)于目標(biāo)路徑的位置誤差量和移動(dòng)單元相對(duì)于目 標(biāo)路徑的定向誤差量中的至少一個(gè)大于預(yù)定量時(shí)，前饋控制部分可停止前 饋控制。根據(jù)這種結(jié)構(gòu)，軌道跟蹤的準(zhǔn)確度得到改進(jìn)。同時(shí)，移動(dòng)單元的 優(yōu)選實(shí)例之一為機(jī)器人。本發(fā)明的第二實(shí)施形態(tài)涉及軌道跟蹤控制方法，其用于控制移動(dòng)單元以跟蹤目標(biāo)路徑。該方法包含將目標(biāo)路徑近似為弧的步驟；根據(jù)移動(dòng)單 元的移動(dòng)速度計(jì)算弧的曲率的步驟；根據(jù)計(jì)算得到的曲率設(shè)置移動(dòng)單元的 控制的步驟。在計(jì)算曲率的步驟中，曲率根據(jù)移動(dòng)單元的移動(dòng)速度來計(jì)算。 根據(jù)該方法，移動(dòng)單元可準(zhǔn)確跟蹤目標(biāo)路徑。在上述軌道跟蹤控制方法中，曲率可根據(jù)移動(dòng)單元的移動(dòng)速度和控制 周期來計(jì)算。在這種情況下，特別地，根據(jù)控制周期計(jì)算曲率使得移動(dòng)單 元能以適用于移動(dòng)單元實(shí)際操作的控制量單位受到控制，因此，改進(jìn)了移 動(dòng)單元跟蹤運(yùn)動(dòng)的準(zhǔn)確度。計(jì)算曲率的步驟可包括在目標(biāo)路徑上設(shè)置最接近于移動(dòng)單元的點(diǎn)的步 驟，在目標(biāo)路徑上設(shè)置最近點(diǎn)前的鄰近點(diǎn)以及最近點(diǎn)后的鄰近點(diǎn)的步驟； 設(shè)置經(jīng)過最近點(diǎn)與鄰近點(diǎn)延伸的弧的步驟。這些步驟簡(jiǎn)化了曲率的計(jì)算。在上面介紹的方法中，最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離可根據(jù)移動(dòng)單 元的移動(dòng)速度來確定。在這種情況下，移動(dòng)單元可受到控制，以便以適合 于其移動(dòng)速度的方式跟蹤目標(biāo)路徑。另外，最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離可根據(jù)移動(dòng)單元的移動(dòng)速度以 及控制周期來確定。在這種情況下，特別地，4艮據(jù)控制周期確定最近點(diǎn)與 每個(gè)鄰近點(diǎn)之間的距離使得移動(dòng)單元能夠以適用于移動(dòng)單元實(shí)際操作的控 制量單位受到控制，因此，移動(dòng)單元的跟蹤運(yùn)動(dòng)的準(zhǔn)確度得到改進(jìn)。例如，最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離L可才艮據(jù)VAT確定，其中，V 為移動(dòng)單元的移動(dòng)速度，AT為移動(dòng)單元的控制周期。另外，根據(jù)曲率"^殳置的移動(dòng)單元的控制可在移動(dòng)單元相對(duì)于目標(biāo)路徑 的位置誤差量以及移動(dòng)單元相對(duì)于目標(biāo)路徑的定向誤差量中的至少一個(gè)大 于預(yù)定量時(shí)停止。根據(jù)該方法，軌道跟蹤的準(zhǔn)確度得到改進(jìn)。同時(shí)，移動(dòng) 單元的優(yōu)選實(shí)例之一為機(jī)器人。本發(fā)明的第三實(shí)施形態(tài)涉及用于控制移動(dòng)單元以跟蹤目標(biāo)路徑的軌道 跟蹤控制程序。軌道跟蹤控制程序包含使得計(jì)算機(jī)執(zhí)行將目標(biāo)路徑近似 為弧以及計(jì)算弧的曲率的步驟；使得計(jì)算機(jī)執(zhí)行根據(jù)計(jì)算得到的曲率設(shè)置 移動(dòng)單元的控制的步驟。根據(jù)此程序，移動(dòng)單元可準(zhǔn)確跟蹤目標(biāo)路徑。如上所述，根據(jù)本發(fā)明相應(yīng)的實(shí)施形態(tài)的目標(biāo)跟蹤控制系統(tǒng)、方法和 程序使得移動(dòng)單元能夠準(zhǔn)確地跟蹤目標(biāo)路徑。


參照附圖，通過閱讀對(duì)優(yōu)選實(shí)施例的介紹，可以明了本發(fā)明的前述以 及其他目的、特征和優(yōu)點(diǎn)，其中，相同的符號(hào)用于表示相同的元件，其中圖1示出了表示由才艮據(jù)本發(fā)明的示例性實(shí)施例的軌道跟蹤控制系統(tǒng)控 制的機(jī)器人的移動(dòng)路徑以及機(jī)器人的目標(biāo)路徑的網(wǎng)格坐標(biāo)系統(tǒng)；圖2示出了由根據(jù)本發(fā)明示例性實(shí)施例的軌道跟蹤控制系統(tǒng)使用的控 制參數(shù)之間的幾何關(guān)系；圖3為一流程圖，其示出了由根據(jù)本發(fā)明的示例性實(shí)施例的軌道跟蹤 控制系統(tǒng)執(zhí)行的過程的流程；圖4為一流程圖，其示出了根據(jù)本發(fā)明的示例性實(shí)施例的軌道跟蹤控 制系統(tǒng)執(zhí)行的處理的流程；圖5示出了由才艮據(jù)本發(fā)明的示例性實(shí)施例的軌道跟蹤控制系統(tǒng)執(zhí)行的 最近點(diǎn)的搜索；圖6示出了根據(jù)本發(fā)明的示例性實(shí)施例的軌道跟蹤控制系統(tǒng)執(zhí)行的最 近點(diǎn)的搜索；圖7示出了由根據(jù)本發(fā)明的示例性實(shí)施例的軌道跟蹤控制系統(tǒng)執(zhí)行的 弧近似；圖8示出了由根據(jù)本發(fā)明示例性實(shí)施例的軌道跟蹤控制系統(tǒng)執(zhí)行的處理。
具體實(shí)施方式
才艮據(jù)本發(fā)明此示例性實(shí)施例的軌道跟蹤控制系統(tǒng)被配置為控制作為移 動(dòng)單元的機(jī)器人的軌道跟蹤。這種軌道跟蹤控制系統(tǒng)由例如安裝在機(jī)器人 中的計(jì)算機(jī)或與機(jī)器人分開提供的計(jì)算機(jī)構(gòu)成。計(jì)算機(jī)包含例如CPU (中 央處理單元)、ROM (只讀存儲(chǔ)器)、RAM (隨機(jī)訪問存儲(chǔ)器)、次記 錄裝置(sub-recording device)(例如硬盤驅(qū)動(dòng)器)、插入可運(yùn)送記錄介 質(zhì)的可運(yùn)送記錄裝置(例如CD-ROM驅(qū)動(dòng)器)、輸入部分、輸出部分。 應(yīng)用程序被存儲(chǔ)在ROM、次記錄裝置、可運(yùn)送記錄裝置中，每個(gè)應(yīng)用程 序在被執(zhí)行時(shí)被裝栽到RAM中并與操作系統(tǒng)協(xié)作，以便向CPU等提供多 種指令。應(yīng)用程序包含用于實(shí)現(xiàn)此示例性實(shí)施例的軌道跟蹤控制系統(tǒng)的特 定計(jì)算機(jī)程序。當(dāng)由軌道跟蹤控制系統(tǒng)進(jìn)行軌道控制時(shí)，應(yīng)用程序凈皮裝載 到RAM中，應(yīng)用程序的處理于是在讀取存儲(chǔ)在次記錄裝置中的多種數(shù)據(jù) 以及將多種數(shù)據(jù)記錄到同 一裝置的同時(shí)被執(zhí)行。作為機(jī)器人的目標(biāo)軌道的目標(biāo)路徑常常被定義為坐標(biāo)集，坐標(biāo)被繪制 在網(wǎng)格坐標(biāo)系統(tǒng)中，如圖1所示，其中，離散坐標(biāo)以網(wǎng)格形式被布置。使 用網(wǎng)格坐標(biāo)系統(tǒng)的優(yōu)點(diǎn)是可容易地搜索最短路徑，且到目的地的可靠路徑 能被搜索。然而，這樣的網(wǎng)格坐標(biāo)系統(tǒng)所定義的目標(biāo)路徑是包含彼此連接 的直線且因此不光滑的路徑，故難以使機(jī)器人跟蹤目標(biāo)路徑。為了對(duì)之進(jìn) 行處理，作為可由機(jī)器人跟蹤的光滑路徑，彎曲路徑可通過將曲線近似方 法等等用于已經(jīng)確定的離散路徑坐標(biāo)來獲得。圖1中的實(shí)線所示為由自由形狀曲線描述的目標(biāo)路徑100。下面，將 介紹如圖2所示配置的機(jī)器人1跟蹤目標(biāo)路徑100的情況。機(jī)器人1具有 兩個(gè)輪ll、 12，每個(gè)輪ll、 12的^走轉(zhuǎn)速度受到致動(dòng)器——例如電動(dòng)機(jī)—— 的控制。關(guān)于機(jī)器人l，建立用跟蹤公式表示的控制定理。 公式(1) <formula>formula see original document page 10</formula>其中，V,為左輪ll的移動(dòng)速度； Vr為右輪12的移動(dòng)速度； V為機(jī)器人l (托架)的切向速度； d為左輪ll與右輪12之間的距離； R為到最近點(diǎn)的曲率半徑； ye,為相對(duì)于目標(biāo)路徑的位置誤差； 6 m，為相對(duì)于目標(biāo)路徑的定向誤差； ky為來自位置誤差的反饋增益；ke為來自定向誤差的反饋增益。在上面的公式中，"l-(d/2R)"以及"l+(d/R)"為前饋項(xiàng)，"kyyerr。r+k e6em)r"為反饋項(xiàng)。前饋項(xiàng)用于控制機(jī)器人l的輪ll、 12，使得機(jī)器人l 跟蹤在機(jī)器人轉(zhuǎn)向的假設(shè)下推定的曲線以畫出具有特定曲率的曲線，反饋 項(xiàng)用于控制機(jī)器人1，以便消除相對(duì)于目標(biāo)路徑的位置與定向誤差。圖1所示圓在目標(biāo)路徑上的任意點(diǎn)A、 B上分別具有曲率半徑RA、 RB 以及曲率中心Oa、 Ob。曲率半徑Ra、 Rb用于公式(I)，以進(jìn)行機(jī)器人 l的軌道跟蹤控制。下面，將使用圖3所示的流程圖介紹本發(fā)明的此示例性實(shí)施例中的軌 道跟蹤控制的流程。開始，目標(biāo)路徑上最接近于機(jī)器人1的點(diǎn)被搜索 (S101)。盡管可通過多種方法來進(jìn)行最近點(diǎn)的這種搜索，此示例性實(shí)施 例4吏用分割法(splitting method )。最近點(diǎn)的搜索將在圖4、圖5、圖6中詳細(xì)示出。參照?qǐng)D5，在目標(biāo)路 徑100上的最近點(diǎn)在機(jī)器人1位于點(diǎn)A時(shí)被搜索的情況下，獲得節(jié)點(diǎn)n的坐標(biāo)與點(diǎn)A之間的距離(S201)。于是，判斷所獲得的距離是否是至此已 經(jīng)獲得的點(diǎn)A與各節(jié)點(diǎn)坐標(biāo)之間距離中最短的(S202 )。如果在步驟S202 中判斷為所獲得的到節(jié)點(diǎn)n的距離是最短的，節(jié)點(diǎn)i^皮記錄(S203)。另 一方面，如果在S202中判斷為所獲得的到節(jié)點(diǎn)n的距離不是最短的，或 者在S203中節(jié)點(diǎn)n被記錄之后，判斷是否已經(jīng)獲得到所有節(jié)點(diǎn)的距離 (S204)。如果在S204中判斷為尚未獲得到所有節(jié)點(diǎn)的距離，則對(duì)于節(jié)點(diǎn)n+l 進(jìn)行S201。于是，如果在S204中判斷為已經(jīng)獲得到所有節(jié)點(diǎn)的距離，鄰 近最接近于機(jī)器人l的節(jié)點(diǎn)(最近節(jié)點(diǎn))的兩個(gè)節(jié)點(diǎn)中與另一個(gè)相比更接 近于最近節(jié)點(diǎn)的 一個(gè)被選擇為鄰近節(jié)點(diǎn)，搜索區(qū)域被設(shè)置為最近節(jié)點(diǎn)與鄰 近節(jié)點(diǎn)之間的區(qū)域(S206)。于是，如圖6所示，限定搜索區(qū)域的這兩個(gè) 節(jié)點(diǎn)之間的中間點(diǎn)被設(shè)置為起始點(diǎn)Po，搜索區(qū)域被設(shè)置為相對(duì)于作為其中 心的起始點(diǎn)P。延伸節(jié)點(diǎn)n與節(jié)點(diǎn)n+l之間距離L的一半的搜索區(qū)域wl(5207) 。在圖6所示實(shí)例中，搜索區(qū)域wl被設(shè)置為在節(jié)點(diǎn)n側(cè)離起始 點(diǎn)Po為(1/2 )wl即(1/4 )L的點(diǎn)PL1與在節(jié)點(diǎn)n+l側(cè)距離起始點(diǎn)P。為(1/2 ) wl即(1/4) L的點(diǎn)Pm之間的區(qū)域。于是，關(guān)于起始點(diǎn)Po，判斷作為n-(1/2 ) wl的節(jié)點(diǎn)PL1以及作為n+ (1/2 ) wl的節(jié)點(diǎn)Pw哪個(gè)更接近于點(diǎn)A(5208) 。下面，在S208中被判斷為更接近于點(diǎn)A的節(jié)點(diǎn)被設(shè)置為近側(cè) 點(diǎn)(proximal point) P，搜索區(qū)域于是被設(shè)置為搜索區(qū)域w2，其相對(duì)于作 為其中心的最接近點(diǎn)P延伸區(qū)域wl的長(zhǎng)度的一半(S209)，用新設(shè)置的 搜索區(qū)域w2進(jìn)行S208,其后重復(fù)同樣的過程。當(dāng)此過程達(dá)到收斂時(shí)，此 時(shí)被確定為最接近于點(diǎn)A的節(jié)點(diǎn)的近側(cè)點(diǎn)P被設(shè)置為最近點(diǎn)，并獲得其坐 標(biāo)?；氐綀D3所示的流程圖，在已經(jīng)如上所述地確定最近點(diǎn)之后，基于機(jī) 器人l的移動(dòng)速度設(shè)置最近點(diǎn)前后的點(diǎn)(S102)。當(dāng)最近點(diǎn)是圖7中的點(diǎn) F時(shí)，設(shè)置分別在目標(biāo)路徑上位于點(diǎn)F前后的點(diǎn)E、 G。點(diǎn)E與點(diǎn)F之間的距離以及點(diǎn)F與點(diǎn)G之間的距離彼此相等，此距 離L被獲得為L(zhǎng)-VAT，其中，V為機(jī)器人l的移動(dòng)速度，AT為控制周
期。也就是說，距離L直接與移動(dòng)速度V以及控制周期厶T成比例。當(dāng)點(diǎn) E、 F、 G已經(jīng)如此i殳置時(shí)，可以唯一地確定通過這三個(gè)點(diǎn)的圓。下面，弧的半徑R通過使用點(diǎn)E、 F、 G即最近點(diǎn)以及最近點(diǎn)前后的 點(diǎn)解圓的公式來計(jì)算(S103)。圓的公式作為軌道。公式(2)x2+y2+ax+by+c=0如果點(diǎn)E、 F、 G的坐標(biāo)分別為E (xq， y。) 、 F (Xl， yi) 、 G (x2， y2)，使用這些坐標(biāo)獲得軌道模擬公式(3)。 公式(3)<formula>formula see original document page 12</formula>
在上面所示的公式(3)中，未知的a、 b、 c通過在公式左側(cè)使用算術(shù) 方法一一例如較低與較高三角形矩陣分解一一解逆矩陣來計(jì)算。于是，使 用計(jì)算得到的未知a、 b、 c，獲得作為軌道的相對(duì)于點(diǎn)A的曲率以及曲率 中心O (x0， y0)。<formula>formula see original document page 12</formula>下面，公式(1)中的反饋項(xiàng)使用機(jī)器人1相對(duì)于最近點(diǎn)F的位置誤 差以及定向誤差來計(jì)算(S104)。于是，公式(1)中的反饋項(xiàng)使用在S103中計(jì)算的弧的半徑R來計(jì)算(5105) 。于是，判斷位置誤差與定向誤差各自的量是否小于或等于預(yù)定誤差量(5106) 。如果判斷為各個(gè)誤差的量小于或等于預(yù)定誤差量，反饋控制與 前饋控制均被執(zhí)行(S107)。也就是說，左右輪ll、 12的移動(dòng)速度使用已 經(jīng)在S104中計(jì)算的反饋項(xiàng)的值以及在S105中計(jì)算的前饋項(xiàng)的值來確定， 控制于是根據(jù)左右輪ll、 12的所確定的移動(dòng)速度來進(jìn)行。另一方面，如果在S106中判斷為位置誤差量與定向誤差量二者或任一 個(gè)超過預(yù)定誤差量，僅進(jìn)行反饋控制(S108)，也就是說，不進(jìn)行前饋控 制。此時(shí)，左右輪ll、 12的移動(dòng)速度僅使用已經(jīng)在S104中計(jì)算的反饋項(xiàng) 的值來確定，控制于是根據(jù)左右輪ll、 12的所確定的移動(dòng)速度來進(jìn)行。如上所述，在此示例性實(shí)施例中，前饋控制的執(zhí)行限制在機(jī)器人l的 軌道跟蹤誤差量小于或等于預(yù)定誤差量的狀態(tài)下。在機(jī)器人1與目標(biāo)路徑 之間的距離與圖8所示實(shí)例中一樣大的情況下，如果對(duì)機(jī)器人1進(jìn)行前饋 控制，在機(jī)器人在目標(biāo)路徑上移動(dòng)的假設(shè)下進(jìn)行計(jì)算，因此，來自前饋控 制的輸入干擾了對(duì)機(jī)器人l的軌道跟蹤中的誤差進(jìn)行校正的反饋控制，結(jié) 果，軌道跟蹤的準(zhǔn)確性劣化。為了抵消這一點(diǎn)，在此示例性實(shí)施例中，當(dāng) 目標(biāo)路徑與機(jī)器人l之間的距離大時(shí)，也就是說，當(dāng)軌道跟蹤中的誤差量大于預(yù)定誤差量時(shí)，僅進(jìn)行反饋控制，以便控制機(jī)器人1的軌道跟蹤，也 就是說，不進(jìn)行前饋控制。例如，當(dāng)機(jī)器人在目標(biāo)方向相反的方向移動(dòng)時(shí)， 停止前饋控制。另外，在本發(fā)明的此示例性實(shí)施例中，在目標(biāo)路徑的弧近似中，根據(jù) 機(jī)器人l的移動(dòng)速度來設(shè)置目標(biāo)路徑的每個(gè)曲率半徑，使得曲率半徑隨著 機(jī)器人1的移動(dòng)速度增大而增大，并使曲率半徑隨著機(jī)器人1的移動(dòng)速度 減小而減小。具體而言，在示例性實(shí)施例中，目標(biāo)路徑被確定為當(dāng)機(jī)器 人1以高速移動(dòng)時(shí)，機(jī)器人1在目標(biāo)路徑小單位指向機(jī)器人急劇轉(zhuǎn)向的點(diǎn) 上溫和轉(zhuǎn)向，當(dāng)機(jī)器人以低速移動(dòng)時(shí)，機(jī)器人l在目標(biāo)路徑小單位指向的 點(diǎn)上急劇轉(zhuǎn)向。以這種方式，機(jī)器人l受到控制，以便準(zhǔn)確跟蹤目標(biāo)路徑。在此示例性實(shí)施例中，最近點(diǎn)與鄰近最近點(diǎn)的每個(gè)點(diǎn)之間的距離被確 定為I^VAT。在這種情況下，通過將機(jī)器人l的移動(dòng)速度V與控制周期 AT相乘獲得的值表示根據(jù)控制周期AT機(jī)器人1的移動(dòng)方向和移動(dòng)速度 不能被改變的時(shí)間段，因此，該值對(duì)應(yīng)于機(jī)器人l的移動(dòng)方向和移動(dòng)速度 不能被改變的距離。因此，如果目標(biāo)路徑使用短于機(jī)器人1的移動(dòng)方向與
移動(dòng)速度不能被改變的上述距離的單位被近似為弧，不可能如目標(biāo)路徑的 這種小單位所指向地控制機(jī)器人1的移動(dòng)。然而，當(dāng)最近點(diǎn)與鄰近其的每個(gè)點(diǎn)之間的距離被如上所述地確定為L(zhǎng)=VAT時(shí)，機(jī)器人1不受在短于機(jī) 器人l的控制周期的時(shí)間段內(nèi)發(fā)生的目標(biāo)路徑曲率變化的影響，因此，機(jī) 器人1的軌道跟蹤控制可準(zhǔn)確進(jìn)行。盡管最近點(diǎn)與鄰近其的每個(gè)點(diǎn)之間的距離在上述示例性實(shí)施例中被確 定為I^VAT,將同樣的距離設(shè)置為大于L-VAT AA夠的。例如，其可 通過一般的表達(dá)式例如L-aVAT+b U， b-常數(shù))確定。 (其他示例性實(shí)施例)盡管根據(jù)本發(fā)明的軌道跟蹤控制系統(tǒng)在上述示例性實(shí)施例中被應(yīng)用到 具有兩個(gè)輪的機(jī)器人，其可作為替代地應(yīng)用到例如具有四個(gè)輪的機(jī)器人、 具有一個(gè)輪的機(jī)器人、具有兩條腿的機(jī)器人、具有四條腿的機(jī)器人等。在 行走的機(jī)器人的情況下，機(jī)器人可得到控制，以便通過控制機(jī)器人的每一 步改變移動(dòng)方向跟蹤目標(biāo)路徑。另外，本發(fā)明可適用于多種車輛，例如兩 輪車、四輪車以及機(jī)器人。盡管參照被認(rèn)為是本發(fā)明的優(yōu)選實(shí)施例對(duì)本發(fā)明進(jìn)行了介紹，可以明 了，本發(fā)明不限于所公開的實(shí)施例或構(gòu)造。相反，本發(fā)明旨在覆蓋多種變 型和等效布置。另外，盡管所公開的發(fā)明的多種元素以多種組合和構(gòu)造示 出，其是示例性的，包括更多、更少或僅僅一個(gè)元素的其他組合和構(gòu)造也 屬于本發(fā)明的范圍。
權(quán)利要求
1. 一種軌道跟蹤控制系統(tǒng)，其控制移動(dòng)單元以跟蹤目標(biāo)路徑，該系統(tǒng)包含曲率計(jì)算部分，其將目標(biāo)路徑近似為弧并計(jì)算弧的曲率；以及前饋控制部分，其根據(jù)由曲率計(jì)算部分計(jì)算得到的曲率設(shè)置移動(dòng)單元的前饋控制，其中，曲率計(jì)算部分根據(jù)移動(dòng)單元的移動(dòng)速度計(jì)算曲率。
2. 根據(jù)權(quán)利要求l的軌道跟蹤控制系統(tǒng)，其中，曲率計(jì)算部分才艮據(jù)移動(dòng)單元的控制周期來確定曲率。
3. 根據(jù)權(quán)利要求1或2的軌道跟蹤控制系統(tǒng)，其中， 曲率計(jì)算部分包含最近點(diǎn)設(shè)置部分，其在目標(biāo)路徑上設(shè)置最接近于移動(dòng)單元的點(diǎn)； 鄰近點(diǎn)設(shè)置部分，其在目標(biāo)路徑上設(shè)置在由最近點(diǎn)設(shè)置部分設(shè)置的最近點(diǎn)之前的鄰近點(diǎn)以及在由最近點(diǎn)設(shè)置部分設(shè)置的最近點(diǎn)之后的鄰近點(diǎn)；以及弧設(shè)置部分，其設(shè)置經(jīng)過由最近點(diǎn)設(shè)置部分設(shè)置的最近點(diǎn)與由鄰近點(diǎn) 設(shè)置部分設(shè)置的鄰近點(diǎn)延伸的弧。
4. 根據(jù)權(quán)利要求3的軌道跟蹤控制系統(tǒng)，其中，鄰近點(diǎn)設(shè)置部分根據(jù)移動(dòng)單元的移動(dòng)速度確定最近點(diǎn)與每個(gè)鄰近點(diǎn)之 間的距離。
5. 根據(jù)權(quán)利要求3的軌道跟蹤控制系統(tǒng)，其中，鄰近點(diǎn)設(shè)置部分根據(jù)移動(dòng)單元的移動(dòng)速度和控制周期設(shè)置最近點(diǎn)與每 個(gè)鄰近點(diǎn)之間的距離。
6. 根據(jù)權(quán)利要求3的軌道跟蹤控制系統(tǒng)，其中，鄰近點(diǎn)設(shè)置部分根據(jù)VAT確定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離L，其 中，V為移動(dòng)單元的移動(dòng)速度，AT為移動(dòng)單元的控制周期。
7. 根據(jù)權(quán)利要求l-6中任意一項(xiàng)的軌道跟蹤控制系統(tǒng)，其中， 在移動(dòng)單元相對(duì)于目標(biāo)路徑的位置誤差量和移動(dòng)單元相對(duì)于目標(biāo)路徑 的定向誤差量中的至少一個(gè)大于預(yù)定量時(shí)，前饋控制部分停止前饋控制。
8. 根據(jù)權(quán)利要求l-7中的任何一項(xiàng)的軌道跟蹤控制系統(tǒng)，其還包含 反饋控制部分，其執(zhí)行反饋控制，以便減小移動(dòng)單元相對(duì)于目標(biāo)路徑的位置誤差和定向誤差中的至少一個(gè)。
9. 根據(jù)權(quán)利要求8的軌道3M宗控制系統(tǒng)，其中， 在前饋控制停止時(shí)僅執(zhí)行反饋控制。
10. 根據(jù)權(quán)利要求l-9中任意一項(xiàng)的軌道跟蹤控制系統(tǒng)，其中，移動(dòng)單 元為機(jī)器人。
11. 一種軌道跟蹤控制方法，其用于控制移動(dòng)單元以跟蹤目標(biāo)路徑， 該方法包含將目標(biāo)路徑近似為??；根據(jù)移動(dòng)單元的移動(dòng)速度計(jì)算弧的曲率；以及 根據(jù)計(jì)算得到的曲率設(shè)置移動(dòng)單元的控制。
12. 根據(jù)權(quán)利要求ll的軌道跟蹤控制方法，其中， 根據(jù)移動(dòng)單元的控制周期來計(jì)算曲率。
13. 根據(jù)權(quán)利要求11或12的軌道跟蹤控制方法，其中， 曲率的計(jì)算包括在目標(biāo)路徑上^L置最接近于移動(dòng)單元的點(diǎn)；在目標(biāo)路徑上i殳置在最近點(diǎn)之前的鄰近點(diǎn)以及在最近點(diǎn)之后的鄰近點(diǎn)；設(shè)置經(jīng)過最近點(diǎn)與鄰近點(diǎn)延伸的弧。
14. 根據(jù)權(quán)利要求13的軌道跟蹤控制方法，其中， 根據(jù)移動(dòng)單元的移動(dòng)速度來確定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離。
15. 根據(jù)權(quán)利要求13的軌道跟蹤控制方法，其中， 根據(jù)移動(dòng)單元的移動(dòng)速度以及控制周期來確定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的5巨離。
16. 根據(jù)權(quán)利要求13的軌道跟蹤控制方法，其中，才艮據(jù)VAT來確定最近點(diǎn)與每個(gè)鄰近點(diǎn)之間的距離L，其中，V為移動(dòng) 單元的移動(dòng)速度，AT為移動(dòng)單元的控制周期。
17. 根據(jù)權(quán)利要求11-16中任意一項(xiàng)的軌道跟蹤控制方法，其包含 設(shè)置移動(dòng)單元的控制，以便減小移動(dòng)單元相對(duì)于目標(biāo)路徑的位置誤差與定向誤差中的至少一個(gè)。
18. 根據(jù)權(quán)利要求11-17中任意一項(xiàng)的軌道跟蹤控制方法，其中， 根據(jù)曲率設(shè)置的移動(dòng)單元的控制在移動(dòng)單元相對(duì)于目標(biāo)路徑的位置誤差量以及移動(dòng)單元相對(duì)于目標(biāo)路徑的定向誤差量中的至少一個(gè)大于預(yù)定量 時(shí)停止。
19. 根據(jù)權(quán)利要求11-18中任意一項(xiàng)的軌道跟蹤控制方法，其中， 移動(dòng)單元為機(jī)器人。
20. —種控制移動(dòng)單元以跟蹤目標(biāo)路徑的軌道跟蹤控制程序，其包含: 使得計(jì)算機(jī)執(zhí)行以下步驟的指令將目標(biāo)路徑近似為弧，根據(jù)移動(dòng)單元的移動(dòng)速度計(jì)算弧的曲率；以及使得計(jì)算機(jī)執(zhí)行以下步驟的指令根據(jù)計(jì)算得到的曲率設(shè)置移動(dòng)單元 的控制。
全文摘要
根據(jù)本發(fā)明的一種軌道跟蹤控制系統(tǒng)控制機(jī)器人(1)以跟蹤目標(biāo)路徑(100)。在此系統(tǒng)中，將目標(biāo)路徑(100)近似為弧，計(jì)算弧的曲率。此時(shí)，根據(jù)機(jī)器人(1)的移動(dòng)速度計(jì)算曲率。于是，根據(jù)所計(jì)算的曲率設(shè)置機(jī)器人(1)的控制。
文檔編號(hào)G05D1/02GK101401047SQ200780008896
公開日2009年4月1日 申請(qǐng)日期2007年3月9日 優(yōu)先權(quán)日2006年3月14日
發(fā)明者朝原佳昭, 森健光, 美馬一博, 藪下英典 申請(qǐng)人:豐田自動(dòng)車株式會(huì)社