一種機(jī)器人小車(chē)路徑跟隨方法
【專(zhuān)利摘要】一種機(jī)器人小車(chē)路徑跟隨方法，屬于信息處理技術(shù)領(lǐng)域。機(jī)器人小車(chē)以圓弧路徑向目標(biāo)跟隨點(diǎn)運(yùn)動(dòng)；機(jī)器人小車(chē)的運(yùn)動(dòng)速度與機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離成正比。本發(fā)明的優(yōu)點(diǎn)是:實(shí)現(xiàn)了機(jī)器人小車(chē)相對(duì)被跟隨者的實(shí)時(shí)跟隨；跟隨算法高效實(shí)用；減小機(jī)器人小車(chē)的抖動(dòng)，使得跟隨運(yùn)動(dòng)平滑自然。
【專(zhuān)利說(shuō)明】
一種機(jī)器人小車(chē)路徑跟隨方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種機(jī)器人小車(chē)路徑跟隨方法，屬于信息處理技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 跟隨機(jī)器人可以幫助人們解決實(shí)際生活中的很多問(wèn)題，比如自主跟隨攝影攝像、 跟隨購(gòu)物、跟隨領(lǐng)路等，因此跟隨機(jī)器人正成為機(jī)器人發(fā)展的一大趨勢(shì)。若想實(shí)現(xiàn)機(jī)器人對(duì) 人的跟隨，需要解決機(jī)器人從當(dāng)前位置如何運(yùn)動(dòng)到被跟隨者所在位置的問(wèn)題，即點(diǎn)到點(diǎn)的 路徑跟隨問(wèn)題。機(jī)器人小車(chē)從一個(gè)點(diǎn)運(yùn)動(dòng)到另一點(diǎn)可采取直線(xiàn)路徑進(jìn)行跟隨，即機(jī)器人小 車(chē)先轉(zhuǎn)向到朝向被跟隨者的方向，然后再以直線(xiàn)運(yùn)動(dòng)到目標(biāo)位置。由于被跟隨者的運(yùn)動(dòng)具 有隨意性，采用簡(jiǎn)單的直線(xiàn)跟隨，雖然路徑最短，但機(jī)器人小車(chē)轉(zhuǎn)向比較急促，容易出現(xiàn)抖 動(dòng)現(xiàn)象，且運(yùn)動(dòng)不夠平滑。

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

