本發(fā)明屬于自動泊車系統(tǒng)領(lǐng)域，具體涉及一種自動泊車路徑跟蹤及車輛控制系統(tǒng)和方法。

背景技術(shù)：

隨著車輛自動化和智能化的不斷提高，自動泊車功能也逐漸成為大量中高端車型的標(biāo)配。然而目前大部分研究只考慮了泊車的路徑規(guī)劃問題，但缺乏車輛入庫過程中的路徑跟蹤及方向盤控制的研究。車輛在入庫過程中容易偏離規(guī)劃路徑，泊車精度低，對泊車位有較高要求且泊車失敗率較高。

技術(shù)實(shí)現(xiàn)要素：

針對上述存在的問題，本發(fā)明提出一種自動泊車路徑跟蹤及車輛控制系統(tǒng)和方法，以實(shí)現(xiàn)精確的路徑跟蹤控制，提高泊車精度，降低對泊車位的要求和泊車失敗率。

為了實(shí)現(xiàn)上述目的，本發(fā)明具體技術(shù)方案如下：一種自動泊車路徑跟蹤及車輛控制方法，包括如下步驟：

1)初始化系統(tǒng)變量，包括計(jì)數(shù)變量i，速度閾值v_limt，速度控制量v_control，單位時間內(nèi)速度增量△v,單位時間內(nèi)速度增量閾值△v_max，前輪轉(zhuǎn)角閾值u_limt，前輪轉(zhuǎn)角控制量u_control，單位時間內(nèi)前輪轉(zhuǎn)角增量△u，單位時間內(nèi)前輪轉(zhuǎn)角增量閾值△u_max；

2)初始化規(guī)劃的泊車路徑狀態(tài)特征數(shù)組p[n+1][5]，初始化方法如下：

2.1)在規(guī)劃的泊車路徑上選擇參考點(diǎn)，即在規(guī)劃的泊車路徑上每隔δs距離取一個參考點(diǎn)，并將每一個參考點(diǎn)依次編號為0，1，…，n；

2.2)確定坐標(biāo)原點(diǎn)，建立世界坐標(biāo)系xoy，即以編號為n的參考點(diǎn)為坐標(biāo)原點(diǎn)，建立世界坐標(biāo)系xoy，其中x軸與泊車位前后兩車中心線平行，且方向指向前車，y軸與x軸垂直且符合右手法則；

2.3)確定參考點(diǎn)狀態(tài)特征參數(shù)，建立泊車路徑狀態(tài)特征數(shù)組p[n+1][5]，其中，p[k][0]表示k點(diǎn)對應(yīng)的參考點(diǎn)x坐標(biāo)值，p[k][1]表示k點(diǎn)對應(yīng)的參考點(diǎn)y坐標(biāo)值，p[k][2]表示k點(diǎn)對應(yīng)的參考點(diǎn)航向角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?，p[k][3]表示k點(diǎn)對應(yīng)的參考點(diǎn)參考速度，向前為正，向后為負(fù)，p[k][4]表示k點(diǎn)對應(yīng)的參考點(diǎn)參考前輪轉(zhuǎn)角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?，k＝0，1，2...n；

3)建立車輛當(dāng)前狀態(tài)數(shù)組p_real[5]，其中，p_real[0]表示車輛當(dāng)前x坐標(biāo)值，p_real[1]表示車輛當(dāng)前y坐標(biāo)值，p_real[2]表示車輛當(dāng)前航向角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?，p_real[3]表示當(dāng)前車速，向前為正，向后為負(fù)，p_real[4]表示車輛當(dāng)前前輪轉(zhuǎn)角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?/p>

4)尋找目標(biāo)點(diǎn)，如果p_real[0]＞p[i][0]時，目標(biāo)點(diǎn)為pi，轉(zhuǎn)步驟6)，否則i＝i+1，轉(zhuǎn)步驟5)；

5)比較i與n+1，若i＜n+1，轉(zhuǎn)步驟4)，否則轉(zhuǎn)步驟7)；

6)車輛向目標(biāo)點(diǎn)pi逼近，轉(zhuǎn)步驟3)；

7)路徑跟蹤結(jié)束。

進(jìn)一步的，上述步驟6)中，車輛向目標(biāo)點(diǎn)pi逼近的過程中，確定速度控制量的方法為：

v_control＝p_real[3]+δv，|p_real[3]+δv|≤v_limt

