一種基于直線控制器的智能車輛曲線跟蹤方法
【專利摘要】一種基于直線控制器的智能車輛曲線跟蹤方法，首先對規(guī)劃路徑進(jìn)行離散化，得到一系列首尾相連的直線段，并進(jìn)行障礙物檢測，在不存在障礙物的情況下，依次以各直線段為預(yù)瞄路徑進(jìn)行處理，包括初始化后，判斷當(dāng)前駕駛員關(guān)注點(diǎn)轉(zhuǎn)換的標(biāo)識符的值，如果Flag為1關(guān)注車輛與預(yù)瞄路徑的距離，如果當(dāng)前關(guān)注距離的相應(yīng)變量于等于初始值的二分之一，車輛就轉(zhuǎn)換關(guān)注點(diǎn)；如果Flag為0則關(guān)注車輛的朝向，包括計(jì)算車輛前輪的轉(zhuǎn)向角δ并判斷車輛當(dāng)前航向角和路徑方向角的角度差，如果小于或等于設(shè)定的相應(yīng)閾值則更換關(guān)注點(diǎn)。本發(fā)明的路徑跟蹤方法可以應(yīng)用于任意形狀路徑的跟蹤，具有良好的適應(yīng)性和高跟蹤精度，魯棒性好。
【專利說明】
一種基于直線控制器的智能車輛曲線跟蹤方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及車輛電子控制技術(shù)，尤其涉及一種基于直線控制器的智能車輛曲線跟蹤方法。

【背景技術(shù)】
[0002]在智能車輛控制領(lǐng)域，路徑跟蹤技術(shù)聯(lián)系著上層的路徑規(guī)劃和車輛底層的自動轉(zhuǎn)向控制，主要目的是使智能車輛能準(zhǔn)確、安全地沿規(guī)劃的路徑行駛，因此，路徑跟蹤算法是非常重要的。路徑跟蹤算法的目標(biāo)是通過計(jì)算控制輸入量以確保車輛能參照生成的路徑曲線進(jìn)行跟蹤。
[0003]為了實(shí)現(xiàn)路徑跟蹤，需要設(shè)計(jì)跟蹤控制器來跟蹤生成路徑曲線離散化之后的直線，路徑跟蹤領(lǐng)域應(yīng)用的控制器主要可以分為兩大類:幾何控制器與基于誤差的控制器。
[0004]幾何控制器包含矢量跟蹤、純追隨等，并不基于待跟蹤路徑與實(shí)際路徑的誤差來進(jìn)行控制，魯棒性好，但是控制精度不高。
[0005]基于誤差的控制器包括經(jīng)典控制、智能控制等。目前經(jīng)典控制的應(yīng)用越來越少，大多數(shù)學(xué)者更鐘情于智能控制，例如模糊控制、神經(jīng)網(wǎng)絡(luò)、遺傳算法，這些算法的實(shí)時性好，而且即使在路徑不是很完美的情況下也可以達(dá)到很高的控制精度。但是這些智能控制算法植入困難，而且魯棒性一般。
[0006]在進(jìn)行路徑跟蹤控制時，參數(shù)的變化對跟蹤效果影響很大，其中縱向速度變化的影響尤其明顯，傳統(tǒng)的路徑跟蹤控制器常采用PID(比例、積分、微分)控制器，將機(jī)器人的航向角誤差和誤差變化率作為控制器的輸入，控制器的輸出作為轉(zhuǎn)向角。而在實(shí)際過程中，航向角還與速度、轉(zhuǎn)動慣量、重心位置、前后輪側(cè)偏系數(shù)等因素有關(guān)，這就使PID控制器參數(shù)的全局整定極為困難，PID的控制參數(shù)要根據(jù)系統(tǒng)參數(shù)特別是某些敏感參數(shù)發(fā)生變化來重新設(shè)定，魯棒性差。


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