[0003] 為了克服現(xiàn)有技術(shù)的不足，本發(fā)明提供一種機(jī)器人小車(chē)路徑跟隨方法，以避免機(jī) 器人小車(chē)跟隨過(guò)程中的抖動(dòng)現(xiàn)象，并提高跟隨動(dòng)作的平滑性。
[0004] -種機(jī)器人小車(chē)路徑跟隨方法，含有以下步驟;機(jī)器人小車(chē)以圓弧路徑向目標(biāo)跟 隨點(diǎn)運(yùn)動(dòng);機(jī)器人小車(chē)的運(yùn)動(dòng)速度與機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離成正比。
[0005] 如圖2所示，具體步驟如下：
[0006] 步驟一:機(jī)器人小車(chē)通過(guò)傳感器測(cè)距并對(duì)目標(biāo)進(jìn)行定位，從而計(jì)算目標(biāo)位于機(jī)器 人小車(chē)坐標(biāo)系中的位置(Ρ，θ)。
[0007] 步驟二：將當(dāng)前所測(cè)得的角度值Θ，與上一次的角度值0pre作比較，如果0-0pre> Θ threshold ($口 Θ threshold - 10 )，令θ - 0pre~l~ Δ θ ( Δ θ - 3 );如果 9_9p：re〈_9 threshold ($口 ^threshold - 10°)，則讓?duì)染徛兓?，令?= ΘΡΜ-Δ θ(如Δ θ = 3).從而實(shí)現(xiàn)角度的緩慢變化，從而達(dá)到速度 平滑。
[0008] 步驟三:根據(jù)(ρ，Θ)和跟隨算法，計(jì)算兩個(gè)驅(qū)動(dòng)輪的速度，從而機(jī)器人小車(chē)對(duì)目標(biāo) 的跟隨。
[0009] 跟隨算法為：
[0010] 若被跟隨者位于D處，則機(jī)器人小車(chē)將沿著虛線(xiàn)圓弧軌跡到達(dá)D處，且其跟隨速度 與P成正比;P為機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離。
[0011] 具體的，建立如圖1所示的機(jī)器人坐標(biāo)系，坐標(biāo)原點(diǎn)位于后輪中心(AB中心），y軸方 向?yàn)樾≤?chē)前進(jìn)方向，X軸方向?yàn)楹筝喌妮S向方向。被跟隨者D在機(jī)器人坐標(biāo)系內(nèi)的坐標(biāo)為(X， y)，則
[0014]假設(shè)左輪的速度為VI，右輪的速度為V2，兩后輪之間的間距為d，移動(dòng)小車(chē)的角速度 為ω，轉(zhuǎn)彎半徑為r (沿x軸正向?yàn)檎?，沿x軸負(fù)向?yàn)樨?fù)值），由此可得：
[0024] f(p)可根據(jù)實(shí)際運(yùn)動(dòng)效果選取，設(shè)f(p)=kp，即可根據(jù)實(shí)際運(yùn)動(dòng)調(diào)節(jié)k值，以達(dá)到 平滑運(yùn)動(dòng)的目的。
[0025] 步驟四:保存當(dāng)前的角度值ΘΡΜ = Θ;重新回到步驟一。
[0026]本發(fā)明的優(yōu)點(diǎn)是：
[0027] 1、實(shí)現(xiàn)了機(jī)器人小車(chē)相對(duì)被跟隨者的實(shí)時(shí)跟隨；
[0028] 2、跟隨算法高效實(shí)用；
[0029] 3、減小機(jī)器人小車(chē)的抖動(dòng)，使得跟隨運(yùn)動(dòng)平滑自然。
【附圖說(shuō)明】
[0030] 當(dāng)結(jié)合附圖考慮時(shí)，通過(guò)參照下面的詳細(xì)描述，能夠更完整更好地理解本發(fā)明以 及容易得知其中許多伴隨的優(yōu)點(diǎn)，但此處所說(shuō)明的附圖用來(lái)提供對(duì)本發(fā)明的進(jìn)一步理解， 構(gòu)成本發(fā)明的一部分，本發(fā)明的示意性實(shí)施例及其說(shuō)明用于解釋本發(fā)明，并不構(gòu)成對(duì)本發(fā) 明的不當(dāng)限定，如圖其中：
[0031] 圖1為本發(fā)明的機(jī)器人小車(chē)路徑跟隨示意圖。
[0032] 圖2為本發(fā)明的路徑跟隨算法流程圖。
[0033]下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明進(jìn)一步說(shuō)明。
【具體實(shí)施方式】
[0034]顯然，本領(lǐng)域技術(shù)人員基于本發(fā)明的宗旨所做的許多修改和變化屬于本發(fā)明的保 護(hù)范圍。
[0035]本技術(shù)領(lǐng)域技術(shù)人員可以理解，除非另外定義，這里使用的所有術(shù)語(yǔ)(包括技術(shù)術(shù) 語(yǔ)和科學(xué)術(shù)語(yǔ))具有與本發(fā)明所屬領(lǐng)域中的普通技術(shù)人員的一般理解相同的意義。還應(yīng)該 理解的是，諸如通用字典中定義的那些術(shù)語(yǔ)應(yīng)該被理解為具有與現(xiàn)有技術(shù)的上下文中的意 義一致的意義，并且除非像這里一樣定義，不會(huì)用理想化或過(guò)于正式的含義來(lái)解釋。
[0036]為便于對(duì)本發(fā)明實(shí)施例的理解，下面將結(jié)合附圖以幾個(gè)具體實(shí)施例為例做進(jìn)一步 的解釋說(shuō)明，且各個(gè)實(shí)施例并不構(gòu)成對(duì)本發(fā)明實(shí)施例的限定。
[0037]實(shí)施例1:如圖1、圖2所示，一種機(jī)器人小車(chē)路徑跟隨方法，第一超聲波模塊A和第 二超聲波模塊B分別安裝在機(jī)器人小車(chē)的兩側(cè)。
[0038] 若被跟隨者位于D處，則機(jī)器人小車(chē)將沿著虛線(xiàn)圓弧軌跡到達(dá)D處，且其跟隨速度 與P成正比;P為機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離。
[0039] 具體的，建立如圖1所示的機(jī)器人坐標(biāo)系，坐標(biāo)原點(diǎn)位于后輪中心(AB中心），y軸方 向?yàn)樾≤?chē)前進(jìn)方向，X軸方向?yàn)楹筝喌妮S向方向。被跟隨者D在機(jī)器人坐標(biāo)系內(nèi)的坐標(biāo)為(X， y)，則：
[0042] -種機(jī)器人小車(chē)路徑跟隨方法，如圖2所示，含有以下步驟；
[0043] 步驟一:機(jī)器人小車(chē)通過(guò)傳感器測(cè)距并對(duì)目標(biāo)進(jìn)行定位，從而計(jì)算目標(biāo)位于機(jī)器 人小車(chē)坐標(biāo)系中的位置(Ρ，θ)。
[0044] 步驟二：將當(dāng)前所測(cè)得的角度值Θ，與上一次的角度值0pre作比較，如果0-0pre> Θ threshold (如IΘ threshold - 10 )，令θ - 9pre+ Δ Θ (如| Δ θ - 3 );如果 0_9pre〈_9 threshold (如 ^threshold - 10°)，則讓?duì)ň徛兓瞀?= ΘΡΜ-Δ Θ(如Δ θ = 3).從而實(shí)現(xiàn)角度的緩慢變化，從而達(dá)到速度 平滑。
[0045] 步驟三:根據(jù)(P，Θ)和跟隨算法，計(jì)算兩個(gè)驅(qū)動(dòng)輪的速度，從而機(jī)器人小車(chē)對(duì)目標(biāo) 的跟隨。
[0046]跟隨算法為：
[0047] 若被跟隨者位于D處，則機(jī)器人小車(chē)將沿著虛線(xiàn)圓弧軌跡到達(dá)D處，且其跟隨速度 與P成正比;P為機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離。
[0048]具體的，建立如圖1所示的機(jī)器人坐標(biāo)系，坐標(biāo)原點(diǎn)位于后輪中心(AB中心），y軸方 向?yàn)樾≤?chē)前進(jìn)方向，X軸方向?yàn)楹筝喌妮S向方向。
[0049]假設(shè)左輪的速度為vi，右輪的速度為V2，兩后輪之間的間距為d，移動(dòng)小車(chē)的角速度 為ω，轉(zhuǎn)彎半徑為r (沿X軸正向?yàn)檎担豖軸負(fù)向?yàn)樨?fù)值），由此可得：
[0051 ]由式可得：
[0053]由圖1中的幾何關(guān)系可得：
[0055]假設(shè)中點(diǎn)速度與P的關(guān)系為f(P)，則中點(diǎn)速度：
[0057]由式可得左右電機(jī)的速度與距離P的關(guān)系為：
[0059] f(p)可根據(jù)實(shí)際運(yùn)動(dòng)效果選取，設(shè)f(p)=kp，即可根據(jù)實(shí)際運(yùn)動(dòng)調(diào)節(jié)k值，以達(dá)到 平滑運(yùn)動(dòng)的目的。
[0060] 步驟四:保存當(dāng)前的角度值ΘΡΜ = Θ;重新回到步驟一。
[0061] 如上所述，對(duì)本發(fā)明的實(shí)施例進(jìn)行了詳細(xì)地說(shuō)明，但是只要實(shí)質(zhì)上沒(méi)有脫離本發(fā) 明的發(fā)明點(diǎn)及效果可以有很多的變形，這對(duì)本領(lǐng)域的技術(shù)人員來(lái)說(shuō)是顯而易見(jiàn)的。因此，這 樣的變形例也全部包含在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種機(jī)器人小車(chē)路徑跟隨方法，其特征在于含有以下步驟;機(jī)器人小車(chē)以圓弧路徑 向目標(biāo)跟隨點(diǎn)運(yùn)動(dòng);機(jī)器人小車(chē)的運(yùn)動(dòng)速度與機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離成正比。2. 根據(jù)權(quán)利要求1所述的一種機(jī)器人小車(chē)路徑跟隨方法，其特征在于含有以下步驟： 步驟一：機(jī)器人小車(chē)通過(guò)傳感器測(cè)距并對(duì)目標(biāo)進(jìn)行定位，從而計(jì)算目標(biāo)位于機(jī)器人小 車(chē)坐標(biāo)系中的位置(ρ，θ); 步驟二:將當(dāng)前所測(cè)得的角度值Θ，與上一次的角度值9pre作比較，如果0-0pre>0 threshQld (如 9threshold = 10。），令 9 = 9pre+A Θ(如 Δ θ = 3);如果 9-0pre〈-0threshtlld(如 0threshc)ld=l〇。），貝 lj 讓?duì)ň徛兓?，令?= ΘΡΜ- △ Θ(如△ θ = 3).從而實(shí)現(xiàn)角度的緩慢變化，從而達(dá)到速度平滑； 步驟三:根據(jù)位置(ρ，θ)和跟隨算法，計(jì)算兩個(gè)驅(qū)動(dòng)輪的速度，從而實(shí)現(xiàn)機(jī)器人小車(chē)對(duì) 目標(biāo)的跟隨； 跟隨算法為： 若被跟隨者位于D處，則機(jī)器人小車(chē)將沿著虛線(xiàn)圓弧軌跡到達(dá)D處，且其跟隨速度與Ρ成 正比;Ρ為機(jī)器人距目標(biāo)點(diǎn)的直線(xiàn)距離； 建立機(jī)器人坐標(biāo)系，坐標(biāo)原點(diǎn)位于后輪中心(ΑΒ中心），y軸方向?yàn)樾≤?chē)前進(jìn)方向，X軸方 向?yàn)楹筝喌妮S向方向； 假設(shè)左輪的速度為VI，右輪的速度為V2，兩后輪之間的間距為d，移動(dòng)小車(chē)的角速度為 ω，轉(zhuǎn)彎半徑為r(沿X軸正向?yàn)檎?，沿X軸負(fù)向?yàn)樨?fù)值），由此可得：假設(shè)中點(diǎn)速度與P的關(guān)系為f (P)，則中點(diǎn)速度： 由式可得： 可得：(6) 由式可得左右電機(jī)的速度與距離ρ的關(guān)系為：f(p)可根據(jù)實(shí)際運(yùn)動(dòng)效果選取，設(shè)f(p)=kp，即可根據(jù)實(shí)際運(yùn)動(dòng)調(diào)節(jié)k值，以達(dá)到平滑 運(yùn)動(dòng)的目的； 步驟四：保存當(dāng)前的角度值θ_=θ;重新回到步驟一。
【文檔編號(hào)】G05D1/02GK105867380SQ201610245269
【公開(kāi)日】2016年8月17日
【申請(qǐng)日】2016年4月19日
【發(fā)明人】王治彪, 畢樹(shù)生, 楊春衛(wèi), 牛傳猛
【申請(qǐng)人】北京九星智元科技有限公司