[發明專利]一種基于直線控制器的智能車輛曲線跟蹤方法

在審

• 申請號: 201410436835.1
• 申請日: 2014-08-29
• 公開（公告）號: CN104181923A
• 公開（公告）日: 2014-12-03
• 發明（設計）人: 李必軍;單云霄;謝云;鄭玲;周劍
• 申請（專利權）人: 武漢大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 武漢科皓知識產權代理事務所(特殊普通合伙) 42222
• 代理人: 嚴彥

1.一種基于直線控制器的智能車輛曲線跟蹤方法，其特征在于：包括首先利用道格拉斯-普克算法對規劃路徑進行離散化，得到一系列首尾相連的直線段，并進行障礙物檢測，在不存在障礙物的情況下，依次以各直線段為預瞄路徑，執行以下步驟，

步驟1，對變量Flag、D、D_pre、D_change、δ和i全部初始化為0，并初始化Error，

其中，

Flag為駕駛員關注點轉換的標識符，

D是車輛當前位置與預瞄路徑之間的最短距離；

D_pre是上一次迭代所得D值；

D_change是指D的變化率；

Error是車輛當前航向角θ_CurrentHeading和路徑方向角θ_pre的角度差；

δ是控制輸入量，表示車輛前輪的轉向角；

i為循環變量，用于標識當前迭代次數；

步驟2，判斷當前Flag的值，如果Flag為1，轉向步驟5；如果Flag為0，則轉向步驟3；

步驟3，計算車輛前輪的轉向角δ，δ＝Kh/Velocity×Error，Kh為增益系數，Velocity為當前車輛的速度，Error是車輛當前航向角和路徑方向角的角度差；

步驟4，判斷Error的值，如果小于或等于設定的相應閾值，則認令Flag＝1并轉向步驟7，否則直接進入步驟7；

步驟5，計算變量D、D_pre、D_change、δ以及當前車輛和預瞄路徑的相對位置S的值，更新當前迭代次數，所用式子如下，

D＝Distance(P_current,Line)；

D_change＝(D-D_pre)/T；

D_pre＝D；

S＝LOCATION(P_current,Line)；

S[i]＝S；

S_saved＝S[0]；

i++；

δ=Kp×D→+Kd/Velocity×D→_change;]]>

其中，

Distance(P_current,Line)為車輛當前位置P_current與預瞄路徑Line之間的最短距離；

D_pre是上一次迭代所得D值，T為當前與上一次迭代時間間隔；

LOCATION(P_current,Line)為預瞄路徑Line起點、車輛當前位置P_current所在點以及預瞄路徑Line終點所形成的三角形面積值，當預瞄路徑Line起點、車輛當前位置P_current所在點、預瞄路徑Line終點順時針排列時，S<0，當預瞄路徑Line起點、車輛當前位置P_current所在點、預瞄路徑Line終點逆時針排列時，S>0；sign(S)為S的符號；

為矢量，D→=sing(S)×D;]]>

為矢量，D→_change=sing(S)×D_change;]]>

S_saved為S的初始值；

Kp、Kd為增益系數；

步驟6，如果當前變量S小于等于初始值S_saved×C，C為預設的閾值比例，令Flag＝0并轉向步驟7，否則直接進入步驟7；

步驟7，如果前輪轉角大于等于預設的轉向角閾值A1，則將此時的車輛前輪的轉向角δ重新賦值，令δ＝A1并轉向步驟9；否則進入步驟8；

步驟8，如果前輪轉角小于等于預設的轉向角閾值A2，A2＝-A1，則將此時的車輛前輪的轉向角δ重新賦值，令δ＝A2并轉向步驟9；否則直接進入步驟9；

步驟9，判斷當前車輛是否到達預瞄路徑的終點，如果到達終點，結束對當前預瞄路徑的相應處理流程；如果沒有到達終點，則轉向步驟2。