基于多gps和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)及其控制算法
【專利摘要】本發(fā)明公開了一種基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)及其控制算法，系統(tǒng)包括模擬汽車方向盤、模擬油門踏板、模擬剎車踏板、角度傳感器、揚(yáng)聲器、MCU、GPS、CAN總線控制器、車載控制電腦、油門踏板輔助駕駛執(zhí)行器、剎車踏板輔助駕駛執(zhí)行器、轉(zhuǎn)向助力電機(jī)、油門踏板、剎車踏板、方向盤、油門踏板位置傳感器、方向盤位置傳感器、剎車踏板位置傳感器。控制算法分為數(shù)據(jù)采集和數(shù)據(jù)處理兩部分。數(shù)據(jù)采集主要對(duì)采集的數(shù)據(jù)進(jìn)行濾波降噪。數(shù)據(jù)處理根據(jù)不同使用工況對(duì)數(shù)據(jù)進(jìn)行處理以及車輛的狀態(tài)估計(jì)、車輛路徑計(jì)算和控制參數(shù)的計(jì)算。能使學(xué)員能感性、具體地體會(huì)教練員訓(xùn)練要求，提高學(xué)習(xí)效率，縮短學(xué)習(xí)時(shí)間，同時(shí)縮短駕校的培訓(xùn)周期、提高培訓(xùn)效率。
【專利說明】
基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)及其控制 算法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于人體動(dòng)作參數(shù)識(shí)別的技術(shù)領(lǐng)域，更具體的說是一種基于多GPS和角度 傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)及其控制算法。
【背景技術(shù)】
[0002] 隨著我國國民經(jīng)濟(jì)的增長，我國的轎車保有量逐年飛躍，因而社會(huì)上相應(yīng)地成立 了大量駕校W滿足人們對(duì)車輛駕駛的需求。隨著競(jìng)爭(zhēng)的不斷加劇，各駕校迫切需要進(jìn)行管 理、技術(shù)創(chuàng)新W謀求生存。在駕校的運(yùn)營中，人員工資的支出占據(jù)了其營運(yùn)成本的相當(dāng)一部 分。另外，由于學(xué)員的駕駛培訓(xùn)需要一定的時(shí)間周期，從而延長了駕校的營運(yùn)周轉(zhuǎn)周期。學(xué) 員的所需培訓(xùn)周期越長，則駕校的營運(yùn)周轉(zhuǎn)就越慢，同時(shí)駕校的成本也相應(yīng)地上升。

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

