一種基于狀態空間的智能車控制算法

陳力成 何新 彭一凡 黎文卓

（南京理工大學 江蘇省南京市 210000）

1 引言

隨著技術發展，高性能的處理器越來廉價，處理速度越來越快，更多的智能車比賽選擇使用攝像頭，而非傳統的電磁導引，工業中也對AGV 提出了更高的要求。目前智能車控制大多采用ARM 的m 系列單片機，少有針對ARM 的a 系列的高主頻處理器算法，同時對于智能車視覺處理的位姿解算算法對于彎道效果較差。本文選擇樹莓派4b 對智能車基于攝像頭的控制進行研究，充分利用處理器的運算速度優勢，提出一種基于狀態空間和機器學習的算法以優化控制，做視覺導引的位姿獲取和最優控制的求解工作。以提高智能車的穩定性和速度。

2 基于狀態空間的智能車控制算法

本文采用狀態空間和機器學習的控制算法實現給定的狀態評價函數最優解求算。基于攝像頭首先進行車身定位算法的研究，著重解決在彎道上對車身位姿的解算。其次對基于狀態空間的控制方法進行研究，并且提出相應的狀態轉移評價函數和優化算法。

2.1 基于圖像的位姿解算算法

2.1.1 相機投影模型[1]

為了將像素平面映射到地面坐標系需要用如下攝像頭內參矩陣，一般可以通過相機的標定或者出廠商直接給定得到，如式（1）所示[2]：

可見有了相機內參矩陣后可以由Z，u,v 求解出X,Y 的真值。設光心坐標系的z 軸與地面在yoz 平面的夾角為β，設地面平面[Xe,Ye]坐標系E 原點位于光心坐標系與地面的交點E，光心到E的距離為Zg,Z 由三角形相似解算，對智能車模型來說由于相機與地面的夾角是固定的，所以將Y 乘以cosβ 就可以將X,Y 從光心坐標系映射到地面平面。從而完成從像素平面到地面平面的變換：

2.1.2 提取引導線[3]

角點檢測源于一種人們的一種非常樸素的思想，即圖像在各個方向灰度有明顯變化。算法的核心思想是利用滑動的高斯窗口移動來判斷灰度發生的變化，如果在各個方向移動窗口都會發生比較大的灰度改變則認為遇到了角點。由于運算能力有限，選擇使用泰勒級數化簡公式，即用一階泰勒展開近似I（x+u,y+v）,略去二階及以上分量，寫成矩陣形式得到如下公式[4]：

圖1：相機投影模型示意圖

圖2：車身的動態坐標系

圖3：直接叉乘誤差原因圖

考慮到引導線（本文采用白色引導線為例）在圖像中對于小高斯窗口表現為邊緣，對于大高斯窗口表現為類似角點，所以這里選擇使用小高斯窗口檢測x 方向上升沿和下降沿來提取，并且根據引導線的y 方向連續性進行遞推提取引導線。

圖4：算法流程圖

當攝像頭采集回一幀圖像時，首先對其進行像素平面到地面平面的變換，得到變換后的矩陣I。對于像素平面窗口權重矩陣w 僅在x 方向上有值，將窗口沿x 軸移動，計算每個窗口的灰度值得到窗口灰度數組array,選取其中相鄰的兩正負脈沖，并對應到I 中計算其間距離是否接近線寬由此確定引導線的位置。

若窗口取3*3 矩陣則權重矩陣w 應該為下式所示：

其中k 為常數，用于放大變化率。計算出該y 值的引導線位置后，由引導線的連續性可知下一個y 值中引導線的x 坐標應該接近該y 值中引導線的x 坐標，當計算出幾個y 值中引導線的x 坐標后我們可以求出x(y)的導數，便可以預測下一個y 值中引導線的x 大概坐標。

由此可以大大的減小計算量，同時還消除了當圖像中出現多條線時對引導線提取的困難， 而且當多條引導線相交時還可以通過x(y)的導數來預測交點處的引導線的x 坐標。

2.1.3 位姿解算