進(jìn)一步的，上述步驟6)中，車輛向目標(biāo)點(diǎn)pi逼近的過程中，確定前輪轉(zhuǎn)角控制量的方法為：

如果p_real[2]＞p[i][2]，

如果p_real[2]＜p[i][2]，

u_control＝p_re；al[4]+δu,|p_real[4]+δu|≤v_limt

進(jìn)一步的，本發(fā)明還提供了一種自動泊車路徑跟蹤及車輛控制系統(tǒng)，其特征在于包括路徑跟蹤控制器、慣性導(dǎo)航模塊、陀螺儀、傳感器單元、電源模塊；其中，電源模塊分別與路徑跟蹤控制器、慣性導(dǎo)航模塊、陀螺儀模塊和傳感器單元相連，用于給各模塊供電；電源模塊還包含電路保護(hù)單元，可在電源接反的情況下自動斷開，可在系統(tǒng)電流過大的情況下自動切斷電源；傳感器單元包含輪速傳感器和方向盤轉(zhuǎn)角傳感器，輪速傳感器用于采集車輛行駛速度，方向盤轉(zhuǎn)角傳感器用于采集方向盤轉(zhuǎn)角。傳感器單元輸出端與路徑跟蹤控制器相連；慣性導(dǎo)航模塊用于獲得車輛當(dāng)前的坐標(biāo)，輸出端與路徑跟蹤控制器相連；陀螺儀用于獲得車輛當(dāng)前的車身角，輸出端與路徑跟蹤控制器相連；路徑跟蹤控制器包括數(shù)據(jù)處理單元和微控制單元mcu，數(shù)據(jù)處理單元用于實(shí)時處理傳感器單元傳來的信息，并將處理后得到的車輛狀態(tài)信息發(fā)送給mcu，mcu用于接收來自于數(shù)據(jù)處理單元、慣性導(dǎo)航模塊和陀螺儀的信息，將得到的車輛狀態(tài)信息與參考車輛狀態(tài)信息相比較，利用跟蹤算法算出當(dāng)前的速度和方向盤轉(zhuǎn)角的最佳增量。

本發(fā)明的有益效果在于：在自動泊車過程中，本發(fā)明可根據(jù)規(guī)劃好的路徑對車輛進(jìn)行較為精確的路徑跟蹤控制，跟蹤過程中實(shí)時對車輛狀態(tài)進(jìn)行矯正，提高了泊車精度，降低了對泊車位的要求和泊車失敗率。

附圖說明

圖1為自動泊車路徑跟蹤及車輛控制系統(tǒng)。

圖2為路徑跟蹤流程圖。

圖3為路徑跟蹤示意圖。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步描述。

如圖1所示，本發(fā)明所述的自動泊車路徑跟蹤及車輛控制系統(tǒng)，包括路徑跟蹤控制器、慣性導(dǎo)航模塊、陀螺儀、傳感器單元、電源模塊。其中：

電源模塊分別與路徑跟蹤控制器、慣性導(dǎo)航模塊、陀螺儀和傳感器單元相連，用于給各模塊供電；電源模塊還包含電路保護(hù)單元，可在電源接反的情況下自動斷開，可在系統(tǒng)電流過大的情況下自動切斷電源。

傳感器單元包含輪速傳感器和方向盤轉(zhuǎn)角傳感器，輪速傳感器用于采集車輛行駛速度，方向盤轉(zhuǎn)角傳感器用于采集方向盤轉(zhuǎn)角。傳感器單元輸出端與路徑跟蹤控制器相連。

慣性導(dǎo)航模塊用于獲得車輛當(dāng)前的坐標(biāo)，輸出端與路徑跟蹤控制器相連。

陀螺儀用于獲得車輛當(dāng)前的車身角，輸出端與路徑跟蹤控制器相連。

路徑跟蹤控制器包括數(shù)據(jù)處理單元和微控制單元mcu，數(shù)據(jù)處理單元用于實(shí)時處理傳感器單元傳來的信息，并將處理后得到的車輛狀態(tài)信息發(fā)送給mcu，mcu用于接收來自于數(shù)據(jù)處理單元、慣性導(dǎo)航模塊和陀螺儀的信息，將得到的車輛狀態(tài)信息與參考車輛狀態(tài)信息相比較，利用跟蹤算法算出當(dāng)前的速度和方向盤轉(zhuǎn)角的最佳增量；具體實(shí)施中，微控制單元mcu用32位單片機(jī)實(shí)現(xiàn)。