[0007]本發(fā)明的目的是為了提高路徑跟蹤精度，簡化路徑跟蹤的參數(shù)設(shè)置，通過模擬人類的駕駛習(xí)慣，提出一種擬人化的、精度高、魯棒性好的路徑跟蹤技術(shù)方案。
[0008]本發(fā)明技術(shù)方案提供一種基于直線控制器的智能車輛曲線跟蹤方法，包括首先利用道格拉斯-普克算法對規(guī)劃路徑進(jìn)行離散化，得到一系列首尾相連的直線段，并進(jìn)行障礙物檢測，在不存在障礙物的情況下，依次以各直線段為預(yù)瞄路徑，執(zhí)行以下步驟，
[0009]步驟I,對變量Flag、D、D_pre、D_change、δ和i全部初始化為O,并初始化Error,
[0010]其中，
[0011]Flag為駕駛員關(guān)注點(diǎn)轉(zhuǎn)換的標(biāo)識符，
[0012]D是車輛當(dāng)前位置與預(yù)瞄路徑之間的最短距離；
[0013]D_pre是上一次迭代所得D值；
[0014]D_change是指D的變化率；
[0015]Error是車輛當(dāng)前航向角Θ _CurrentHeading和路徑方向角Θ _pre的角度差；
[0016]δ是控制輸入量，表示車輛前輪的轉(zhuǎn)向角；
[0017]i為循環(huán)變量，用于標(biāo)識當(dāng)前迭代次數(shù)；
[0018]步驟2，判斷當(dāng)前Flag的值，如果Flag為1，轉(zhuǎn)向步驟5 ;如果Flag為0，則轉(zhuǎn)向步驟3 ;
[0019]步驟3,計(jì)算車輛前輪的轉(zhuǎn)向角δ，δ = Kh/VelocityXError, Kh為增益系數(shù)，Velocity為當(dāng)前車輛的速度，Error是車輛當(dāng)前航向角和路徑方向角的角度差；
[0020]步驟4,判斷Error的值,如果小于或等于設(shè)定的相應(yīng)閾值,則令Flag = I并轉(zhuǎn)向步驟7，否則直接進(jìn)入步驟7 ;
[0021]步驟5,計(jì)算變量D、D_pre、D_change、δ以及當(dāng)前車輛和預(yù)貓路徑的相對位置S的值，更新當(dāng)前迭代次數(shù)，所用式子如下，
[0022]D = Distance(P_current, Line)；
[0023]D_change = (D~D_pre) /T ；
[0024]D_pre = D ；
[0025]S = LOCAT1N(P_current, Line)；
[0026]S[i] = S ；
[0027]S_saved = S[0]；
[0028]i++ ；
[0029]δ = Kp xD + Kd / Velocity x D_change;
[0030]其中，
[0031]Distance (P_current, Line)為車輛當(dāng)前位置P_current與預(yù)貓路徑Line之間的最短距離；
[0032]D_pre是上一次迭代所得D值，T為當(dāng)前與上一次迭代時間間隔；
[0033]LOCAT1N(P_current, Line)為預(yù)貓路徑 Line 起點(diǎn)、車輛當(dāng)前位置 P_current 所在點(diǎn)以及預(yù)瞄路徑Line終點(diǎn)所形成的三角形面積值，當(dāng)預(yù)瞄路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)、預(yù)貓路徑Line終點(diǎn)順時針排列時，S〈0,當(dāng)預(yù)貓路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)、預(yù)瞄路徑Line終點(diǎn)逆時針排列時，S>0 ;sign⑶為S的符號；
[0034]3 為矢量，Z)=,s7g/?(5‘)x Z);
[0035]D —change 為欠'丨丨.:，D _changc=sign(5) x D changc;
[0036]S_saved為S的初始值；
[0037]Kp、Kd為增益系數(shù)；
[0038]步驟6，如果當(dāng)前變量S小于等于初始值5_8&%(1\(:，C為預(yù)設(shè)的閾值比例，令Flag = O并轉(zhuǎn)向步驟7，否則直接進(jìn)入步驟7 ；
[0039]步驟7，如果前輪轉(zhuǎn)角大于等于預(yù)設(shè)的轉(zhuǎn)向角閾值A(chǔ)l，則將此時的車輛前輪的轉(zhuǎn)向角δ重新賦值，令δ = π/6并轉(zhuǎn)向步驟9;否則進(jìn)入步驟8;
[0040]步驟8，如果前輪轉(zhuǎn)角小于等于預(yù)設(shè)的轉(zhuǎn)向角閾值Α2，Α2 = -Al，則將此時的車輛前輪的轉(zhuǎn)向角S重新賦值，令S =-η/6并轉(zhuǎn)向步驟9;否則直接進(jìn)入步驟9;
[0041]步驟9，判斷當(dāng)前車輛是否到達(dá)預(yù)瞄路徑的終點(diǎn)，如果到達(dá)終點(diǎn)，結(jié)束對當(dāng)前預(yù)瞄路徑的相應(yīng)處理流程；如果沒有到達(dá)終點(diǎn)，則轉(zhuǎn)向步驟2。
[0042]而且，步驟3中，利用比例控制器計(jì)算車輛前輪的轉(zhuǎn)向角δ。
[0043]而且,步驟5中，利用比例微分控制器計(jì)算變量D、D_pre、D_change、δ以及當(dāng)前車輛和預(yù)瞄路徑Line的相對位置S的值。
[0044]本發(fā)明相對現(xiàn)有技術(shù)的優(yōu)點(diǎn)與積極效果在于:
[0045](I)本發(fā)明的路徑跟蹤方法可以應(yīng)用于任意形狀路徑的跟蹤，具有良好的適應(yīng)性和高跟蹤精度，引入GIS領(lǐng)域中的道格拉斯-普克算法作為曲線離散化的基礎(chǔ)算法，很好的符合了控制器的要求；
[0046](2)本發(fā)明的路徑跟蹤方法偏重實(shí)用性，可植入性，支持利用簡單的PID控制器組合成能夠精確實(shí)現(xiàn)復(fù)雜路徑跟蹤過程的算法；
[0047](3)本發(fā)明的路徑跟蹤方法通過模擬人類的駕駛習(xí)慣提出，為了達(dá)到目標(biāo)位置首先關(guān)注車子的朝向，等朝向與目標(biāo)朝向誤差小于一定的閾值時，再去關(guān)注與目標(biāo)位置的距離，并結(jié)合經(jīng)典控制的PID算法，相對于原始PID的綜合考慮朝向和距離，本發(fā)明將目標(biāo)與距離分開單獨(dú)控制，并且不斷的根據(jù)實(shí)際情況進(jìn)行轉(zhuǎn)換，這符合駕駛員的駕駛習(xí)慣，每次只關(guān)注一個量。并考慮了速度對路徑跟蹤的影響，具有精度高、魯棒性好等優(yōu)點(diǎn)。

【專利附圖】

【附圖說明】
[0048]圖1為本發(fā)明實(shí)施例使用直線控制器的跟蹤算法流程圖；
[0049]圖2為本發(fā)明實(shí)施例S的幾何意義說明圖；
[0050]圖3為本發(fā)明實(shí)施例正弦函數(shù)在速度為lm/s的速度魯棒性示意圖；
[0051]圖4為本發(fā)明實(shí)施例正弦函數(shù)在速度為3m/s的速度魯棒性示意圖；
[0052]圖5為本發(fā)明實(shí)施例正弦函數(shù)在速度為5m/s的速度魯棒性示意圖；
[0053]圖6為本發(fā)明實(shí)施例正弦函數(shù)在速度為lOm/s的速度魯棒性示意圖。

【具體實(shí)施方式】
[0054]下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步的詳細(xì)說明。
[0055]本發(fā)明設(shè)計(jì)了一種擬人化的跟蹤控制器來實(shí)現(xiàn)曲線路徑跟蹤，即利用簡單的PID控制器組合成能夠精確實(shí)現(xiàn)復(fù)雜路徑跟蹤過程的算法，具體實(shí)施時本領(lǐng)域技術(shù)人員可采用計(jì)算機(jī)軟件技術(shù)實(shí)現(xiàn)自動運(yùn)行。實(shí)施例的流程圖見圖1，具體實(shí)現(xiàn)過程如下:
[0056]一、首先，利用道格拉斯-普克算法對已生成的規(guī)劃路徑曲線進(jìn)行離散化，將曲線離散成一系列首尾相連的直線段，并進(jìn)行障礙物檢測:
[0057]實(shí)施例首先利用道格拉斯-普克算法對已生成的曲線進(jìn)行離散化，將曲線路徑離散成一系列首尾相連的直線段；
[0058]為便于實(shí)施參考起見，提供經(jīng)典的道格拉斯-普克算法步驟如下:
[0059]I)在曲線首尾兩點(diǎn)A、B之間連接一條直線AB，該直線為曲線的弦；
[0060]2)得到曲線上離該直線段距離最大的點(diǎn)C，計(jì)算其與AB的距離d ；
[0061]3)比較該距離與預(yù)先給定的閾值的大小，如果小于等于閾值，則該直線段作為曲線的近似，該段曲線處理完畢，如果大于閾值，則進(jìn)入4)。具體實(shí)施時，本領(lǐng)域技術(shù)人員可自行設(shè)定相應(yīng)閾值。
[0062]4)此時距離大于閾值，則用C將曲線分為兩段AC和BC，并分別對兩段繼續(xù)進(jìn)行前三步的處理。
[0063]5)當(dāng)所有曲線都處理完畢時，依次連接各個分割點(diǎn)形成的折線，即可以作為曲線的近似。
[0064]該算法的優(yōu)勢在于它能在彎曲比較小的曲線上生成比較稀疏的點(diǎn)，在彎曲比較大的曲線上生成比較密集的點(diǎn)，這正是控制器離散化所需要的情況，也符合車輛的駕駛習(xí)慣，在直線道路上僅需要微動方向，保持車輛平穩(wěn)，在彎曲的道路上則要根據(jù)實(shí)際情況不斷的調(diào)整方向。
[0065]由于最初生成的路徑是避開障礙物的，但是經(jīng)過道格拉斯普克算法離散化成一段段小直線之后，與最初路徑存在少量不同，但是由于生成路徑時已經(jīng)將障礙物和車輛本身增加和緩沖距離，大多數(shù)情況下離散后的路徑應(yīng)該是安全的。出于安全性的考慮，對于離散化后的路徑需要再次進(jìn)行障礙物檢測。障礙物用多邊形來表示，離散化后的路徑用直線段來表示，障礙物檢測可通過判斷各直線段和多邊形是否相交實(shí)現(xiàn)。如果發(fā)現(xiàn)某條直線段存在碰撞的現(xiàn)象，那么就要考慮在該路段之前重新進(jìn)行路徑規(guī)劃生成路徑，如果仍然無法通過，則需要緊急制動。在不存在障礙物的情況下，進(jìn)入后續(xù)處理。
[0066]二、對離散化所得每段直線段，依次作為預(yù)瞄路徑(即當(dāng)前處理的直線路徑)，執(zhí)行以下步驟:
[0067]步驟1:對變量Flag、D、D_pre、D_change、δ和i全部初始化為O,并初始化Error。其中Flag為駕駛員關(guān)注點(diǎn)轉(zhuǎn)換的標(biāo)識符，F(xiàn)lag為O時駕駛員關(guān)注車輛的朝向，F(xiàn)lag為I時駕駛員關(guān)注車輛與預(yù)瞄路徑的側(cè)向偏移；D是車輛當(dāng)前位置與預(yù)瞄路徑之間的最短距離，可通過車輛上的距離傳感器獲得；D_pre是上一次迭代所得的D值，初始時也取O ;D_change是指D的變化率；Error是車輛當(dāng)前航向角Θ _CurrentHeading和路徑方向角Θ _pre的角度差，車輛當(dāng)前航向角可以由車輛上的慣導(dǎo)系統(tǒng)直接得到，預(yù)瞄路徑方向角可以由預(yù)瞄路徑的直線斜率得到；δ是控制輸入量，表示車輛前輪的轉(zhuǎn)向角；i為循環(huán)變量，用于標(biāo)識當(dāng)前迭代次數(shù)。
[0068]即對變量Flag、D、D_pre、D_change、Error、δ 全部初始化，
[0069]Flag = O ；
[0070]D = 0；
[0071]D_pre = O ；
[0072]D_change = 0 ；
[0073]Error = Θ _pre- Θ _CurrentHeading ；
[0074]5=0；
[0075]i = 0 ；
[0076]步驟2:判斷當(dāng)前Flag的值，如果Flag為1，則當(dāng)前關(guān)注車輛與預(yù)瞄路徑的距離，并轉(zhuǎn)向步驟5 ;如果Flag不為1(為O)，則當(dāng)前關(guān)注車輛的朝向，轉(zhuǎn)向步驟3。
[0077]實(shí)施例設(shè)置Flag是為了模擬人類的駕駛習(xí)慣提出的標(biāo)識，為了達(dá)到目標(biāo)位置首先關(guān)注車子的朝向，等朝向能夠保證可以到達(dá)目標(biāo)位置之后，再去關(guān)注與目標(biāo)位置的距離。
[0078]步驟3:利用比例⑵控制器計(jì)算車輛前輪的轉(zhuǎn)向角δ，δ = Rh/Velocity X Error, Kh為增益系數(shù),具體實(shí)施時，本領(lǐng)域技術(shù)人員可自行設(shè)定取值，可采用經(jīng)驗(yàn)值或?qū)嶒?yàn)值；Velocity為當(dāng)前車輛的速度，Error是車輛當(dāng)前航向角和路徑方向角的角度差。
[0079]步驟4:判斷Error的值,如果小于或等于設(shè)定的相應(yīng)閾值(具體實(shí)施時，本領(lǐng)域技術(shù)人員可自行設(shè)定相應(yīng)閾值，例如實(shí)施例取0.05)，則認(rèn)為當(dāng)前車子的航向角與預(yù)瞄方向一致，已經(jīng)可以滿足車子到達(dá)預(yù)瞄點(diǎn)，駕駛員的關(guān)注點(diǎn)就從車子的航向角轉(zhuǎn)換為車子與預(yù)貓點(diǎn)的距離，令Flag = I并轉(zhuǎn)向步驟7 ;如果Error不滿足設(shè)定的相應(yīng)閾值,直接進(jìn)入步驟7。
[0080]預(yù)瞄路徑中，直線段的兩端點(diǎn)分別為跟蹤的起點(diǎn)和終點(diǎn)，作為車輛當(dāng)前跟蹤目標(biāo)終點(diǎn)的端點(diǎn)即為預(yù)瞄點(diǎn)，如圖2中的終點(diǎn)就是車輛在當(dāng)前直線段的預(yù)瞄點(diǎn)，如圖2的起點(diǎn)就是上一段直線段的預(yù)瞄點(diǎn)。
[0081]步驟5:是模擬駕駛員控制車輛與預(yù)瞄路徑之間的距離的過程，利用ro(比例、微分)控制器來模擬人類的決策行為，包括計(jì)算D、D_pre、D_Change、δ以及當(dāng)前車輛和路徑Line的相對位置S的值。
[0082]實(shí)施例中首先計(jì)算車輛當(dāng)前位置P_current與預(yù)瞄路徑之間的最短距離D ;則D的變化率D_change就由車輛當(dāng)前位置與預(yù)瞄路徑的最短距離D與上一次迭代所得距離D_pre之間的差再除以當(dāng)前和上一次迭代的時間間隔T得到；把當(dāng)前的D值賦給D_pre ;具體來說，第一次執(zhí)行步驟5時，D_pre為初始化所得0，T采用當(dāng)前和初始化時刻的時間間隔；后續(xù)執(zhí)行步驟5時，D_pre為上一次迭代執(zhí)行步驟5所得距離D，T采用當(dāng)前和上一次迭代執(zhí)行步驟5的時間間隔。
[0083]變量S表示當(dāng)前車輛和預(yù)瞄路徑Line的相對位置，S的值為預(yù)瞄路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)以及預(yù)瞄路徑Line終點(diǎn)(即預(yù)瞄點(diǎn))所形成的三角形面積值，該值是利用向量的方式計(jì)算出來，因此是帶有符號的矢量值，當(dāng)預(yù)瞄路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)、預(yù)瞄路徑Line終點(diǎn)順時針排列時，S〈0，并且S的值參見圖2中三角形①的面積,反之當(dāng)預(yù)貓路徑Line起點(diǎn)、車輛當(dāng)前位所在點(diǎn)、預(yù)貓路徑Line終點(diǎn)逆時針排列時，S>0，并且S的值參見圖2中三角形②的面積。
[0084]由于距離是標(biāo)量，不是矢量，對于D來說必須考慮車輛在所預(yù)瞄路徑的左側(cè)或者右側(cè)，才能作為一個變量來反饋信息。本發(fā)明通過引入S的符號來將距離D轉(zhuǎn)化為矢量
[0085]D =Sign(S) x D;
[0086]當(dāng)車輛處于預(yù)瞄路徑的左側(cè)時，S〈0，隨著距離的增大車輛朝向角減??；當(dāng)處于右側(cè)時，S>0，隨著距離的增大車輛朝向角增大。同理D_change和D—樣，轉(zhuǎn)化為矢量
D —change:
[0087]￡) _changc=.s/g./?(S) x D changc;
[0088]通過利用H)控制器，車輛前輪的轉(zhuǎn)向角δ為增益系數(shù)Kp與矢量3 I的積，再加上增益系數(shù)Kd與為矢量3—change的積再除以當(dāng)前車輛的速度Velocity。具體實(shí)施時，本領(lǐng)域技術(shù)人員可自行設(shè)定增益系數(shù)Kp、Kd的取值，可采用經(jīng)驗(yàn)值或?qū)嶒?yàn)值。
[0089]即:
[0090]D = Distance(P_current, Line)；
[0091]D_change = (D~D_pre) /T ；
[0092]D_pre = D ；
[0093]S = LOCAT1N(P_current, Line)；
[0094]S[i] = S ；
[0095]S_saved = S[0]；
[0096]i++ ；
[0097]δ = Kp X D + Kd / Velocity X D_changc;
[0098]S_saved為S的初始值，即當(dāng)?shù)谝淮螆?zhí)行步驟5時，i = 0，所得S存為S[0]，作為初始值供后續(xù)每次執(zhí)行步驟6時判斷使用。
[0099]步驟6:如果當(dāng)前關(guān)注距離的相應(yīng)變量S小于等于初始值S_savedXC,C為預(yù)設(shè)的閾值比例(具體實(shí)施時，本領(lǐng)域技術(shù)人員可自行設(shè)定C的取值，例如實(shí)施例取二分之一)，車輛就轉(zhuǎn)換關(guān)注點(diǎn)，令Flag = O并轉(zhuǎn)向步驟7，由當(dāng)前的關(guān)注距離轉(zhuǎn)換為關(guān)注朝向角；否則直接進(jìn)入步驟7 ；
[0100]步驟7:限制車輛前輪的轉(zhuǎn)向角δ的輸出，包括如果前輪轉(zhuǎn)角大于等于預(yù)設(shè)的轉(zhuǎn)向角閾值A(chǔ)l，實(shí)施例取Al = /6，則將此時的車輛前輪的轉(zhuǎn)向角δ重新賦值，令δ = ji/6并轉(zhuǎn)向步驟9;否則進(jìn)入步驟8;
[0101]步驟8:限制車輛前輪的轉(zhuǎn)向角δ的輸出，包括如果前輪轉(zhuǎn)角小于等于預(yù)設(shè)的轉(zhuǎn)向角閾值Α2，實(shí)施例取Α2 =-Ji/6，則將此時的車輛前輪的轉(zhuǎn)向角δ重新賦值，令δ=-/6并轉(zhuǎn)向步驟9 ;否則直接進(jìn)入步驟9 ；
[0102]具體實(shí)施時，本領(lǐng)域技術(shù)人員可自行預(yù)設(shè)Al、Α2的取值，Al為正值，Α2 = -Al。
[0103]步驟9:判斷當(dāng)前車輛是否到達(dá)預(yù)瞄路徑的終點(diǎn)，如果到達(dá)終點(diǎn)，結(jié)束對當(dāng)前預(yù)瞄路徑的相應(yīng)處理流程；如果沒有到達(dá)終點(diǎn)，則轉(zhuǎn)向步驟2。
[0104]本發(fā)明采用經(jīng)典的自行車模型進(jìn)行仿真實(shí)驗(yàn)，取得了良好的效果:
[0105]X = vcos ( Θ )；
[0106]y = vsin ( Θ )；
[0107]Θ = V K ；
Γ πδ
[0108]K 二一；
[0109](X，y)為車輛的后輪軸中心的位置；θ為車輛的朝向；V為車輛的速度；K為曲率，范圍[-0.190.19];車輛的初始位置[xy Θ ]為[000], δ為前輪轉(zhuǎn)角；L為車前后輪軸距。仿真參數(shù)的設(shè)定如下表:
[0110]

