一種基于狀態(tài)空間的智能車控制算法

陳力成 何新 彭一凡 黎文卓

（南京理工大學(xué) 江蘇省南京市 210000）

1 引言

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

2 基于狀態(tài)空間的智能車控制算法

本文采用狀態(tài)空間和機(jī)器學(xué)習(xí)的控制算法實(shí)現(xiàn)給定的狀態(tài)評價函數(shù)最優(yōu)解求算。基于攝像頭首先進(jìn)行車身定位算法的研究，著重解決在彎道上對車身位姿的解算。其次對基于狀態(tài)空間的控制方法進(jìn)行研究，并且提出相應(yīng)的狀態(tài)轉(zhuǎn)移評價函數(shù)和優(yōu)化算法。

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

2.1.1 相機(jī)投影模型[1]

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

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

2.1.2 提取引導(dǎo)線[3]

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

圖1：相機(jī)投影模型示意圖

圖2：車身的動態(tài)坐標(biāo)系

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

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

圖4：算法流程圖

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

若窗口取3*3 矩陣則權(quán)重矩陣w 應(yīng)該為下式所示：

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

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

2.1.3 位姿解算

2.1.3.1 直線位姿解算

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

2.1.3.2 曲線位姿解算

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

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

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

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

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

圖5：幀拼接仿真結(jié)果圖

將多個關(guān)鍵幀拼接就可以由向量遞推得到車身坐標(biāo)系下引導(dǎo)線的位置了。

2.2 狀態(tài)空間方法

傳統(tǒng)的PID 控制由于是單輸入單輸出的無法區(qū)分角度和位置的影響，并且存在著超調(diào)等一系列問題所以在本系統(tǒng)中難以起到非常好的效果。相比較之下模糊PID 以角度和位置輸入可以構(gòu)成雙輸入單輸出的模型，擁有比傳統(tǒng)PID 更好的效果但是由于對系統(tǒng)本身的狀態(tài)轉(zhuǎn)移關(guān)系不夠明確無法實(shí)現(xiàn)狀態(tài)轉(zhuǎn)移評價。

狀態(tài)空間方法是一種基于系統(tǒng)數(shù)學(xué)模型經(jīng)行控制的方法，相比于PID 控制擁有更好的表現(xiàn)，但是由于其極大的依賴于狀態(tài)轉(zhuǎn)移矩陣和控制輸入矩陣模型的建模準(zhǔn)確性，所以對于系統(tǒng)的建模提出了更高的要求。

為了優(yōu)化系統(tǒng)模型的準(zhǔn)確性，采用空間分割的方法，機(jī)器學(xué)習(xí)方法和模糊非線性近似的方法提高系統(tǒng)模型的精度，并且對于狀態(tài)轉(zhuǎn)移的不確定性因素使用高斯噪聲近似并在狀態(tài)評價函數(shù)中考慮其不確定性。

2.2.1 狀態(tài)方程

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

由基礎(chǔ)物理學(xué)知識推到得到如下狀態(tài)方程：

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

圖6：下降法求解仿真結(jié)果圖

2.2.2 參數(shù)的確定

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

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

2.2.3 狀態(tài)評價函數(shù)

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

對于智能車系統(tǒng)，基本的目標(biāo)是減小θ 和x，盡可能的增大vy，所以最基本的狀態(tài)評價函數(shù)如下：

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

整個算法設(shè)計流程圖如圖4 所示。

3 實(shí)驗(yàn)結(jié)果

3.1 幀拼接遞推linux仿真

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

3.2 下降法求解最優(yōu)解的基于ceres庫的樹莓派仿真

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

4 結(jié)束語

本文研究了在高性能處理器下的控制算法，提出了基于攝像頭的位姿解算算法和基于狀態(tài)轉(zhuǎn)移評價的控制算法，充分的利用了樹莓派4b 的高計算性能以達(dá)到更優(yōu)的控制。最終使用攝像頭實(shí)現(xiàn)了車身位姿的高精度解算和動態(tài)車身坐標(biāo)下地圖的構(gòu)建，在此基礎(chǔ)上基于機(jī)器學(xué)習(xí)得到的狀態(tài)方程求得了梯度下降下的最優(yōu)控制。

對于多元函數(shù)的最值計算采用梯度下降的計算量較大，可以考慮進(jìn)一步基于機(jī)器學(xué)習(xí)得到的狀態(tài)轉(zhuǎn)移方程求其近似的解析解以加速解算，或者進(jìn)一步細(xì)化狀態(tài)方程。