基于雷達測距的自動泊車目標檢測與定位設計

李升凱，吳長水

（上海工程技術大學 機械與汽車工程學院，上海 201620）

0 引言

在汽車新四化的大趨勢下，智能化作為其關鍵領域，著力于自動駕駛的發展將極大改善人類的出行感受。由于車位窄、停車環境復雜多樣、駕駛員技術水平不一等原因，越來越多的人會期望將自動泊車系統運用到汽車上。目前，國內外自動泊車系統各有不同，但都是基于視覺、雷達或二者數據融合作為感知層方案。純視覺的泊車方案應用場景受限，為讓自動泊車系統可以得到普遍應用，采用超聲波雷達和視覺數據融合的方案無疑是一種合理的選擇。對障礙物的測距和定位、從而實現對外界的高精度感知，對上層的決策規劃算法至關重要。

超聲波存在傳播速度受外界環境溫度較高、時間不同步、散射角大造成的方向性差、對不同路面（如柏油路、草地、碎石路面）回波敏感性差異大等問題，導致測距與定位精度不足，有學者利用雙比較整流電路有效減小了盲區，并且設計出軟件檢測回波峰值的方法。圍繞在惡劣環境中提高超聲波精度的主題，已有學者提出了基于回波的自適應延時估計算法。還有學者為了增大換能器發射功率，提高回波能量，設計了一款新型放大電路，并對換能器進行阻抗匹配，提高了測距精度與量程。以上方法分別對硬件電路和軟件算法進行了優化，提高了測量精度，但是對自動泊車中復雜的場景仍難以實現自適應測距，例如在碎石路面或者草坪上的泊車場景，會出現誤檢的情況，影響泊車算法的高效運行。因此，本文基于傳感器驅動芯片各種特征參數、環境溫度、驅動策略等要素提出一種實時矯正測距與定位算法，適用于復雜的泊車環境以獲得準確的障礙物信息。

1 系統硬件設計

1.1 傳感器布置

本系統采用12 路超聲波雷達，前后各4 個短距離雷達用來在自動泊車過程中探測障礙物，避免發生碰撞。左右各2 個長距離雷達用來探測車位及車位周邊情況，為自動泊車的路徑規劃算法提供準確的路徑數據信息。傳感器布置示意如圖1 所示。圖1 中，紅色雙箭頭和藍色雙箭頭代表自發自收，綠色單箭頭代表不發只收，用來有效定位與測距。

圖1 傳感器布置示意圖Fig.1 Schematic diagram of sensors layout

1.2 超聲波收發電路

傳感器由推挽中心抽頭變壓器供電，以獲得最大效率。傳感器驅動器建立供電電壓VTANK 和傳感器連接之間的接口，如圖2 所示。

圖2 超聲波雷達驅動框圖Fig.2 Ultrasonic radar driver block diagram

發射模式下，通過外部變壓器和傳感器產生超聲波信號，閉合SW給變壓器的中心抽頭供電，通過控制SW與SW的交替通斷，為變壓器供電、進而驅動壓電陶瓷振動產生超聲波，其中2 個開關的通斷頻率就是發射頻率。接收模式下，斷開SW，消除傳感器供電的噪聲耦合。DRVS 通過下拉電阻接地，限制電流。將SW與SW斷開，此時變壓器的一次繞組只與中心抽頭相連，而回波信號被傳感器引腳AINS 和AING 收集，信號經過模擬放大器、模數轉換，再進一步經過數字濾波器的處理，降低了傳感器對頻帶以外不想要信號的靈敏度。這里，AINS 接信號線，AING 接傳感器地，如圖3 所示。

圖3 系統外部電路Fig.3 System external circuit

