Pure Pursuit 純幾何跟蹤算法
Pure Pursuit算法是一種通過車輛運(yùn)動幾何學(xué)（阿克曼轉(zhuǎn)向幾何）對前輪轉(zhuǎn)角進(jìn)行調(diào)整來消除橫向偏差的橫向控制算法,其參考點(diǎn)為后軸中心點(diǎn)。
該算法的思想是：基于當(dāng)前車輛后軸中心位置，在參考路徑上向前
l
d
l_d
ld?的距離匹配一個預(yù)瞄點(diǎn)，將此預(yù)瞄點(diǎn)作為該周期的goal point。假設(shè)車輛后軸中心點(diǎn)可以按照一定的轉(zhuǎn)彎半徑R形式抵達(dá)該預(yù)瞄點(diǎn)，然后根據(jù)預(yù)瞄距離
l
d
l_d
ld?、轉(zhuǎn)彎半徑R和車輛坐標(biāo)系下預(yù)瞄點(diǎn)的朝向角
α
\alpha
α之間的幾何關(guān)系來確定前輪轉(zhuǎn)角，其原理圖如下：
為了使車輛后軸中心點(diǎn)跟蹤圓弧虛線路徑到達(dá)C點(diǎn)，在三角形OAC中，需要滿足的正弦定理關(guān)系：
l d s i n 2 α = R s i n ( π 2 ? α ) (1) \frac{l_d}{sin2\alpha}=\frac{R}{sin(\frac{\pi}{2}-\alpha)} \tag{1} sin2αld??=sin(2π??α)R?(1)
又因為： s i n 2 α = 2 s i n α c o s α , s i n ( π 2 ? α ) = c o s α sin2\alpha=2sin\alpha cos\alpha,sin(\frac{\pi}{2}-\alpha)=cos\alpha sin2α=2sinαcosα,sin(2π??α)=cosα,所以化簡上述公式可得：
R = l d 2 s i n α (2) R=\frac{l_d}{2sin\alpha} \tag{2} R=2sinαld??(2)
為了達(dá)到這種幾何關(guān)系，作為控制量的前輪轉(zhuǎn)角需要滿足什么關(guān)系呢？在阿克曼轉(zhuǎn)向（三角形OAB）中，有：
t a n δ ≈ L R (3) tan\delta \approx \frac{L}{R} \tag{3} tanδ≈RL?(3)
因此，聯(lián)立上面的公式(2)(3)，得到：
δ ( t ) = a r c t a n ( 2 L s i n α ( t ) l d ) (4) \delta(t) = arctan(\frac{2Lsin\alpha(t)}{l_d}) \tag{4} δ(t)=arctan(ld?2Lsinα(t)?)(4)
其中，車輛坐標(biāo)系下預(yù)瞄點(diǎn)的朝向角 α ( t ) \alpha(t) α(t)為變量。
另外，定義橫向位置誤差為車輛當(dāng)前姿態(tài)和預(yù)瞄點(diǎn)在橫向上的誤差：
e y ( t ) = l d s i n α ( t ) (5) e_y(t) = l_d sin\alpha(t) \tag{5} ey?(t)=ld?sinα(t)(5)
聯(lián)立公式（4）（5），可得到前輪轉(zhuǎn)角計算公式：
δ
(
t
)
=
a
r
c
t
a
n
(
2
L
l
d
2
e
y
(
t
)
)
(6)
\delta(t) =arctan(\frac{2L}{l_d^2}e_y(t)) \tag{6}
δ(t)=arctan(ld2?2L?ey?(t))(6)
一般而言，考慮小角度假設(shè)，公式（6）可變成：
δ
(
t
)
=
a
r
c
t
a
n
(
2
L
l
d
2
e
y
(
t
)
)
≈
2
L
l
d
2
e
y
(
t
)
(7)
\delta(t) = arctan(\frac{2L}{l_d^2}e_y(t)) \approx \frac{2L}{l_d^2}e_y(t) \tag{7}
δ(t)=arctan(ld2?2L?ey?(t))≈ld2?2L?ey?(t)(7)
因此，純跟蹤法本質(zhì)上是一個P控制器,輸入為橫向位置誤差，輸出為前輪轉(zhuǎn)角。跟蹤的控制效果將由預(yù)瞄距離
l
d
l_d
ld?決定，通常
l
d
l_d
ld?定義為關(guān)于速度的函數(shù)表達(dá)式：
l
d
=
l
d
0
+
k
v
(8)
l_d =l_{d0}+k v \tag{8}
ld?=ld0?+kv(8)
算法特點(diǎn)：
- 短的預(yù)瞄距離提供更精確的跟蹤，而較長預(yù)瞄距離提供更平滑的跟蹤。
- 參數(shù)k值太小將導(dǎo)致不穩(wěn)定，而k值太大將導(dǎo)致跟蹤精度下降。
- 高水平的魯棒性：例如，對路徑中的不連續(xù)性的良好處理。
- 彎道跟蹤中，隨著速度的增加，穩(wěn)態(tài)誤差也增大。
Stanley算法（前輪反饋控制）
前輪反饋控制(Front wheel feedback)又稱Stanley控制其核心思想是基于前軸中心的路徑跟蹤偏差量對方向盤轉(zhuǎn)向控制量進(jìn)行計算。
Stanley方法是一種基于橫向跟蹤誤差為前軸中心到最近路徑點(diǎn)的距離的非線性反饋函數(shù)，并且能實現(xiàn)橫向跟蹤誤差指數(shù)收斂于0。根據(jù)車輛位姿與給定路徑的相對幾何關(guān)系可以直觀的獲得控制車輛方向盤轉(zhuǎn)角的控制變量。
Stanley法計算得到的前輪轉(zhuǎn)角控制量由兩部分構(gòu)成:
- 一部分是航向誤差引起的轉(zhuǎn)角，即當(dāng)前車身方向與參考軌跡最近點(diǎn)的切線方向的夾角;
- 另一部分是橫向誤差引起的轉(zhuǎn)角，即前軸中心到參考軌跡最近點(diǎn)的橫向距離。
Stanley法的原理示意圖如下所示：
在不考慮橫向跟蹤誤差的情況下，前輪轉(zhuǎn)角應(yīng)當(dāng)與給定路徑參考點(diǎn)的切線方向一致。其中，
θ
φ
\theta_\varphi
θφ?表示車輛航向與最近路徑點(diǎn)切線方向之間的夾角，在沒有任何橫向誤差的情況下，前輪方向應(yīng)與所在路徑點(diǎn)的方向相同,故消除航向誤差的前輪轉(zhuǎn)角為；
δ
φ
(
t
)
=
θ
φ
(
t
)
(9)
\delta_\varphi(t) = \theta_\varphi(t) \tag{9}
δφ?(t)=θφ?(t)(9)
在不考慮航向跟蹤偏差的情況下，橫向跟蹤誤差越大，前輪轉(zhuǎn)向角越大，假設(shè)車輛預(yù)期軌跡在距離前輪
d
(
t
)
d(t)
d(t)處與參考路徑上最近點(diǎn)切線相交，根據(jù)幾何關(guān)系得出如下非線性比例函數(shù):
δ
y
(
t
)
=
θ
y
(
t
)
=
a
r
c
t
a
n
(
e
y
(
t
)
d
(
t
)
)
=
a
r
c
t
a
n
(
k
e
y
(
t
)
v
(
t
)
)
(10)
\delta_y(t)=\theta_y(t)=arctan(\frac{e_y(t)}{d(t)})=arctan(\frac{k e_y(t)}{v(t)}) \tag{10}
δy?(t)=θy?(t)=arctan(d(t)ey?(t)?)=arctan(v(t)key?(t)?)(10)
其中
d
(
t
)
d(t)
d(t)與車速相關(guān)，用車速
v
(
t
)
v(t)
v(t)和增益參數(shù)k表示.
因此，前輪轉(zhuǎn)角為:
δ
(
t
)
=
δ
φ
(
t
)
+
δ
y
(
t
)
=
δ
φ
(
t
)
+
a
r
c
t
a
n
(
k
e
y
(
t
)
v
(
t
)
)
,
δ
(
t
)
∈
[
δ
m
i
n
,
δ
m
a
x
]
(11)
\delta(t)=\delta_\varphi(t)+\delta_y(t)=\delta_\varphi(t)+arctan(\frac{k e_y(t)}{v(t)}),\delta(t) \in [\delta_{min},\delta_{max}] \tag{11}
δ(t)=δφ?(t)+δy?(t)=δφ?(t)+arctan(v(t)key?(t)?),δ(t)∈[δmin?,δmax?](11)
算法特點(diǎn)：
- 如果速度反向可能導(dǎo)致數(shù)值不穩(wěn)定，可以向控制器添加正的松弛常數(shù) k s k_s ks?;
- 在實車中,以更高的速度行駛時成為一個問題;
- 可以通過在航向上添加前饋項來改進(jìn)曲線的跟蹤；
- 與Pure Pursuit相比，Stanley方法更直觀，但它在調(diào)優(yōu)時也會遇到類似的問題。
- 與Pure Pursuit相比，經(jīng)過精心調(diào)整的Stanley跟蹤器不會“偷工減料”，但相當(dāng)于過沖轉(zhuǎn)彎，這種影響可以歸因于沒有預(yù)瞄距離。
- 與Pure Pursuit方法類似，中高速情況下，曲線跟蹤的的穩(wěn)態(tài)誤差變得顯著。
LQR控制算法（全狀態(tài)反饋控制-最優(yōu)控制）
LQR控制算法是一種廣泛應(yīng)用于工業(yè)控制和機(jī)器人控制中的線性控制算法，它利用狀態(tài)反饋控制來實現(xiàn)系統(tǒng)的穩(wěn)定性和優(yōu)化性能。
LQR控制算法的基本思想：通過對系統(tǒng)的狀態(tài)進(jìn)行反饋控制，使系統(tǒng)的輸出響應(yīng)達(dá)到最優(yōu)，同時保持系統(tǒng)的穩(wěn)定性。具體來說，LQR控制算法通過將系統(tǒng)的狀態(tài)反饋控制矩陣K計算出來，使得系統(tǒng)的狀態(tài)能夠以最小的代價函數(shù)進(jìn)行控制，控制示意圖如下：
在LQR控制算法中，代價函數(shù)用來描述系統(tǒng)的響應(yīng)性能和能耗。通常情況下，代價函數(shù)可以表示為系統(tǒng)狀態(tài)與控制輸入的加權(quán)和，即：
J
=
∫
[
X
(
t
)
T
Q
X
(
t
)
+
U
(
t
)
T
R
U
(
t
)
]
d
t
(12)
J = ∫[X(t)^T Q X(t) + U(t)^T R U(t)]dt \tag{12}
J=∫[X(t)TQX(t)+U(t)TRU(t)]dt(12)
其中，Q和R分別是狀態(tài)和控制輸入的權(quán)重矩陣，它們的大小和系統(tǒng)狀態(tài)和控制輸入的維度相同。通過對代價函數(shù)進(jìn)行求導(dǎo)并令其等于0，可以得到連續(xù)的狀態(tài)反饋矩陣K的表達(dá)式：
K
=
?
R
?
1
B
T
P
(13)
K = -R^{-1} B^T P \tag{13}
K=?R?1BTP(13)
其中，
P
P
P是Riccati方程的解，它可以通過求解Riccati方程得到，具體的求解方法可以使用數(shù)值方法，例如MATLAB中的lqr函數(shù)。
一旦得到了狀態(tài)反饋矩陣K，就可以將其用于控制系統(tǒng)中。具體來說，控制輸入（連續(xù)的）可以表示為：
U
(
t
)
=
?
K
X
(
t
)
(14)
U(t) = -KX(t) \tag{14}
U(t)=?KX(t)(14)
通過這種方式，系統(tǒng)的狀態(tài)可以被反饋到控制輸入中，從而實現(xiàn)系統(tǒng)的控制。
以車輛的橫向控制為例：
步驟1：建立道路-車輛誤差狀態(tài)空間方程
X
(
t
)
˙
=
A
X
(
t
)
+
B
U
(
t
)
Y
(
t
)
=
C
X
(
t
)
(15)
\dot{X(t)}=A X(t) + B U(t) \\ Y(t)=C X(t) \tag{15}
X(t)˙?=AX(t)+BU(t)Y(t)=CX(t)(15)
步驟2：對連續(xù)的誤差狀態(tài)方程進(jìn)行離散化，一般A矩陣采用中點(diǎn)歐拉離散，B及C矩陣采用前向歐拉離散，得到離散的誤差狀態(tài)方程如下：
X
(
k
+
T
)
=
A
d
X
(
k
)
+
B
d
U
(
k
)
Y
(
k
)
=
C
d
X
(
k
)
(16)
X(k+T)=A_d X(k) + B_d U(k) \\ Y(k)=C_d X(k) \tag{16}
X(k+T)=Ad?X(k)+Bd?U(k)Y(k)=Cd?X(k)(16)
步驟3：構(gòu)建代價函數(shù)
J
J
J,建立離散的LQR問題
J
=
1
2
X
T
(
N
)
F
X
(
N
)
+
1
2
∑
k
=
0
N
?
1
[
X
T
(
k
)
Q
X
(
k
)
+
U
T
(
k
)
R
U
(
k
)
]
(17)
J = \frac{1}{2}X^T(N)FX(N) + \frac{1}{2} \sum_{k = 0}^{N-1} [X^T(k) Q X(k) + U^T(k) R U(k)] \tag{17}
J=21?XT(N)FX(N)+21?k=0∑N?1?[XT(k)QX(k)+UT(k)RU(k)](17)
步驟4：求解離散的LQR問題(動態(tài)規(guī)劃法)
結(jié)論:
U
(
k
)
=
?
K
(
k
)
X
(
k
)
(18)
U(k) = -K(k) X(k) \tag{18}
U(k)=?K(k)X(k)(18)
其中：
K
(
k
)
=
[
B
T
(
k
)
P
(
k
+
1
)
B
(
k
)
+
R
(
k
)
]
?
1
B
T
(
k
)
P
(
k
+
1
)
A
(
k
)
(19)
K(k) = [B^T(k)P(k+1)B(k) + R(k)]^{-1} B^T(k)P(k+1)A(k) \tag{19}
K(k)=[BT(k)P(k+1)B(k)+R(k)]?1BT(k)P(k+1)A(k)(19)
而且離散的Riccati方程為：
P
(
k
)
=
A
T
(
k
)
P
(
k
+
1
)
A
(
k
)
?
A
T
(
k
)
P
(
k
+
1
)
B
(
k
)
[
B
T
(
k
)
P
(
k
+
1
)
B
(
k
)
+
R
(
k
)
]
?
1
B
T
(
k
)
P
(
k
+
1
)
A
(
k
)
(20)
P(k) = A^T(k)P(k+1)A(k) - A^T(k)P(k+1)B(k) [B^T(k)P(k+1)B(k) + R(k)]^{-1} B^T(k)P(k+1)A(k) \tag{20}
P(k)=AT(k)P(k+1)A(k)?AT(k)P(k+1)B(k)[BT(k)P(k+1)B(k)+R(k)]?1BT(k)P(k+1)A(k)(20)
因此，求解出矩陣
P
P
P就可以得到控制律。
求解矩陣 P P P：
1、令 P P P等于最終狀態(tài)權(quán)重矩陣;
2、迭代黎卡提方程求出新的 P n e x t P_{next} Pnext?;
3、當(dāng)兩次P的差值足夠小時，計算反饋矩陣K;文章來源：http://www.zghlxwxcb.cn/news/detail-798015.html
4、根據(jù)反饋矩陣K獲取最優(yōu)控制量U;文章來源地址http://www.zghlxwxcb.cn/news/detail-798015.html
matrix lqrSolver(matrix A,matrix B,matrix Q, matrix R,double esp,double iterate_max)
{
double gap = 999999;
double iterate = 0;
matrix P = Q;
matrix P_next = zero(4, 4);
while (iterate < iterate_max && gap > esp)
{
P_next = A.transpose() * P * A - A.transpose() * P * B * (B.transpose() * P * B + R).inv() * B.transpose() * P * A;
gap = (P_next - P).norm();
P = P_next;
iterate++;
}
return P_next;
}
到了這里，關(guān)于自動駕駛控制算法——基礎(chǔ)控制算法原理的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