[0003] 針對(duì)W上問題，本發(fā)明的目的在于，提供一種基于多GI^和角度傳感器的駕駛?cè)溯o 助訓(xùn)練系統(tǒng)及其控制算法，通過實(shí)現(xiàn)一部分駕駛培訓(xùn)的自動(dòng)化過程，增強(qiáng)教練員對(duì)學(xué)員的 指導(dǎo)效率，幫助駕?？s短營運(yùn)周期，節(jié)省人力成本，提高營運(yùn)效益。
[0004] 為了實(shí)現(xiàn)上述任務(wù)，本發(fā)明采用W下技術(shù)方案得W實(shí)現(xiàn)：
[0005] -種基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)，包括模擬汽車方向盤、模擬 油口踏板和模擬剎車踏板，其特征在于，所述的模擬汽車方向盤、模擬油口踏板和模擬剎車 踏板分別通過第一角度傳感器、第二角度傳感器和第S角度傳感器分別連接MCU，在MCU上 連接有第一 GPS、第二GPS和揚(yáng)聲器，MCU上還連接有CAN總線控制器、油口踏板輔助駕駛執(zhí)行 器和剎車踏板輔助駕駛執(zhí)行器;其中：
[0006] CAN總線控制器依次連接車載中央控制器、轉(zhuǎn)向助力電機(jī)、方向盤、方向盤位置傳 感器；
[0007] 油口踏板輔助駕駛執(zhí)行器依次連接油口踏板和油口踏板位置傳感器;其中油口踏 板位置傳感器與車載中央控制器相連接；
[000引剎車踏板輔助駕駛執(zhí)行器依次連接剎車踏板和剎車踏板位置傳感器;其中剎車踏 板位置傳感器與車載中央控制器相連接。
[0009] 根據(jù)本發(fā)明，所述的油口踏板輔助駕駛執(zhí)行器和剎車踏板輔助駕駛執(zhí)行器均含有 伺服電機(jī)、彈性結(jié)合套和驅(qū)動(dòng)短板。
[0010] 上述基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)的訓(xùn)練方法，其特征在于，在 有人輔助訓(xùn)練模式和無人輔助訓(xùn)練模式下，采用不同的控制算法實(shí)現(xiàn)對(duì)學(xué)員的輔助訓(xùn)練， 具體的步驟為：
[0011] 步驟一，學(xué)員或教練員根據(jù)實(shí)際需求，選擇系統(tǒng)的工作模式；
[0012] 步驟二，當(dāng)工作在有人輔助訓(xùn)練模式時(shí)，系統(tǒng)利用安裝在汽車駕駛室副駕駛位置 模擬汽車方向盤、油口踏板和剎車踏板，W及在運(yùn)些部件上安裝角度傳感器，采集教練員的 駕駛動(dòng)作數(shù)據(jù)；
[0013] 步驟S，將采集到的教練員動(dòng)作數(shù)據(jù)在MCU中重建為對(duì)該執(zhí)行機(jī)構(gòu)的控制數(shù)據(jù)；
[0014] 步驟四，當(dāng)工作在無人輔助訓(xùn)練模式時(shí)，根據(jù)實(shí)驗(yàn)標(biāo)定的數(shù)據(jù)和角度傳感器測(cè)量 得到的當(dāng)前車輛信息，計(jì)算出車輛的行進(jìn)路徑，并利用結(jié)合BP神經(jīng)網(wǎng)絡(luò)算法的PID控制器計(jì) 算執(zhí)行機(jī)構(gòu)的控制參數(shù)；
[0015] 步驟五，輔助駕駛執(zhí)行機(jī)構(gòu)依據(jù)MCU的控制信號(hào)，在學(xué)員的手部、腳步施加一定的 外力，使學(xué)員能依照教練員的手腳動(dòng)作完成對(duì)車輛的駕駛操作，從而強(qiáng)化駕駛行為學(xué)習(xí)，同 時(shí)系統(tǒng)通過各個(gè)角度傳感器對(duì)執(zhí)行機(jī)構(gòu)的執(zhí)行情況進(jìn)行檢測(cè)，對(duì)控制輸出信號(hào)進(jìn)行修正；
[0016] 步驟六，無人輔助訓(xùn)練模式中，利用GPS提供的數(shù)據(jù)W及實(shí)驗(yàn)標(biāo)定數(shù)據(jù)得到車輛路 徑計(jì)算所需的邊界條件，并利用六參數(shù)PI的十算車輛方向的控制參數(shù)。
[0017] 在步驟二中，訓(xùn)練員通過汽車駕駛室副駕駛位置安裝的模擬方向盤、模擬油口踏 板、模擬剎車踏板進(jìn)行操作，輸入動(dòng)作信號(hào);運(yùn)些動(dòng)作信號(hào)被安裝于模擬方向盤、模擬油口 踏板、模擬剎車踏板上的=個(gè)角度傳感器所捕獲，并W模擬信號(hào)的形式傳遞到MCU。
[001引在步驟五中，MC閑尋數(shù)據(jù)經(jīng)由CAN總線控制器傳遞給車載中央控制器，從而控制方 向盤的運(yùn)動(dòng)，并帶動(dòng)學(xué)員的雙手進(jìn)行運(yùn)動(dòng)；油口踏板輔助駕駛執(zhí)行器和剎車踏板輔助駕駛 執(zhí)行器直接受到MCU的控制，從而對(duì)學(xué)員的雙腳施加一定外力，并驅(qū)動(dòng)雙腳對(duì)油口踏板和剎 車踏板進(jìn)行操作;最后，方向盤、油口踏板和剎車踏板的運(yùn)動(dòng)狀態(tài)信息又分別由方向盤位置 傳感器、油口踏板位置傳感器、剎車踏板位置傳感器進(jìn)行采集，然后經(jīng)由車載中央控制器、 CAN總線控制器傳遞給MCU，從而形成閉合控制回路。
[0019] 本發(fā)明的基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)W及控制算法和訓(xùn)練方 法。與現(xiàn)有技術(shù)相比，帶來的技術(shù)效果是:利用多個(gè)傳感器對(duì)教練員的人體動(dòng)作信息進(jìn)行采 集和識(shí)別，并在MCU中對(duì)運(yùn)些參數(shù)進(jìn)行重建，或者系統(tǒng)在無人模式下無需教練員的干預(yù)，直 接根據(jù)依照一定的算法計(jì)算出執(zhí)行機(jī)構(gòu)的控制參數(shù)，然后驅(qū)動(dòng)學(xué)員的手腳進(jìn)行相應(yīng)動(dòng)作， 從而使學(xué)員方面能直接、精確地模仿教練員的動(dòng)作行為，能感性、具體地體會(huì)教練員的訓(xùn)練 要求，提高學(xué)習(xí)效率，縮短學(xué)習(xí)時(shí)間，同時(shí)縮短駕校方面的運(yùn)營周期、提高生產(chǎn)培訓(xùn)效率，形 成良好的社會(huì)經(jīng)濟(jì)效益。
【附圖說明】
[0020] 圖1為本發(fā)明的基于多GI^和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)硬件結(jié)構(gòu)示意圖。 [0021 ]圖2為油口踏板和剎車踏板輔助駕駛執(zhí)行器示意圖。
[0022] 圖3為傳感器信號(hào)濾波算法結(jié)構(gòu)示意圖。
[0023] 圖4為信號(hào)濾波結(jié)果演示圖，其中上圖為原始信號(hào)，下圖為濾波后信號(hào)。
[0024] 圖5為倒庫路徑計(jì)算算法原理圖。
[0025] 圖6為BP神經(jīng)網(wǎng)絡(luò)PID控制算法結(jié)構(gòu)示意圖。
[0026] 圖7為BP神經(jīng)網(wǎng)絡(luò)PID控制算法控制效果演示圖。
[0027] 圖8為BP神經(jīng)網(wǎng)絡(luò)PID控制算法控制誤差曲線。
[0028] 下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)闡述。
【具體實(shí)施方式】
[0029] 參見圖I，本實(shí)施例給出一種基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)，包 括汽車駕駛室副駕駛位置安裝的模擬汽車方向盤1、模擬油口踏板2和模擬剎車踏板3,模擬 汽車方向盤1、模擬油口踏板2和模擬剎車踏板3分別通過第一角度傳感器4、第二角度傳感 器5和第S角度傳感器6分別連接MClK 9 )，在MClK 9)上連接有第一 GPS (7 )、第二GPS (8)和揚(yáng) 聲器10，MCU(9)上還連接有CAN總線控制器11、油口踏板輔助駕駛執(zhí)行器（13)和剎車踏板輔 助駕駛執(zhí)行器14;其中：
[0030] CAN總線控制器11依次連接車載中央控制器12、轉(zhuǎn)向助力電機(jī)15、方向盤18、方向 盤位置傳感器20;
[0031] 油口踏板輔助駕駛執(zhí)行器13依次連接油口踏板16和油口踏板位置傳感器19;其中 油口踏板位置傳感器19與車載中央控制器12相連接；
[0032] 剎車踏板輔助駕駛執(zhí)行器14依次連接剎車踏板17和剎車踏板位置傳感器21;其中 剎車踏板位置傳感器21與車載中央控制器12相連接。
[0033] 本實(shí)施例中，油口踏板輔助駕駛執(zhí)行器13、剎車踏板輔助駕駛執(zhí)行器14均包括伺 服電機(jī)、彈性結(jié)合套和驅(qū)動(dòng)短板，其結(jié)構(gòu)如圖2所示。
[0034] 上述基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)的控制算法，首先根據(jù)GPS數(shù) 據(jù)計(jì)算車輛的初始位置和初始側(cè)偏角在內(nèi)的初始邊界條件，然后計(jì)算出所需的車輛行進(jìn)路 徑，再利用BP神經(jīng)網(wǎng)絡(luò)算法擬合出PID控制器的控制參數(shù)用于控制車輛的行進(jìn)路徑和行進(jìn) 姿態(tài)，同時(shí)同樣采用PID控制器計(jì)算出執(zhí)行器所需的對(duì)的油口踏板、剎車踏板的操作量。
[0035] 基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)的訓(xùn)練方法，在有人輔助訓(xùn)練模 式或無人輔助訓(xùn)練模式下，采用不同的控制算法實(shí)現(xiàn)對(duì)學(xué)員的輔助訓(xùn)練，具體的步驟為：
[0036] 步驟一，學(xué)員或教練員根據(jù)實(shí)際需求，選擇系統(tǒng)的工作模式；
[0037] 步驟二，當(dāng)工作在有人輔助訓(xùn)練模式時(shí)，利用安裝在汽車駕駛室副駕駛位置模擬 汽車方向盤、油口踏板和剎車踏板，W及在運(yùn)些部件上安裝角度傳感器，采集教練員的駕駛 動(dòng)作數(shù)據(jù)；
[0038] 步驟S，將采集到的教練員動(dòng)作數(shù)據(jù)在MCU中重建為對(duì)該執(zhí)行機(jī)構(gòu)的控制數(shù)據(jù)；
[0039] 步驟四，當(dāng)工作在無人輔助訓(xùn)練模式時(shí)，根據(jù)實(shí)驗(yàn)標(biāo)定的數(shù)據(jù)和角度傳感器測(cè)量 得到的當(dāng)前車輛信息，計(jì)算出車輛的行進(jìn)路徑，并利用結(jié)合BP神經(jīng)網(wǎng)絡(luò)算法的PID控制器計(jì) 算執(zhí)行機(jī)構(gòu)的控制參數(shù)；
[0040] 步驟五，輔助駕駛執(zhí)行機(jī)構(gòu)依據(jù)MCU的控制信號(hào)，在學(xué)員的手部、腳步施加一定的 外力，使學(xué)員能依照教練員的手腳動(dòng)作完成對(duì)車輛的駕駛操作，從而強(qiáng)化駕駛行為學(xué)習(xí)，同 時(shí)系統(tǒng)通過各個(gè)角度傳感器對(duì)執(zhí)行機(jī)構(gòu)的執(zhí)行情況進(jìn)行檢測(cè)，對(duì)控制輸出信號(hào)進(jìn)行修正；
[0041] 步驟六，無人輔助訓(xùn)練模式中，利用GPS提供的數(shù)據(jù)W及實(shí)驗(yàn)標(biāo)定數(shù)據(jù)得到車輛路 徑計(jì)算所需的邊界條件，并利用六參數(shù)PI的十算車輛方向的控制參數(shù)。
[0042] 在步驟二中，訓(xùn)練員通過汽車駕駛室副駕駛位置安裝的模擬方向盤1、模擬油口踏 板2、模擬剎車踏板3進(jìn)行操作，輸入動(dòng)作信號(hào);運(yùn)些動(dòng)作信號(hào)被安裝于模擬方向盤1、模擬油 口踏板2、模擬剎車踏板3上的角度傳感器4~6所捕獲，并W模擬信號(hào)的形式傳遞到MCU(9)。
[0043] W下部分將結(jié)合實(shí)例對(duì)上述的流程中設(shè)及的具體算法和執(zhí)行機(jī)構(gòu)運(yùn)行原理進(jìn)行 闡述。
[0044] 1、教練員人體動(dòng)作信息的采集：
[0045] 在汽車駕駛室副駕駛位置安裝模擬汽車方向盤1、模擬油口踏板2和模擬剎車踏板 3,并在上述模擬汽車方向盤1、模擬油口踏板2和模擬剎車踏板3上分別安裝=個(gè)角度傳感 器(4、5、6)，用W采集教練員的駕駛動(dòng)作數(shù)據(jù)。傳感器采集到的信息W模擬信號(hào)的形式傳遞 給MClK 9 )，并經(jīng)過其內(nèi)部A/D模塊轉(zhuǎn)換后，成為數(shù)字信號(hào)。
[0046] MCU(9)所采集到的信號(hào)數(shù)據(jù)包含了訓(xùn)練員對(duì)方向盤、油口踏板和剎車踏板的操作 信息。
[0047] 令乂=^1、義2、義3^其中義1、義2、^分別為1〇](9)所直接獲得的關(guān)于模擬汽車方向盤 1、模擬油口踏板2和模擬剎車踏板3位置的電壓信號(hào)時(shí)間序列。
[0048] 原始信號(hào)X包含直流分量和隨機(jī)噪聲，在對(duì)駕駛員的人體動(dòng)作參數(shù)進(jìn)行識(shí)別前，需 要先對(duì)其進(jìn)行去均值和濾波處理。其中，駕駛員對(duì)車輛的操作經(jīng)驗(yàn)可知，一般情況下，駕駛 員對(duì)方向盤、油口踏板和剎車踏板的操作較為平穩(wěn)，因此可W認(rèn)為有用信號(hào)主要包含在的 低頻段。
[0049] 對(duì)傳感器信號(hào)的處理流程如圖3所示，可W用W下公式對(duì)傳感器時(shí)間序列信號(hào)X進(jìn) 行濾波：
[0050]
(1)
[0化。其中，K為濾波系數(shù)，4為t時(shí)刻的信號(hào)估計(jì)值。
[0052] 該濾波函數(shù)的幅頻特性為
[0053]
(2)
[0054] 如圖4所示，通過適當(dāng)選取濾波系數(shù)K的大小，即可完成對(duì)傳感器信號(hào)的實(shí)時(shí)濾波。
[0055] MCU(9)對(duì)濾波后的電壓信號(hào)乘W-個(gè)轉(zhuǎn)換系數(shù)C后，即可得到訓(xùn)練員對(duì)模擬方向 盤、油口踏板和剎車的實(shí)際操作量71、72、73，即
[0056]
提）
[0057]其中，Y=[yi、y2、y3]T。
[005引2、執(zhí)行機(jī)構(gòu)控制參數(shù)的重建：
[0059] MCU(9)有兩種不同的執(zhí)行機(jī)構(gòu)控制參數(shù)重建算法，分別對(duì)應(yīng)兩種不同的運(yùn)行模 式。
[0060] 第一種運(yùn)行模式為：當(dāng)訓(xùn)練員與學(xué)員同時(shí)乘坐于駕駛室內(nèi)，且學(xué)員坐在正駕駛位 置、訓(xùn)練員坐在副駕駛位置時(shí)，訓(xùn)練員可W直接對(duì)本實(shí)施例的基于多GPS和角度傳感器的駕 駛?cè)溯o助訓(xùn)練系統(tǒng)進(jìn)行動(dòng)作輸入，W糾正學(xué)員的駕駛行為。
[0061] 第二種運(yùn)行模式為：當(dāng)只有學(xué)員乘坐于駕駛室的正駕駛位置，并于駕校訓(xùn)練場(chǎng)所 練習(xí)科目二相關(guān)項(xiàng)目時(shí)，學(xué)員可能需要基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)提 供規(guī)范的操作W糾正自身的駕駛行為。此時(shí)基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系 統(tǒng)可W直接根據(jù)實(shí)驗(yàn)標(biāo)定數(shù)據(jù)和車輛傳感器檢測(cè)到的車輛狀態(tài)參數(shù)，依照一套特定的算 法，重建出執(zhí)行機(jī)構(gòu)的控制參數(shù)。
[00創(chuàng) 2.1模式一算法
[0063]針對(duì)第一種運(yùn)行工況，基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)能夠直接 根據(jù)=個(gè)角度傳感器(4,5,6)采集得到的數(shù)據(jù)W重建執(zhí)行機(jī)構(gòu)的控制參數(shù)?？刂茀?shù)的重 建采用PID算法：
[0064] u(k)=u(k-l) + {kpe(k)巧 ie 化)+To[e 化)-化化-l)+e(k-2)]} (4)
[00化]其中，e 化）=Wk)-D化） （5)
[0066] Y化)為系統(tǒng)的給定值，即
D化)為系統(tǒng)的實(shí)際輸出，該值 為油口踏板角傳感器、剎車踏板角度傳感器、方向盤角度傳感器采集到的角度值，即
kp為比例系數(shù)矩陣，Ti為積分時(shí)間常數(shù)矩陣，Td為微分時(shí)間常數(shù) 矩陣。
[0067] 2.2模式二算法
[0068] 在科目二場(chǎng)地練習(xí)過程中，教練車一般行駛速度較低，并且練習(xí)場(chǎng)地的地勢(shì)較為 平坦，且駕校內(nèi)的交通環(huán)境一般較為簡單，因此，采用本實(shí)施例的基于多GPS和角度傳感器 的駕駛?cè)溯o助訓(xùn)練系統(tǒng)，可W采用無人輔助培訓(xùn)的模式幫助學(xué)員完成科目二的相關(guān)項(xiàng)目練 習(xí)。其原理如圖5所示。W車輛倒庫練習(xí)為例，基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系 統(tǒng)通過執(zhí)行機(jī)構(gòu)控制學(xué)員的駕駛行為，進(jìn)而在倒庫起點(diǎn)和車庫終點(diǎn)間的路徑節(jié)點(diǎn)上改變車 輛狀態(tài)，完成車輛自動(dòng)倒庫和輔助訓(xùn)練的功能。
[0069] 在該工況下，基于多GI^和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)，需要利用GPS(7)和 GPS(S)采集車輛的空間位置信息和狀態(tài)參數(shù)。
[0070] 其具體算法為：
[0071] 1)車輛的空間位置信息獲取
[0072] 利用安裝于車輛前端和后端的GPS(7)和GPS(S)采集到車輛前端和后端的坐標(biāo)位 置(Xi，yi)G，其中i = l，2。
[0073] 此時(shí)，基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)直接獲取的坐標(biāo)為全球坐 標(biāo)，需要將(Xi，yi)G轉(zhuǎn)換為倒庫場(chǎng)地的局部坐標(biāo)(Xi，yi)B，轉(zhuǎn)換的算法為：
[0074]
終）
[0075] 其中，a和b分別為倒庫起點(diǎn)的全球坐標(biāo);0為倒庫場(chǎng)地的局部坐標(biāo)系與全球坐標(biāo)系 之間的夾角。
[0076] 由于駕校訓(xùn)練場(chǎng)一般處于空曠地帶，因此GPS RTK&DGI^的測(cè)量精度可W達(dá)到厘米 級(jí)。為了提高位置測(cè)量的精度，可W采用卡爾曼濾波器等估計(jì)算法對(duì)車身的狀態(tài)參數(shù)進(jìn)行 估計(jì)。
[0077] 利用獲得的坐標(biāo)信息，基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)還可W獲 得車輛的橫擺角度4，即
[0078
巧)
[0079] 利用W上算法，可W精確地估算出車輛在倒庫場(chǎng)地的局部坐標(biāo)系中的原點(diǎn)位置， 從而為后續(xù)的路徑計(jì)算提供精確的邊界條件。
[0080] 2)倒庫路徑的生成與計(jì)算
[0081] 當(dāng)車輛從停車點(diǎn)開始倒庫時(shí)，其起點(diǎn)不是固定的，而是大致分布在一 個(gè)圓形范圍內(nèi)。運(yùn)是由于學(xué)員在將車輛停泊到倒庫起始線時(shí)的行為誤差所導(dǎo)致的。并且，車 輛在開始倒庫時(shí)，其車輛的橫擺角度4 一般情況下也不為零。因此，基于多GI^和角度傳感器 的駕駛?cè)溯o助訓(xùn)練系統(tǒng)需要根據(jù)起始邊界條件，計(jì)算出一條倒庫路徑。
[0082] 實(shí)際上，倒庫路徑除了受到車輛初始狀態(tài)參數(shù)影響外，還受到路徑節(jié)點(diǎn)的要求限 審IJ。其原因是，車輛在倒庫的過程中，還必須要經(jīng)過某些節(jié)點(diǎn)，W使學(xué)員能通過后視鏡看到 各個(gè)轉(zhuǎn)向點(diǎn)處的參照物，運(yùn)就進(jìn)一步給出了倒庫路徑的邊界條件。
[0083] 通過考慮車輛的初始狀態(tài)參數(shù)，并在各節(jié)點(diǎn)處給定車輛橫擺角如，本發(fā)明利用一 種基于埃米爾特插值法的算法生成車輛的倒庫路徑。算法的具體過程如下：
[0084] (1)通過實(shí)驗(yàn)標(biāo)定，人為地在每個(gè)轉(zhuǎn)向節(jié)點(diǎn)處用畫一個(gè)經(jīng)過該節(jié)點(diǎn)，且W該節(jié)點(diǎn)為 中屯、的小圓弧，用W擬合車輛在該點(diǎn)附近轉(zhuǎn)向所駛過的軌跡。從而給定了系統(tǒng)在每個(gè)轉(zhuǎn)向 節(jié)點(diǎn)處的轉(zhuǎn)向起點(diǎn)(Ail,Bii)和終點(diǎn)(Ai2，Bi2)，W及車輛在起點(diǎn)和終點(diǎn)對(duì)應(yīng)的車輛橫擺角V i 和 iTi。
[0085] (2)利用埃米爾特插值法，擬合出^11，811)和(4(1-1)2,8(1-1)2)之間的曲線。由于給 定了兩個(gè)點(diǎn)的坐標(biāo)位置及其各自的斜率，因此可W用一條=次曲線來擬合出兩點(diǎn)間的路徑 曲線，曲線的具體表達(dá)公式如下：
[0086] 巧）
[0087]
[008引
[0089]
[0090]
[0091] 重復(fù)利用上述公式，即可把經(jīng)過各個(gè)所需節(jié)點(diǎn)的具體路徑抽象為數(shù)學(xué)表達(dá)式，從 而使系統(tǒng)能根據(jù)車輛的初始狀態(tài)。重建出當(dāng)前車輛所需的倒庫路徑。
[0092] 有了路徑函數(shù)后，還可W通過實(shí)驗(yàn)標(biāo)定，標(biāo)定出車身幾何中屯、經(jīng)過各點(diǎn)處的速度、 加速度要求，為油口踏板輔助駕駛執(zhí)行器、剎車踏板輔助駕駛執(zhí)行器的控制提供目標(biāo)參數(shù)。
[0093] 2.2.3車輛控制參數(shù)的計(jì)算
[0094] 生成倒車路徑后，基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)需要依據(jù)車身 狀態(tài)參數(shù)和車輛的空間位置計(jì)算出車身控制參數(shù)，即油口踏板輔助駕駛執(zhí)行器、剎車踏板 輔助駕駛執(zhí)行器、方向盤的轉(zhuǎn)角山、(12、(13。因此，運(yùn)里需要設(shè)計(jì)一個(gè)控制算法^(11、(12、(13)，使 車輛行駛的曲線與車輛計(jì)算生成的倒庫曲線S盡可能接近。
[00%]在駕校內(nèi)，車輛處于倒庫或其他項(xiàng)目練習(xí)時(shí)，基于多GI^和角度傳感器的駕駛?cè)溯o 助訓(xùn)練系統(tǒng)通過安裝于車輛的兩個(gè)GPS(7,8)獲取車輛當(dāng)前的幾何中屯、位置(x，y)和車輛的 橫擺角度攀，從而獲取車輛當(dāng)前的狀態(tài)參數(shù)。
[0096]有了本實(shí)施例的基于多GI^和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)所提供的目標(biāo)曲 線函數(shù)和車輛的狀態(tài)參數(shù)后，還需要設(shè)計(jì)一個(gè)目標(biāo)跟蹤算法，使車輛的狀態(tài)參數(shù)與目標(biāo)函 數(shù)保持一致。由于車輛在訓(xùn)練場(chǎng)內(nèi)倒庫時(shí)，可W認(rèn)為車輛在大多數(shù)時(shí)間處于勻速狀態(tài)，運(yùn)里 采用如下的算法來控制車輛曲線行駛狀態(tài)：
[00971
巧)
[009引其中：ei化）=A X+A y，A X = X-Xd, A y = 5f-yd，（Xd，yd)為過車輛幾何中屯、點(diǎn)的路徑 曲線的法線與路徑曲線的交點(diǎn)；
[0099]
[0100] e〇(k) = ei 化)+62 化）
[0101] 由于該控制算法需要整定6個(gè)參數(shù)，并且設(shè)計(jì)對(duì)兩個(gè)目標(biāo)量進(jìn)行優(yōu)化，運(yùn)里采用基 于BP神經(jīng)網(wǎng)絡(luò)的方法對(duì)運(yùn)些參數(shù)進(jìn)行整定。B巧巾經(jīng)網(wǎng)絡(luò)PID控制算法的結(jié)構(gòu)示意圖如圖6所 示，B巧巾經(jīng)網(wǎng)絡(luò)PID控制算法在計(jì)算機(jī)上的仿真結(jié)果如圖7和圖8所示，其具體方法如下：
[0102] ①選定神經(jīng)網(wǎng)絡(luò)的輸入層節(jié)點(diǎn)數(shù)M和隱藏層節(jié)點(diǎn)數(shù)Q，初始化各層的加權(quán)系數(shù) wJf(O)、W;'f(0).選定學(xué)習(xí)速率n和慣性系數(shù)口；
[0103] ②采樣得到系統(tǒng)的實(shí)際參數(shù)ri化)和目標(biāo)yi化)參數(shù)輸入誤差eo化）=ri化)-yi化）；
[0104] ③對(duì)ri化）、yi化)和eo化)進(jìn)行歸一化處理，作為BP神經(jīng)網(wǎng)絡(luò)的輸入；
[010引④計(jì)算BP神經(jīng)網(wǎng)絡(luò)各層的輸入、輸出，輸出層的輸出為4。1、1'11^01、1^2、1'12^02。其 中，隱藏層的輸入和輸出分別為：
[0106] (1 巧
[0107] (II.)
[0108] 其中，為隱藏層第j個(gè)神經(jīng)元到第i個(gè)神經(jīng)元的加權(quán)系數(shù);f[ ?]表示活化函 數(shù)，上標(biāo)的（1)、（ 2)、（ 3)分別表示輸入層、隱藏層和輸出層。
[0109] 輸出層的輸入輸出分別為：
[0110] 巧巧
[0111] 巧巧
[0112] wf 3表示輸出層第i個(gè)神經(jīng)元到第I個(gè)神經(jīng)元的加權(quán)系數(shù)，g[ ?]表示活化函數(shù)。
[0113] ⑤計(jì)算PID控制器的輸出U化）；
[0114] ⑥計(jì)算修正輸出層的加權(quán)系數(shù)：
[0115]
[0116]
[0117]
[011 引
[0119]
[0120] ⑧令k = k+l，返回第二步進(jìn)行迭代。
[0121] 由于BP神經(jīng)網(wǎng)絡(luò)的收斂速度較慢，因此一般情況下還需要配合專家系統(tǒng)，從而實(shí) 現(xiàn)車輛的快速控制。由于運(yùn)類算法非本發(fā)明所側(cè)重內(nèi)容，因此運(yùn)里不再寶述。W上倒庫輔助 訓(xùn)練的算法實(shí)例可W推廣到其他的科目二練習(xí)實(shí)例中。
[0122] 3、執(zhí)行機(jī)構(gòu)對(duì)學(xué)員駕駛行為的糾正
[0123] 基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)得到控制參數(shù)后，即可通過控制 油口踏板輔助駕駛執(zhí)行器13、剎車踏板輔助駕駛執(zhí)行器14和轉(zhuǎn)向助力電機(jī)15W控制學(xué)員的 操作行為。
[0124] 油口踏板輔助駕駛執(zhí)行器13和剎車踏板輔助駕駛執(zhí)行器14的具體結(jié)構(gòu)如圖2所 示，分別安裝于油口踏板和剎車踏板上方?；诙郍I^和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng) 對(duì)伺服電機(jī)發(fā)送出控制信號(hào)后，伺服電機(jī)驅(qū)動(dòng)驅(qū)動(dòng)短板，將學(xué)員的腳往油口或剎車踏板壓。 同時(shí)，基于多GI^和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)通過與車載中央控制器12的通信，得 到當(dāng)前油口、剎車踏板的角度，從而實(shí)現(xiàn)閉合控制。其中，油口踏板輔助駕駛執(zhí)行器13和剎 車踏板輔助駕駛執(zhí)行器14中的彈性結(jié)合套用于提供一定的預(yù)緊力，W消除驅(qū)動(dòng)短板和學(xué)員 腳部之間的空隙。
[0125] 為了輔助學(xué)員練習(xí)對(duì)車輛方向的操控，基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn) 練系統(tǒng)通過與車載中央控制器12互聯(lián)W獲取對(duì)方向盤的轉(zhuǎn)向助力電機(jī)15的控制鏈路。通過 將轉(zhuǎn)向助力電機(jī)的控制參數(shù)傳遞給車載中央控制器12,基于多GPS和角度傳感器的駕駛?cè)?輔助訓(xùn)練系統(tǒng)從而經(jīng)由其實(shí)現(xiàn)對(duì)車輛行駛方向的控制。此時(shí)，學(xué)員可W將手放置于車輛方 向盤上。方向盤會(huì)反拖學(xué)員的手部進(jìn)行運(yùn)動(dòng)，使學(xué)員感受方向盤的轉(zhuǎn)動(dòng)角度。
[0126] 揚(yáng)聲器10可W在模式二中起到提醒學(xué)員的作用。當(dāng)車輛到達(dá)某一參照點(diǎn)后，基于 多GI^和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)通過揚(yáng)聲器10提醒學(xué)員注意觀測(cè)各個(gè)參照物。
【主權(quán)項(xiàng)】
1. 一種基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)，包括汽車駕駛室副駕駛位置 安裝的模擬汽車方向盤（1)、模擬油門踏板(2)和模擬剎車踏板(3)，其特征在于，所述的模 擬汽車方向盤（1)、模擬油門踏板(2)和模擬剎車踏板(3)分別通過第一角度傳感器(4)、第 二角度傳感器(5)和第三角度傳感器(6)分別連接Μ⑶(9)，在MCU(9)上連接有第一 GPS(7)、 第二GPS(8)和揚(yáng)聲器（10)，MCU(9)上還連接有CAN總線控制器（11)、油門踏板輔助駕駛執(zhí)行 器(13)和剎車踏板輔助駕駛執(zhí)行器(14);其中： CAN總線控制器（11)依次連接車載中央控制器（12)、轉(zhuǎn)向助力電機(jī)（15)、方向盤（18)、 方向盤位置傳感器(20); 油門踏板輔助駕駛執(zhí)行器（13)依次連接油門踏板（16)和油門踏板位置傳感器（19);其 中油門踏板位置傳感器(19)與車載中央控制器(12)相連接； 剎車踏板輔助駕駛執(zhí)行器（14)依次連接剎車踏板（17)和剎車踏板位置傳感器(21);其 中剎車踏板位置傳感器(21)與車載中央控制器(12)相連接。2. 如權(quán)利要求1所述的基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)，其特征在于， 所述的油門踏板輔助駕駛執(zhí)行器（13)和剎車踏板輔助駕駛執(zhí)行器(14)均包含伺服電機(jī)、彈 性結(jié)合套和驅(qū)動(dòng)短板。3. 權(quán)利要求1或2所述的基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)的控制算法， 其特征在于，首先根據(jù)GPS數(shù)據(jù)計(jì)算車輛的初始位置和初始側(cè)偏角在內(nèi)的初始邊界條件，然 后計(jì)算出所需的車輛行進(jìn)路徑，再利用BP神經(jīng)網(wǎng)絡(luò)算法擬合出PID控制器的控制參數(shù)用于 控制車輛的行進(jìn)路徑和行進(jìn)姿態(tài)，同時(shí)同樣采用PID控制器計(jì)算出執(zhí)行器所需的對(duì)的油門 踏板、剎車踏板的操作量。4. 采用權(quán)利要求1或2所述的基于多GPS和角度傳感器的駕駛?cè)溯o助訓(xùn)練系統(tǒng)的訓(xùn)練方 法，其特征在于，在有人輔助訓(xùn)練模式或無人輔助訓(xùn)練模式下，采用不同的控制算法實(shí)現(xiàn)對(duì) 學(xué)員的輔助訓(xùn)練，具體的步驟為： 步驟一，學(xué)員或教練員根據(jù)實(shí)際需求，選擇系統(tǒng)的工作模式； 步驟二，當(dāng)工作在有人輔助訓(xùn)練模式時(shí)，利用安裝在汽車駕駛室副駕駛位置模擬汽車 方向盤、油門踏板和剎車踏板，以及在這些部件上安裝角度傳感器，采集教練員的駕駛動(dòng)作 數(shù)據(jù)； 步驟三，將采集到的教練員動(dòng)作數(shù)據(jù)在MCU中重建為對(duì)該執(zhí)行機(jī)構(gòu)的控制數(shù)據(jù)； 步驟四，當(dāng)工作在無人輔助訓(xùn)練模式時(shí)，根據(jù)實(shí)驗(yàn)標(biāo)定的數(shù)據(jù)和角度傳感器測(cè)量得到 的當(dāng)前車輛信息，計(jì)算出車輛的行進(jìn)路徑，并利用結(jié)合BP神經(jīng)網(wǎng)絡(luò)算法的PID控制器計(jì)算執(zhí) 行機(jī)構(gòu)的控制參數(shù)； 步驟五，輔助駕駛執(zhí)行機(jī)構(gòu)依據(jù)MCU的控制信號(hào)，在學(xué)員的手部、腳步施加一定的外力， 使學(xué)員能依照教練員的手腳動(dòng)作完成對(duì)車輛的駕駛操作，從而強(qiáng)化駕駛行為學(xué)習(xí)，同時(shí)系 統(tǒng)通過各個(gè)角度傳感器對(duì)執(zhí)行機(jī)構(gòu)的執(zhí)行情況進(jìn)行檢測(cè)，對(duì)控制輸出信號(hào)進(jìn)行修正； 步驟六，無人輔助訓(xùn)練模式中，利用GPS提供的數(shù)據(jù)以及實(shí)驗(yàn)標(biāo)定數(shù)據(jù)得到車輛路徑計(jì) 算所需的邊界條件，并利用六參數(shù)PID計(jì)算車輛方向的控制參數(shù)。5. 如權(quán)利要求4所述的訓(xùn)練方法，其特征在于，在步驟二中，訓(xùn)練員通過汽車駕駛室副 駕駛位置安裝的模擬方向盤（1 )、模擬油門踏板(2 )、模擬剎車踏板(3)進(jìn)行操作，輸入動(dòng)作 信號(hào);這些動(dòng)作信號(hào)被安裝于模擬方向盤（1)、模擬油門踏板(2)、模擬剎車踏板(3)上的角 度傳感器(4)~(6)所捕獲，并以模擬信號(hào)的形式傳遞到MCU(9)。6.如權(quán)利要求4所述的訓(xùn)練方法，其特征在于，在步驟五中，MCU(9)將數(shù)據(jù)經(jīng)由CAN總線 控制器（11)傳遞給車載中央控制器（12)，從而控制方向盤（18)的運(yùn)動(dòng)，并帶動(dòng)學(xué)員的雙手 進(jìn)行運(yùn)動(dòng);油門踏板輔助駕駛執(zhí)行器(13)和剎車踏板輔助駕駛執(zhí)行器(14)直接受到MCU(9) 的控制，從而對(duì)學(xué)員的雙腳施加一定外力，并驅(qū)動(dòng)雙腳對(duì)油門踏板（16)和剎車踏板（17)進(jìn) 行操作;最后，方向盤（18)、油門踏板（16)和剎車踏板（17)的運(yùn)動(dòng)狀態(tài)信息又分別由方向盤 位置傳感器(20 )、油門踏板位置傳感器（19 )、剎車踏板位置傳感器(21)進(jìn)行采集，然后經(jīng)由 車載中央控制器(12)、CAN總線控制器(11)傳遞給MCU(9)，從而形成閉合控制回路。
【文檔編號(hào)】G09B9/052GK106023715SQ201610423473
【公開日】2016年10月12日
【申請(qǐng)日】2016年6月15日
【發(fā)明人】韓毅, 吳學(xué)勤, 李娟娟, 宋文鳳, 王滿力, 魏芳, 張學(xué)文
【申請(qǐng)人】長安大學(xué)