文章目錄

• 參考資料
• 1. 基本概念
• • 1.1 運(yùn)動學(xué)模型的離散狀態(tài)方程
  • 1.2 LQR求解步驟
• 2. python實現(xiàn)
• • 2.1 車輛模型
  • 2.2 相關(guān)參數(shù)設(shè)置
  • 2.3 生成軌跡曲線
  • 2.4 角度歸一化
  • 2.5 解代數(shù)里卡提方程
  • 2.6 LQR控制算法
  • 2.7 主函數(shù)

參考資料

• 基于運(yùn)動模型的LQR優(yōu)化
• 強(qiáng)化學(xué)習(xí)與最優(yōu)控制：用于軌跡跟蹤的 LQR
• Linear–quadratic regulator (LQR) steering control
• 路徑規(guī)劃與軌跡跟蹤系列算法
• 求解離散黎卡提矩陣代數(shù)方程

---

前文：LQR控制算法原理

---

1. 基本概念

LQR的控制理論原理推導(dǎo)可以翻看前文。

1.1 運(yùn)動學(xué)模型的離散狀態(tài)方程

對于一個離散時間系統(tǒng)：
$$\mathbf{X}(k+1)=A\mathbf{X}(k)+B\mathbf{u}(k)$$

其中， $A\in R^{n\times n}$ ， $B\in R^{n\times m}$。
關(guān)于最優(yōu)問題，就在于如何選擇合適的 $u_0,u_1,\dots$ ，使得狀態(tài)量 $x_0,x_1,\dots$ 足夠小，因此得到好的調(diào)節(jié)和控制；或者使得 $u_0,u_1,\dots$ 足夠小，以使用更少的能量。這兩個量通常相互制約，如果采用更大的輸入$\mathbf{u}$，就會驅(qū)使?fàn)顟B(tài)量更快達(dá)到0。

這是一個典型的多目標(biāo)優(yōu)化最優(yōu)控制問題，為了表示控制系統(tǒng)達(dá)到穩(wěn)定控制所付出的代價，定義如下二次型代價函數(shù)：
$$J=\sum _{k=1}^N\left({\mathbf{X}}^{T}Q\mathbf{X}+{\mathbf{u}}^{T}R\mathbf{u}\right)$$

其中，$Q$為半正定的狀態(tài)加權(quán)矩陣, $R$為正定的控制加權(quán)矩陣，且兩者通常取為對角陣；$Q$矩陣元素變大意味著希望狀態(tài)量$\mathbf{X}$能夠快速趨近于零；$R$矩陣元素變大意味著希望控制輸入能夠盡可能小。

在軌跡跟蹤中，前一項優(yōu)化目標(biāo)表示跟蹤過程路徑偏差的累積大小，第二項優(yōu)化目標(biāo)表示跟蹤過程控制能量的損耗，這樣就將軌跡跟蹤控制問題轉(zhuǎn)化為一個最優(yōu)控制問題。

給定一個大小為$n\times n$的實對稱矩陣A ，若對于任意長度為$n$的非零向量$x$，有$x^{T}Ax>0$恒成立，則矩陣A是一個正定矩陣。

對于上述目標(biāo)函數(shù)的優(yōu)化求解，使用線性二次型調(diào)節(jié)器 ( Linear-Quadratic Regulator)，解出的最優(yōu)控制規(guī)律u是關(guān)于狀態(tài)變量$X$的線性函數(shù)

$$\mathbf{u}=\left[?{\left(R+B^{T}PB\right)}^{?1}{B}^{T}PA\right]\mathbf{X}=K\mathbf{X}$$
其中，P是下述黎卡提方程的解 :
$$P=A^{T}PA?A^{T}PB{\left(R+B^{T}PB\right)}^{?1}{B}^{T}PA+Q$$

形如$y^{\prime }=P(x)y^2+Q(x)y+R(x)$的非線性微分方程稱為黎卡提方程。 針對黎卡提方程，可以采用循環(huán)迭代的思想求解P：
1）令等式右邊的P_old=Q；
2）計算等式右邊的值為P_new
3）比較P_old和P_new，若兩者的差值小于預(yù)設(shè)值， 則認(rèn)為等式兩邊相等；否則再令P_old=P_new，繼續(xù)循環(huán)。
備注
AtsushiSakai 的PythonRobotics的代碼就是這么求解的

1.2 LQR求解步驟

綜上，采用LQR算法進(jìn)行控制率求解的步驟概括為：

確定迭代范圍 $N$，預(yù)設(shè)精度$EPS$

設(shè)置迭代初始值 $P=Q_f$，其中$Q_f=Q$

循環(huán)迭代， $t=1,\dots ,N$
$$P_{new}=Q+A^{T}PA?A^{T}PB{\left(R+B^{T}PB\right)}^{?1}{B}^{T}PA$$
若$||P_{new}?P||<EPS$：跳出循環(huán)；否則：$P=P_{new}$