圖2所示為發(fā)明所述的自動泊車路徑跟蹤及車輛控制方法的流程圖，以平行泊車倒車入庫為例進(jìn)行說明，自動泊車路徑跟蹤及車輛控制方法的步驟如下：

1)初始化系統(tǒng)變量，包括計(jì)數(shù)變量i；速度閾值v_limt，速度控制量v_control，單位時間內(nèi)速度增量△v,單位時間內(nèi)速度增量閾值△v_max；前輪轉(zhuǎn)角閾值u_limt，前輪轉(zhuǎn)角控制量u_control，單位時間內(nèi)前輪轉(zhuǎn)角增量△u，單位時間內(nèi)前輪轉(zhuǎn)角增量閾值△u_max；本實(shí)施例中，計(jì)數(shù)變量i＝0，速度閾值v_limt＝10km/h，速度控制量v_control＝0，單位時間內(nèi)速度增量δv＝0,單位時間內(nèi)速度增量閾值δv_max＝0.35km/h，前輪轉(zhuǎn)角閾值u_limt＝35°，前輪轉(zhuǎn)角控制量u_control＝0，單位時間內(nèi)前輪轉(zhuǎn)角增量δu＝0，單位時間內(nèi)前輪轉(zhuǎn)角增量閾值δu_max＝1°；每過單位時間向車輛發(fā)送控制量v_control和u_control；本實(shí)施例中，單位時間為20ms。

2)初始化規(guī)劃的泊車路徑狀態(tài)特征數(shù)組p[n+1][5]，初始化方法如下：

2.1)在規(guī)劃的泊車路徑上選擇參考點(diǎn)，即在規(guī)劃的泊車路徑上每隔δs距離取一個參考點(diǎn)，并將每一個參考點(diǎn)依次編號為0，1，…，n；本實(shí)施例中，δs取值10厘米。

2.2)確定坐標(biāo)原點(diǎn)，建立世界坐標(biāo)系xoy，即以編號為n的參考點(diǎn)為坐標(biāo)原點(diǎn)，建立世界坐標(biāo)系xoy，如圖3所示，其中x軸與泊車位前后兩車中心線平行，且方向指向前車，y軸與x軸垂直且符合右手法則，1表示參考泊車軌跡，2表示實(shí)際泊車軌跡，3表示前車，4表示后車。

2.3)確定參考點(diǎn)狀態(tài)特征參數(shù)，建立泊車路徑狀態(tài)特征數(shù)組p[n+1][5]，其中，p[k][0]表示k點(diǎn)對應(yīng)的參考點(diǎn)x坐標(biāo)值，p[k][1]表示k點(diǎn)對應(yīng)的參考點(diǎn)y坐標(biāo)值，p[k][2]表示k點(diǎn)對應(yīng)的參考點(diǎn)航向角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎琾[k][3]表示k點(diǎn)對應(yīng)的參考點(diǎn)參考速度，向前為正，向后為負(fù)，p[k][4]表示k點(diǎn)對應(yīng)的參考點(diǎn)參考前輪轉(zhuǎn)角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?，k＝0，1，2...n；

3)建立車輛當(dāng)前狀態(tài)數(shù)組p_real[5]，其中p_real[0]表示車輛當(dāng)前x坐標(biāo)值，p_real[1]表示車輛當(dāng)前y坐標(biāo)值，p_real[2]表示車輛當(dāng)前航向角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?，p_real[3]表示當(dāng)前車速，向前為正，向后為負(fù)，p_real[4]表示車輛當(dāng)前前輪轉(zhuǎn)角，順時針方向?yàn)樨?fù)，逆時針方向?yàn)檎?/p>

4)尋找目標(biāo)點(diǎn)，如果p_real[0]＞p[i][0]時，目標(biāo)點(diǎn)為pi，轉(zhuǎn)步驟6)，否則i＝i+1，轉(zhuǎn)步驟5)；

5)比較i與n+1，若i<n+1，轉(zhuǎn)步驟4)，否則轉(zhuǎn)步驟7)；

6)車輛向目標(biāo)點(diǎn)pi逼近，轉(zhuǎn)步驟3)，其中，在逼近過程中，確定速度控制量的方法為：

v_control＝p_real[3]+δv，|p_real[3]+δv|≤v_limt

確定前輪轉(zhuǎn)角控制量的方法為：

如果p_real[2]＞p[i][2]，

如果p_real[2]＜p[i][2]，

u_control＝p_real[4]+δu，|p_real[4]+δu|≤v_limt

7)路徑跟蹤結(jié)束。