2.1.3.1 直線位姿解算

直線的位姿解算較為簡單本文不再詳述。

2.1.3.2 曲線位姿解算

相對于直線的位姿解算，曲線的位姿解算要復雜的多，若用旋轉向量和位移向量描述則發現其只有一個旋轉自由度和一個位移自由度，且旋轉自由度可由IMU 解算。但常用的攝像頭位姿解算如DLT、PNP 算法[5]需要有配對點對，但是智能車的攝像機引導線相似度大很難找到配對點對。

由于攝像頭只能看到車前的引導線而無法觀測到車身所處的引導線位置，我們只能根據歷史的數據推算出車身相對于線的估計位置，即在運動過程中通過耦合IMU 元件進行動態的建圖。

由于智能車是一個動態系統，我們很難建立基于世界坐標系的精確坐標，所以還是選擇建立相對車身的動態坐標系。如圖2 所示P 為與光心坐標系Z 軸于地面交點同Ye 的引導線上的點，其在車身坐標系下的坐標可由由當前幀計算得出,設為(Xep,Yep)。為了得到車身相對于線的位置需要如圖借用歷史關鍵幀的數據推算。

每當新的一幀到來時首先讀取IMU 數據經六軸解算計算得出當前車身在世界坐標系的角度φ，計算出的引導線位置并按像素坐標系下等y 間隔將其分解為多個車身坐標系下向量的組合，將向量左乘旋轉矩陣R 得到世界坐標系下的向量。

若直接將模長歸一化后的向量于歷史幀向量叉乘會由于起點不對齊造成誤差，如圖3 所示。故首先選出歷時幀中3 個連續向量與當前幀中向量做模長歸一化后的叉乘，取絕對值最小的當前幀3 連續向量作為配對向量。由這3 個向量的第一個向量起點開始沿引導線移動向量起點做向量與3 個歷史向量中間的向量做匹配，直到起點移動到這3 個向量的最后一個向量終點為止。取其中叉乘絕對值最小的作為向量，由此實現兩幀拼接。

圖5：幀拼接仿真結果圖

將多個關鍵幀拼接就可以由向量遞推得到車身坐標系下引導線的位置了。

2.2 狀態空間方法

傳統的PID 控制由于是單輸入單輸出的無法區分角度和位置的影響，并且存在著超調等一系列問題所以在本系統中難以起到非常好的效果。相比較之下模糊PID 以角度和位置輸入可以構成雙輸入單輸出的模型，擁有比傳統PID 更好的效果但是由于對系統本身的狀態轉移關系不夠明確無法實現狀態轉移評價。

狀態空間方法是一種基于系統數學模型經行控制的方法，相比于PID 控制擁有更好的表現，但是由于其極大的依賴于狀態轉移矩陣和控制輸入矩陣模型的建模準確性，所以對于系統的建模提出了更高的要求。

為了優化系統模型的準確性，采用空間分割的方法，機器學習方法和模糊非線性近似的方法提高系統模型的精度，并且對于狀態轉移的不確定性因素使用高斯噪聲近似并在狀態評價函數中考慮其不確定性。

2.2.1 狀態方程

測量得到前輪舵機轉角與車身移動半徑的關系，將其存入詞典，攝像頭在檢測到彎道時判斷其車身處曲率半徑，查詞典計算舵機轉角，從而保證理想狀態下車身運動與引導線同向。在此基礎上，我們可以把曲線的狀態轉移矩陣近似為直線的狀態轉移矩陣。

由基礎物理學知識推到得到如下狀態方程：

為了使得狀態方程適用于各種地面環境，采用動態改變摩檫力方法適應。記錄輸入Fi，測量狀態向量通過梯度下降的方法最小化預測值和下一時刻測量狀態向量的差距從而得到最小化偏差的fi，由于改變f1 與f2 只有一個極值點，所以梯度下降時確定能收斂到最小值的。

圖6：下降法求解仿真結果圖

2.2.2 參數的確定