選用IO 接口驅動的探頭，探頭內置elmos 驅動芯片，主MCU 和IC 之間采用三線物理接線，分別是供電、數據線、接地。參照驅動芯片研發的通信協議，設計底層驅動，通過控制高、低電平的脈寬實現探頭驅動IC 與MCU 之間的多命令通信。選用英飛凌TC277 作為主控芯片，TC277 具有3 個CPU 核，CPU 最高頻率為200 MHz。同時設有片上存儲器和豐富的外設資源，如保證數據安全傳輸的64 通道DMA 控制器、通用定時器模塊（GTM）等。

2 實時矯正測距與定位設計

實時矯正測距與定位方法主要是對系統的多路超聲波雷達設計合理的發波時序，融合溫度矯正、時間同步矯正、環境自適應矯正，可在情況復雜的環境中實現更加準確的測距與定位。

2.1 IC 參數配置

利用elmos 開發的上位機軟件實現對超聲波雷達的回波信號探測與參數配置，碎石路面回波探測圖如圖4 所示。圖4 中，橫坐標表示距離，縱坐標表示回波強度，紅色線條代表設定的閾值曲線，綠色線條代表回波信號，藍色線條代表IO 高低電平信號。當回波信號高于閾值曲線時，IO 線被拉低，此時MCU 即可檢測到障礙物。閾值可通過上位機軟件進行配置，以獲得準確的回波檢測結果。圖4 中未設置目標障礙物，用來觀測復雜路面對障礙物的回波影響。由圖4 可見，在2000 mm 和2600 mm 處碎石回波信號較大，有較大可能對障礙物的檢測造成影響，所以需要提高相應閾值及優化IC 的濾波降噪配置。

圖4 碎石路面回波信號檢測Fig.4 Echo signals detection on the gravel pavement

改變閾值參數和濾波降噪都是通過elmos 的開發板利用JTAG 接口對IC 內部寄存器和EEPROM進行配置，最終得到一組理想配置參數。經過多次參數配置，最終同樣在碎石路面放置標準PVC 管，回波信號如圖5 所示。經過配置合理的閾值、濾波降噪后，碎石的回波信號衰減較強，同時可在2500 mm 的位置依然可穩定檢測到PVC 管回波信號。

圖5 優化后碎石路面遠距障礙物回波信號檢測Fig.5 Detection of echo signals from long-distance obstacles on the gravel pavement after optimization

優化后碎石路面近距障礙物回波信號檢測結果如圖6 所示。由圖6 可知，聲波強度隨時間逐漸減弱，圖5 的2500 mm 處和圖6 的1400 mm 處回波信號形成明顯對比，距離越遠、回波信號越弱，因此為了能適應近、中、遠距離的最優測量，需要設計合理矯正算法。

圖6 優化后碎石路面近距障礙物回波信號檢測Fig.6 Detection of echo signals from close obstacles on the gravel pavement after optimization

2.2 實時矯正算法設計

2.2.1 溫度矯正

超聲波速率受環境溫度影響極大，溫度每變化1 ℃，速度變化0.6 m/s，進而影響對障礙物的檢測。超聲波速率與溫度關系如式（1）所示：

其中，是超聲波速率；T是當前環境溫度。

當前環境溫度由超聲波雷達的溫度和車輛外部溫度融合，前者溫度可由MCU 發送溫度獲取命令取得，車輛外溫通過CAN 總線獲取。

設定5 s 獲取一次超聲波雷達溫度，設置冗余機制，同時獲取多個超聲波雷達溫度，設定所得各傳感器溫度偏差在10%范圍內數據有效。研究推得的數學公式可寫為：

其中，T是超聲波雷達溫度，T是車輛外溫。

2.2.2 時間同步矯正

由于MCU 驅動多個超聲波雷達會產生計時不同步的問題，導致測距精度降低，所以需要設計時間同步矯正算法。此處需要用到的數學公式可寫為：

其中，是矯正后的回波時間；是未矯正的回波時間；t是當前發波通道的發波時間戳； t是當前收波通道的時間戳。

2.2.3 環境自適應矯正設計