計算反饋系數(shù) $K=?{\left(R+B^T{P}_{new}B\right)}^{?1}{B}^T{P}_{new}A$

最終得優(yōu)化的控制量 $u^{?}=KX$

2. python實現(xiàn)

2.1 車輛模型

我們以后軸中心為車輛中心的單車運(yùn)動學(xué)模型為例，它的離散狀態(tài)方程如下：

$$\begin{array}{rl}\mathbf{X}(k+1)& =?\begin{array}{ccc}1& 0& ?T{v}_r\sin {\psi }_r\\ 0& 1& T{v}_r\cos {\psi }_r\\ 0& 0& 1\end{array}?\mathbf{X}(k)+?\begin{array}{cc}T\cos {\psi }_r& 0\\ T\sin {\psi }_r& 0\\ \frac{T\tan {\delta }_r}{L}& \frac{v_{r}T}{L{\cos }^2{\delta }_r}\end{array}?\mathbf{u}(k)\\ & =A\mathbf{X}(k)+B\mathbf{u}(k)\end{array}$$
式中，$v_r$代表參考軌跡上每一個軌跡點要求的速度值； ${\delta }_r$是每一個軌跡點的參考前輪轉(zhuǎn)角控制量。$\mathbf{X}=x?x_{ref}$為狀態(tài)量誤差，$\mathbf{u}=u?u_{ref}$為控制量誤差。

期望的系統(tǒng)響應(yīng)特性有以下兩點:

跟蹤偏差能夠快速、穩(wěn)定地趨近于零，并保持平衡；

前輪轉(zhuǎn)角控制輸入盡可能小。
采用線性二次調(diào)節(jié)原理解決這個問題。

python實現(xiàn)代碼如下。

import mathclass KinematicModel_3:"""假設(shè)控制量為轉(zhuǎn)向角delta_f和加速度a"""def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = L# 實現(xiàn)是離散的模型self.dt = dtdef update_state(self, a, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v+a*self.dtdef get_state(self):return self.x, self.y, self.psi, self.vdef state_space(self, ref_delta, ref_yaw):"""將模型離散化后的狀態(tài)空間表達(dá)Args:delta (_type_): 參考輸入Returns:_type_: _description_"""A = np.matrix([[1.0,0.0,-self.v*self.dt*math.sin(ref_yaw)],[0.0, 1.0, self.v*self.dt*math.cos(ref_yaw)],[0.0,0.0,1.0]])B = np.matrix([[self.dt*math.cos(ref_yaw), 0],[self.dt*math.sin(ref_yaw), 0],[self.dt*math.tan(ref_delta)/self.L, self.v*self.dt/(self.L*math.cos(ref_delta)*math.cos(ref_delta))]])return A,B

2.2 相關(guān)參數(shù)設(shè)置

N=100 # 迭代范圍 EPS = 1e-4 # 迭代精度 Q = np.eye(3)*3 R = np.eye(2)*2. dt=0.1 # 時間間隔，單位：s L=2 # 車輛軸距，單位：m v = 2 # 初始速度 x_0=0 # 初始x y_0=-3 #初始y psi_0=0 # 初始航向角

2.3 生成軌跡曲線

class MyReferencePath:def __init__(self):# set reference trajectory# refer_path包括4維：位置x, 位置y， 軌跡點的切線方向, 曲率k self.refer_path = np.zeros((1000, 4))self.refer_path[:,0] = np.linspace(0, 100, 1000) # xself.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0)+2.5*np.cos(self.refer_path[:,0]/2.0) # y# 使用差分的方式計算路徑點的一階導(dǎo)和二階導(dǎo)，從而得到切線方向和曲率for i in range(len(self.refer_path)):if i == 0:dx = self.refer_path[i+1,0] - self.refer_path[i,0]dy = self.refer_path[i+1,1] - self.refer_path[i,1]ddx = self.refer_path[2,0] + self.refer_path[0,0] - 2*self.refer_path[1,0]ddy = self.refer_path[2,1] + self.refer_path[0,1] - 2*self.refer_path[1,1]elif i == (len(self.refer_path)-1):dx = self.refer_path[i,0] - self.refer_path[i-1,0]dy = self.refer_path[i,1] - self.refer_path[i-1,1]ddx = self.refer_path[i,0] + self.refer_path[i-2,0] - 2*self.refer_path[i-1,0]ddy = self.refer_path[i,1] + self.refer_path[i-2,1] - 2*self.refer_path[i-1,1]else: dx = self.refer_path[i+1,0] - self.refer_path[i,0]dy = self.refer_path[i+1,1] - self.refer_path[i,1]ddx = self.refer_path[i+1,0] + self.refer_path[i-1,0] - 2*self.refer_path[i,0]ddy = self.refer_path[i+1,1] + self.refer_path[i-1,1] - 2*self.refer_path[i,1]self.refer_path[i,2]=math.atan2(dy,dx) # yaw# 計算曲率:設(shè)曲線r(t) =(x(t),y(t)),則曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).# 參考：https://blog.csdn.net/weixin_46627433/article/details/123403726self.refer_path[i,3]=(ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) # 曲率k計算def calc_track_error(self, x, y):"""計算跟蹤誤差A(yù)rgs:x (_type_): 當(dāng)前車輛的位置xy (_type_): 當(dāng)前車輛的位置yReturns:_type_: _description_"""# 尋找參考軌跡最近目標(biāo)點d_x = [self.refer_path[i,0]-x for i in range(len(self.refer_path))] d_y = [self.refer_path[i,1]-y for i in range(len(self.refer_path))] d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]s = np.argmin(d) # 最近目標(biāo)點索引yaw = self.refer_path[s, 2]k = self.refer_path[s, 3]angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))e = d[s] # 誤差if angle < 0:e *= -1return e, k, yaw, s