為了通過機器學習確定k 參數需要大量的數據，為了得到數據集需要先使用PID 控制器在運行，不同參數下獲得大量測量狀態向量和輸入向量并將其存儲入上位機。在得到足夠大量的測量狀態向量和輸入向量后，對于由組成的三維空間按照每個變量的小，中，大進行空間分割將第一象限劃分為3*3*3個空間。由于不考慮倒車所以vy只存在正數，僅需要對其余3 個象限進行劃分，再對于每個空間進行梯度下降求解k 參數的最優解。

由于變量很多梯度下降無法保證收斂于最優解，所以采用多梯度下降法和模擬退火梯度下降法來求解全局最優解。由于對稱性，只需要求解一二象限就可以推算出其余象限，降低了工作量，但是狀態空間劃分的缺點在于在空間內部有較好的近似，但在空間邊緣的近似較差，為解決這個問題可以在空間邊界引入模糊非線性近似。

2.2.3 狀態評價函數

在得到狀態方程后，為計算出最優輸出量，需要解算出下一個時刻的最優狀態，而評價函數的最值就決定了下一時刻系統向什么狀態轉變。同時在這個過程中需要考慮到系統狀態轉移的隨機誤差，可以用高斯誤差近似，通過卡爾曼濾波遞推式的方差遞推獲得卡爾曼狀態轉移方差作為狀態轉移的方差

對于智能車系統，基本的目標是減小θ 和x，盡可能的增大vy，所以最基本的狀態評價函數如下：

其中p 為狀態評價函數，p+為最大狀態變換，p-為最小狀態變換，ki 為待定系數，cov_х, cov_θ, cov_vy 為卡爾曼預測的2σ 協方差橢圓距均值距離最遠的點所取得得值即95%概率下最大偏差值，而協方差橢球方程可以通過求解特征值和特征向量求解。對p 進行多梯度下降可以以較高的概率收斂于最優解。由于狀態變換的隨機誤差有三個維度的方向較為復雜，故主要考慮其最大狀態變換，最小狀態變換的結果要位于可接受的裕度內，否則要添加限制條件重新梯度下降。

整個算法設計流程圖如圖4 所示。

3 實驗結果

3.1 幀拼接遞推linux仿真

設實際引導線符合y=x2，觀測到的引導線增加一項σ 為0.5 的高斯噪聲,此處采用opencv 庫的rng 生成，實際中由于相鄰兩幀時間間隔極短陀螺儀測得的角速度不產生累計誤差，故忽略旋轉矩陣帶來的旋轉誤差，故符合y=x2+rng(0.5)。在第二幀x 坐標大于第一幀4.7 時，求第一幀x=10 在第二幀的匹配x 坐標。通過eigen 庫的矩陣運算后得到如圖5 所示結果，表明幀拼接遞推的拼接精度和魯棒性較高。

3.2 下降法求解最優解的基于ceres庫的樹莓派仿真

采用上文提到的狀態評價函數p 得到的待優化函數為二元二次函數，此函數為非凸函數故需要多次梯度下降求解最優值。Ceres庫提供的二次階下降方法牛頓法可以更快的收斂。仿真中設該二元二次待優化函數為p=100-2.6*x2-5.3x+2.0*y2+4.8*y，對于0 初始值采用ceres 庫優化解法只3 次迭代收斂，加以樹莓派的高計算性能，用時0.545ms 可以很好的適應運動系統的實時性要求。如圖6 所示。

4 結束語

本文研究了在高性能處理器下的控制算法，提出了基于攝像頭的位姿解算算法和基于狀態轉移評價的控制算法，充分的利用了樹莓派4b 的高計算性能以達到更優的控制。最終使用攝像頭實現了車身位姿的高精度解算和動態車身坐標下地圖的構建，在此基礎上基于機器學習得到的狀態方程求得了梯度下降下的最優控制。

對于多元函數的最值計算采用梯度下降的計算量較大，可以考慮進一步基于機器學習得到的狀態轉移方程求其近似的解析解以加速解算，或者進一步細化狀態方程。