如前文所述，為減少復雜環境對回波檢測造成的干擾，同時滿足不同距離設置不同的檢測參數以避障的需求，本文設計了一種不同環境下配置3 組不同測量參數以滿足不同環境狀況和探測距離的探測算法。由于存在安裝位置、角度、震動的偏差，環境自適應算法需在溫度、時間矯正的基礎上進行整車環境標定，具體的標定方法如下：使用業界標準障礙物PVC 管對超聲波進行多位置的多次測量，記錄各位置平均距離。

設定10 cm 為界，通過兩點線性插值法，得到2 個校準參數。標定工作在臺架上進行，將超聲波雷達按照設計的位置進行安裝。臺架模型實物圖見圖7。

圖7 臺架模型Fig.7 Bench model

這里，進一步求得的數學公式為：

其中，和是校準參數；是經溫度和時間矯正后的距離；是經標定測試校準后的距離。

本系統使用的超聲波雷達可通過IO 設定其內部寄存器對應的閾值參數、測量參數。其中，閾值參數設定包括各個距離段的回波檢測閾值，測量參數設定包括發射信號的配置、回波檢測方式、數字增益、降噪配置等。

所用超聲波雷達支持在線存儲3 組發射信號配置參數，包括發射信號脈沖數、測量時間、閾值放大系數，根據近、中、遠及泊車路面（草地、柏油路、碎石路面）不同環境的回波特性進行多次參數調試，確定不同環境下最優參數配置，大大提高了測距定位系統的可靠性。

2.2.4 軟件設計

傳感器IC 內有寄存器和EEPROM，分別用來存儲特定配置參數。域控制器通過一根IO 硬線與IC進行通信，控制IO 線高、低電平的時序可實現多命令通信。通過前期進行的測試，確定在不同環境下各自較優配置參數和各階段閾值水平，通過發送MEAS＿SETUP 和THRES＿SETUP 命令分別設置較優參數與閾值。此后發送READ＿STATUS 命令可獲取狀態信息，隨之就是發送SEND＿X 或RECEIVE＿X命令進行發波或收波。

實時矯正算法邏輯流程如圖8 所示。由圖8 可知，算法流程基于參數配置成功，上電后IC 寄存器設為默認值，首次收波或發波命令基于默認配置，任意一個傳感器發波MCU 開始計時，回波檢測是MCU 捕獲IO 的跳變沿，得到回波時間與幅值等信息。得到初步回波時間，下一步將進行溫度的矯正。為了提高系統可靠性、避免因單個探頭故障而產生漏檢，進行多探頭溫度的冗余比較。若溫度矯正失敗、則重新進行首次收發波命令，成功、則進行時間同步矯正計算出初步距離。根據標定得到的校準系數進行線性插值對距離進行校準，當校準后的距離≤50 cm 時，配置ProfileA 的測量參數進行發波或收波；當校準后的距離≤200 cm時，配置ProfileB的測量參數進行發波或收波；當校準后的距離200 cm 時，配置ProfileC 的測量參數進行發波或收波；系數校準后得到的測量距離輸入自動泊車算法中。

圖8 實時矯正算法邏輯流程圖Fig.8 Real-time correction algorithm logic flow chart

2.3 目標定位算法

定位原理設計如圖9 所示，展示了一發三收的定位原理，圖9 中只展示2 個目標物用來示意，可實現左、右兩側多目標同步定位。在實際情況中，由于超聲波呈不規則錐形包絡線發射，同一物體在聲波反射的可探測角度內可探測其多個部位。在二維坐標系中，設置為自發自收探頭位置、并設置為坐標原點，、為只收探頭位置，探頭之間位置、即、由安裝位置已確定。距離、、的值可由超聲波測出，目標物1 的位置可由余弦定理得出：

圖9 定位原理設計Fig.9 Positioning principle design

其中，、分別為目標物的橫縱坐標。

2.4 發波時序設計

采用的超聲波雷達有2 種測量模式，分別是：自發自收和不發只收。