2.4 角度歸一化

def normalize_angle(angle):"""Normalize an angle to [-pi, pi].:param angle: (float):return: (float) Angle in radian in [-pi, pi]copied from https://atsushisakai.github.io/PythonRobotics/modules/path_tracking/stanley_control/stanley_control.html"""while angle > np.pi:angle -= 2.0 * np.piwhile angle < -np.pi:angle += 2.0 * np.pireturn angle

2.5 解代數(shù)里卡提方程

def cal_Ricatti(A,B,Q,R):"""解代數(shù)里卡提方程Args:A (_type_): 狀態(tài)矩陣AB (_type_): 狀態(tài)矩陣BQ (_type_): Q為半正定的狀態(tài)加權(quán)矩陣, 通常取為對角陣；Q矩陣元素變大意味著希望跟蹤偏差能夠快速趨近于零；R (_type_): R為正定的控制加權(quán)矩陣，R矩陣元素變大意味著希望控制輸入能夠盡可能小。Returns:_type_: _description_"""# 設(shè)置迭代初始值Qf=QP=Qf# 循環(huán)迭代for t in range(N):P_=Q+A.T@P@A-A.T@P@B@np.linalg.pinv(R+B.T@P@B)@B.T@P@Aif(abs(P_-P).max()<EPS):breakP=P_return P_

2.6 LQR控制算法

def lqr(robot_state, refer_path, s0, A, B, Q, R):"""LQR控制器"""# x為位置和航向誤差x=robot_state[0:3]-refer_path[s0,0:3]P = cal_Ricatti(A,B,Q,R)K = -np.linalg.pinv(R + B.T @ P @ B) @ B.T @ P @ Au = K @ xu_star = u #u_star = [[v-ref_v,delta-ref_delta]] # print(u_star)return u_star[0,1]

2.7 主函數(shù)

from celluloid import Camera # 保存動圖時用，pip install celluloid # 使用隨便生成的軌跡 def main():reference_path = MyReferencePath()goal = reference_path.refer_path[-1,0:2]# 運(yùn)動學(xué)模型ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)x_ = []y_ = []fig = plt.figure(1)# 保存動圖用camera = Camera(fig)# plt.ylim([-3,3])for i in range(500):robot_state = np.zeros(4)robot_state[0] = ugv.xrobot_state[1] = ugv.yrobot_state[2]=ugv.psirobot_state[3]=ugv.ve, k, ref_yaw, s0 = reference_path.calc_track_error(robot_state[0], robot_state[1])ref_delta = math.atan2(L*k,1)A, B = ugv.state_space(ref_delta,ref_yaw)delta = lqr(robot_state, reference_path.refer_path,s0, A, B, Q, R)delta = delta+ref_deltaugv.update_state(0, delta) # 加速度設(shè)為0，恒速x_.append(ugv.x)y_.append(ugv.y)# 顯示動圖plt.cla()plt.plot(reference_path.refer_path[:,0], reference_path.refer_path[:,1], "-.b", linewidth=1.0, label="course")plt.plot(x_, y_, "-r", label="trajectory")plt.plot(reference_path.refer_path[s0,0], reference_path.refer_path[s0,1], "go", label="target")# plt.axis("equal")plt.grid(True)plt.pause(0.001)# camera.snap()# 判斷是否到達(dá)最后一個點if np.linalg.norm(robot_state[0:2]-goal)<=0.1:print("reach goal")break# animation = camera.animate()# animation.save('trajectory.gif')main()

跟蹤效果如下：

完整python代碼文件見github倉庫

總結(jié)

以上是生活随笔為你收集整理的【自动驾驶】LQR控制实现轨迹跟踪的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。