【權(quán)利要求】
1.一種基于直線控制器的智能車輛曲線跟蹤方法，其特征在于:包括首先利用道格拉斯-普克算法對規(guī)劃路徑進(jìn)行離散化，得到一系列首尾相連的直線段，并進(jìn)行障礙物檢測，在不存在障礙物的情況下，依次以各直線段為預(yù)瞄路徑，執(zhí)行以下步驟， 步驟1，對變量Flag、D、D_pre、D_change、δ和i全部初始化為O,并初始化Error, 其中， Flag為駕駛員關(guān)注點(diǎn)轉(zhuǎn)換的標(biāo)識符， D是車輛當(dāng)前位置與預(yù)瞄路徑之間的最短距離； D_pre是上一次迭代所得D值； D_change是指D的變化率； Error是車輛當(dāng)前航向角Θ _CurrentHeading和路徑方向角Θ _pre的角度差； δ是控制輸入量，表示車輛前輪的轉(zhuǎn)向角； i為循環(huán)變量，用于標(biāo)識當(dāng)前迭代次數(shù)； 步驟2，判斷當(dāng)前Flag的值，如果Flag為1，轉(zhuǎn)向步驟5 ;如果Flag為0，則轉(zhuǎn)向步驟3 ； 步驟3,計(jì)算車輛前輪的轉(zhuǎn)向角δ，δ = Kh/VelocityXError, Kh為增益系數(shù)，Velocity為當(dāng)前車輛的速度，Error是車輛當(dāng)前航向角和路徑方向角的角度差； 步驟4,判斷Error的值,如果小于或等于設(shè)定的相應(yīng)閾值,則認(rèn)令Flag = I并轉(zhuǎn)向步驟7，否則直接進(jìn)入步驟7; 步驟5,計(jì)算變量D、D_pre、D_change、δ以及當(dāng)前車輛和預(yù)貓路徑的相對位置S的值，更新當(dāng)前迭代次數(shù)，所用式子如下，
D = Distance(P_current, Line)；
D_change = (D~D_pre)/T ；
D_pre = D ；
S = LOCAT1N(P_current, Line)； S[i] =S;
S_saved = S[0]；
i++ ；
δ 二 Kp X D + Kd / Velocity x D_change; 其中， Distance (P_current, Line)為車輛當(dāng)前位置P_current與預(yù)貓路徑Line之間的最短距離； D_pre是上一次迭代所得D值，T為當(dāng)前與上一次迭代時間間隔； LOCAT1N(P_current, Line)為預(yù)貓路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)以及預(yù)瞄路徑Line終點(diǎn)所形成的三角形面積值，當(dāng)預(yù)瞄路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)、預(yù)貓路徑Line終點(diǎn)順時針排列時，S〈0,當(dāng)預(yù)貓路徑Line起點(diǎn)、車輛當(dāng)前位置P_current所在點(diǎn)、預(yù)瞄路徑Line終點(diǎn)逆時針排列時，S>0 ;sign⑶為S的符號； 3為矢量，D =sign(S) X D;
D changc 為矢量，D _changc=.s/g/7(5) x D_changc; S_saved為S的初始值； Kp、Kd為增益系數(shù)； 步驟6,如果當(dāng)前變量S小于等于初始值S_savedXC,C為預(yù)設(shè)的閾值比例,令Flag =O并轉(zhuǎn)向步驟7，否則直接進(jìn)入步驟7 ； 步驟7，如果前輪轉(zhuǎn)角大于等于預(yù)設(shè)的轉(zhuǎn)向角閾值A(chǔ)l，則將此時的車輛前輪的轉(zhuǎn)向角δ重新賦值，令δ =Al并轉(zhuǎn)向步驟9;否則進(jìn)入步驟8; 步驟8，如果前輪轉(zhuǎn)角小于等于預(yù)設(shè)的轉(zhuǎn)向角閾值Α2，Α2 = -Al，則將此時的車輛前輪的轉(zhuǎn)向角δ重新賦值，令δ =Α2并轉(zhuǎn)向步驟9;否則直接進(jìn)入步驟9; 步驟9，判斷當(dāng)前車輛是否到達(dá)預(yù)瞄路徑的終點(diǎn)，如果到達(dá)終點(diǎn)，結(jié)束對當(dāng)前預(yù)瞄路徑的相應(yīng)處理流程；如果沒有到達(dá)終點(diǎn)，則轉(zhuǎn)向步驟2。
2.根據(jù)權(quán)利要求1所述基于直線控制器的智能車輛曲線跟蹤方法，其特征在于:步驟3中，利用比例控制器計(jì)算車輛前輪的轉(zhuǎn)向角δ。
3.根據(jù)權(quán)利要求1所述基于直線控制器的智能車輛曲線跟蹤方法，其特征在于:步驟5中，利用比例微分控制器計(jì)算變量0、0_口代、0_01^1^6、δ以及當(dāng)前車輛和預(yù)貓路徑Line的相對位置S的值。
【文檔編號】G05D1/02GK104181923SQ201410436835
【公開日】2014年12月3日 申請日期:2014年8月29日 優(yōu)先權(quán)日:2014年8月29日
【發(fā)明者】李必軍, 單云霄, 謝云, 鄭玲, 周劍 申請人:武漢大學(xué)