為了更加準確定位障礙物，可利用一發多收的方法實現三角定位。為了避免相鄰探頭發波的干擾、同時提高測距定位的效率，設計合理的發波時序。前后8 個短距超聲波探測距離為15～250 cm，用來避撞；左右4 個長距超聲波的探測距離為30～500 cm，用來探測車位信息。設計每次一個短距超聲波雷達和2 個長距超聲波雷達同時發波，發波的短距雷達及臨邊短距雷達和最近一次發波的長距雷達同時收波，需要循環8 次即可遍歷所有雷達。文中設計了系統收發波時序如圖10 所示。

圖10 超聲波雷達收發波時序示意圖Fig.10 Schematic diagram of the time sequence of transmitting and receiving waves of ultrasonic radars

圖10 中表示出車頭4 個短距雷達及車后側2個長距雷達的收發波時序，其余6 個雷達收發波時序原理相同，圖中的探頭號見圖1 傳感器布置圖。

2.5 Free Space 設計

基于目標定位與測距算法設計Free Space 區域用來實現超聲波自動泊車。前后共8 個UPA 雷達、水平發射角為120°，設定最大探測距離為2.5 m，左、右共4 個APA 雷達、水平發射角為80°，設定最大探測距離為5 m。在泊車過程中，通過串口打印并保存多組探測數據。在每個探頭水平發射角內，等角度取20 組探測數據，超過最大探測距離的位置記為最大探測距離。這里給出的Free Space 展示如圖11 所示，射線區域內即為自由泊車區域。

圖11 Free Space 展示Fig.11 Free Space display

3 實驗驗證與分析

以一款純電動汽車為載體，利用智能駕駛域控制器集成軟件，在室外靜態條件下進行算法驗證。選擇業界標準PVC 管作為被測障礙物，選擇360 cm×250 cm 規格的標定布，如圖12 所示。

圖12 實車驗證Fig.12 Real vehicle verification

對系統功能進行測試，為了驗證實時矯正算法的有效性，本文分別針對矯正前與矯正后進行了2次對比測試，選擇汽車前面4 個傳感器進行測量。在標定布上隨機擺放PVC 管，設定自發自收模式，利用串口在PC 機得到實時測量數據，實際距離用激光測距儀標定。

未矯正實測誤差結果如圖13 所示。按上述測量方法測得數據見表1。表1 中，激光測距儀測量的是探頭到PVC 管同一高度中心位置的距離。測量距離分別是同一位置3 次的測量值，平均值則為3 次測量距離的平均后的結果值。可見，未經矯正過的超聲波雷達測距最大誤差可達8 cm，難以實現泊車過程中準確的測量與定位。

表1 未矯正的短距超聲波雷達實測數據Tab.1 Measured data of uncorrected short-range ultrasonic radars

圖13 未矯正實測誤差圖Fig.13 Uncorrected measured error graph

集成了實時矯正算法后，采用與上面相同的測量方法進行實車測試，所得數據見表2。由表2 可見，最大誤差控制在1.5 cm以內，滿足自動泊車算法中對測距與定位的精度要求。研究后得到的矯正后實測誤差結果如圖14 所示。

圖14 矯正后實測誤差圖Fig.14 Measured error graph after correction

表2 矯正后短距超聲波雷達實測數據Tab.2 Measured data of short-range ultrasonic radars after correction

4 結束語

本文利用TC277 芯片的豐富資源和elmos 高性能探頭IC，配置IC 各個參數并基于實時矯正算法設計了12 路超聲波雷達自動泊車測距與定位系統，應用于泊車ECU 中，經過實車對前方4 個短距超聲波雷達的功能測試，在20～250 cm 范圍內可將測量精度提高到1.5 cm，證明該算法較大地提高了檢測精度，可適應于復雜環境中的泊車應用，為實現視覺與超聲波雷達數據融合以進行可視化距離檢測提供有效幫助。