1. 后輪反饋控制

后輪反饋控制（Rear wheel feedback）算法是利用后輪中心的跟蹤偏差來進行轉(zhuǎn)向控制量計算的方法，屬于Frenet坐標系的一個應(yīng)用。通過選擇合適的李雅普諾夫函數(shù)設(shè)計控制率，利用后輪中心的跟蹤偏差來進行轉(zhuǎn)向控制量計算的方法。

2. 算法原理

后輪反饋控制算法原理如上圖所示，其中

• $P$：當前距離車輛最近的路經(jīng)點；

• $e_y$：$P$點與車輛后輪中心點的橫向偏差$AP$，實際上對應(yīng)的就是frenet坐標下的$l$；

• $\phi$：車輛朝向與$X$軸正方向的夾角，即航向角；

• ${\phi }_r$：$P$點切線與$X$軸正方向的夾角；

• ${\phi }_e$：車輛航向角誤差，即$\phi ?{\phi }_r$；

• $\overrightarrow{n_{\tau }}$：$P$點法線的單位向量；

• $\overrightarrow{{\tau }_r}$：$P$點切線的單位向量；

• $L$：軸距

• ${\delta }_f$：前輪轉(zhuǎn)角

• $v$：車輛的速度

由前面的文章frenet坐標與cartesian坐標相互轉(zhuǎn)換與代碼實現(xiàn)和上圖的幾何關(guān)系可得車輛在參考軌跡上的投影點$P$處的線速度$\dot{s}$和橫向誤差$e_y$（即對應(yīng)frenet坐標系下的$\dot{l}$）的表達式為
$$\dot{s}=\frac{\vec{v}\overrightarrow{{\tau }_r}}{1?k_{r}l}=\frac{|\vec{v}||\overrightarrow{{\tau }_r}|cos{\phi }_e}{1?k_r{e}_y}=\frac{|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}\text{\hspace{1em}(1)}$$

$$\dot{e_y}=\dot{l}=\vec{v}\overrightarrow{n_r}=|\vec{v}||\overrightarrow{n_r}|cos(\frac{\pi }{2}?{\phi }_e)=|\vec{v}|sin{\phi }_e\text{\hspace{1em}(2)}$$

由圖中幾何關(guān)系可得航向誤差為
$${\phi }_e=\phi ?{\phi }_r\text{\hspace{1em}(3)}$$
則航向誤差變化率為
$$\dot{{\phi }_e}=\dot{\phi }?\dot{{\phi }_r}=\dot{\phi }?\frac{\dot{s}}{R_r}=\dot{\phi }?\dot{s}{k}_r\text{\hspace{1em}(4)}$$

注：一個剛體的角速度 = 線速度/線速度到速度瞬心的距離，上式中$R_r$為點$P$處的瞬時圓心半徑。

將(1)代入(4)可得
$$\dot{{\phi }_e}=\dot{\phi }?\dot{s}{k}_r=\dot{\phi }?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}\text{\hspace{1em}(4)}$$
綜上可得后輪反饋控制算法對應(yīng)模型的微分方程為

$$?\begin{array}{ll}\dot{s}& =\frac{|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}\\ \dot{e_y}& =|\vec{v}|sin{\phi }_e\\ \dot{{\phi }_e}& =\dot{\phi }?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}\end{array}\text{\hspace{1em}(5)}$$

有了模型的微分方程，我們可以根據(jù)李雅普諾夫第二方法設(shè)計該模型的車輛航向變化率$\dot{\phi }$。

李雅普諾夫第二方法：

穩(wěn)定的系統(tǒng)能量總是不斷被耗散的，李雅普諾夫通過定義一個標量函數(shù) $V(x)$ （通常能代表廣義能量）來分析穩(wěn)定性。這種方法的避免了直接求解方程，也沒有進行近似線性化，所以也一般稱之為直接法。$V(x)$需滿足：

1. $V(x)=0$ 當且僅當 $x=0$
2. $V(x)>0$ 當且僅當 $x\ne 0$
3. $\dot{V}(x)\le 0$ 當 $x\ne 0$

則稱系統(tǒng)在李雅普諾夫意義下是穩(wěn)定的，特別的，若$x\ne 0$時，有$\dot{V}(x)<0$，則系統(tǒng)是漸進穩(wěn)定的。

這里我們比較關(guān)心的就是橫向誤差$e_y$和${\phi }_e$，期望他們隨著時間的增長逐漸收斂于0，我們可以定義李雅普諾夫函數(shù)形式如下：
$$V(e_y,{\phi }_e)=\frac{1}{2}{e}_y^2+\frac{1}{2k_e}{\phi }_e^2\text{\hspace{1em}(6)}$$
其中$k_e>0$。

為了使$(e_y,{\phi }_e)$在平衡點$(0,0)$處穩(wěn)定，根據(jù)李雅普諾夫第二方法的穩(wěn)定判據(jù)，$V(x)$需滿足：

1. $V(e_y,{\phi }_e)=0$ 當且僅當 $e_y=0,{\phi }_e=0$
2. $V(e_y,{\phi }_e)>0$ 當且僅當 $e_y\ne 0,{\phi }_e\ne 0$
3. $\dot{V}(e_y,{\phi }_e)\le 0$ 當 $e_y\ne 0,{\phi }_e\ne 0$

對于1、2兩條，我們選取的李亞普函數(shù)$V(e_y,{\phi }_e)$顯然滿足。所以滿足李雅普諾夫第二方法的前兩條。

對于$\dot{V}(e_y,{\phi }_e)$我們結(jié)合(5)可得
$$\begin{array}{rl}\dot{V}(e_y,{\phi }_e)& =e_y\dot{e_y}+\frac{1}{k_e}{\phi }_e\dot{{\phi }_e}\\ & =e_y|\vec{v}|sin{\phi }_e+\frac{1}{k_e}{\phi }_e(\dot{\phi }?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y})\end{array}\text{\hspace{1em}(7)}$$
令$\dot{V}(e_y,{\phi }_e)=0$，結(jié)合(7)式可得
$$\begin{array}{rl}{e}_y|\vec{v}|sin{\phi }_e+\frac{1}{k_e}{\phi }_e(\dot{\phi }?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y})& =0\\ k_e{e}_y|\vec{v}|\frac{sin{\phi }_e}{{\phi }_e}+\dot{\phi }?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}& =0\end{array}\text{\hspace{1em}(8)}$$
由上式可以得到零界航向變化率$\dot{{\phi }_0}$等于
$$\dot{{\phi }_0}=\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}?k_e{e}_y|\vec{v}|\frac{sin{\phi }_e}{{\phi }_e}\text{\hspace{1em}(9)}$$
為了使$\dot{V}(e_y,{\phi }_e)$滿足要求3，我們可以設(shè)計一個調(diào)節(jié)函數(shù)$g(e_y,{\phi }_e)=k_{\phi }|\vec{v}|{\phi }_e>0$，其中$k_{\phi }>0$，我們可以將車輛航向的變化率$\dot{\phi }$設(shè)置為
$$\dot{\phi }=\dot{{\phi }_0}?g(e_y,{\phi }_e)=\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}?k_e{e}_y|\vec{v}|\frac{sin{\phi }_e}{{\phi }_e}?k_{\phi }|\vec{v}|{\phi }_e\text{\hspace{1em}(10)}$$
將(10)代入(7)得
$$\begin{array}{rl}\dot{V}(e_y,{\phi }_e)& =e_y|\vec{v}|sin{\phi }_e+\frac{1}{k_e}{\phi }_e(\dot{\phi }?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y})\\ & =e_y|\vec{v}|sin{\phi }_e+\frac{1}{k_e}{\phi }_e(\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y}?k_e{e}_y|\vec{v}|\frac{sin{\phi }_e}{{\phi }_e}?k_{\phi }|\vec{v}|{\phi }_e?\frac{k_r|\vec{v}|cos{\phi }_e}{1?k_r{e}_y})\\ & =e_y|\vec{v}|sin{\phi }_e+\frac{1}{k_e}{\phi }_e(?k_e{e}_y|\vec{v}|\frac{sin{\phi }_e}{{\phi }_e}?k_{\phi }|\vec{v}|{\phi }_e)\\ & =?\frac{k_{\phi }}{k_e}|\vec{v}|{\phi }_e^2<0\end{array}\text{\hspace{1em}(11)}$$
因此我們設(shè)計的公式(10)即車輛航向的變化率$\dot{\phi }$滿足李雅普諾夫的穩(wěn)定性條件。

根據(jù)前面文章介紹過的車輛運動學模型下的幾何關(guān)系
$$tan{\delta }_f=\frac{L}{R}\text{\hspace{1em}(12)}$$
車輛的航向變化率與車速的轉(zhuǎn)彎半徑關(guān)系$\dot{\phi }=v/R$，結(jié)合上式可得
$${\delta }_f=arctan\frac{\dot{\phi }L}{v}\text{\hspace{1em}(13)}$$

3. 算法和仿真實現(xiàn)

rear_wheel_feedback.py

import math
import numpy as npclass C:# System configK_theta = 1.0K_e = 0.5def normalize_angle(angle):a = math.fmod(angle + np.pi, 2 * np.pi)if a < 0.0:a += (2.0 * np.pi)return a - np.pidef rear_wheel_feedback_control(vehicle, ref_path):theta_e, er, k, ind = calc_preparation(vehicle, ref_path)vr = vehicle.vdot_phi = vr * k * math.cos(theta_e) / (1.0 - k * er) - \C.K_theta * abs(vr) * theta_e - C.K_e * vr * math.sin(theta_e) * er / (theta_e + 1e-19)delta = math.atan2(vehicle.L * dot_phi, vr)return delta, ind, erdef calc_preparation(vehicle, ref_path):"""計算角度誤差theta_e、橫向誤差er、曲率rk和索引index"""rx, ry, ref_yaw, ref_kappa = ref_path[:, 0], ref_path[:, 1], ref_path[:, 2], ref_path[:, 4]dx = [vehicle.x - icx for icx in rx]dy = [vehicle.y - icy for icy in ry]d = np.hypot(dx, dy)index = np.argmin(d)rk = ref_kappa[index]ryaw = ref_yaw[index]vec_nr = np.array([math.cos(ryaw + math.pi / 2.0),math.sin(ryaw + math.pi / 2.0)])vec_target_2_rear = np.array([vehicle.x - rx[index],vehicle.y - ry[index]])er = np.dot(vec_target_2_rear, vec_nr)theta_e = normalize_angle(vehicle.yaw - ryaw)return theta_e, er, rk, index

kinematic_bicycle_model.py

import math
import numpy as npclass Vehicle:def __init__(self,x=0.0,y=0.0,yaw=0.0,v=0.0,dt=0.1,l=3.0):self.steer = 0self.x = xself.y = yself.yaw = yawself.v = vself.dt = dtself.L = l  # 軸距self.x_front = x + l * math.cos(yaw)self.y_front = y + l * math.sin(yaw)def update(self, a, delta, max_steer=np.pi):delta = np.clip(delta, -max_steer, max_steer)self.steer = deltaself.x = self.x + self.v * math.cos(self.yaw) * self.dtself.y = self.y + self.v * math.sin(self.yaw) * self.dtself.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dtself.v = self.v + a * self.dtself.x_front = self.x + self.L * math.cos(self.yaw)self.y_front = self.y + self.L * math.sin(self.yaw)class VehicleInfo:# Vehicle parameterL = 3.0  #軸距W = 2.0  #寬度LF = 3.8  #后軸中心到車頭距離LB = 0.8  #后軸中心到車尾距離MAX_STEER = 0.6  # 最大前輪轉(zhuǎn)角TR = 0.5  # 輪子半徑TW = 0.5  # 輪子寬度WD = W  #輪距LENGTH = LB + LF  # 車輛長度def draw_trailer(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):vehicle_outline = np.array([[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],[vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],[vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2, vehicle_info.TW / 2]])rr_wheel = wheel.copy() #右后輪rl_wheel = wheel.copy() #左后輪fr_wheel = wheel.copy() #右前輪fl_wheel = wheel.copy() #左前輪rr_wheel[1,:] += vehicle_info.WD/2rl_wheel[1,:] -= vehicle_info.WD/2#方向盤旋轉(zhuǎn)rot1 = np.array([[np.cos(steer), -np.sin(steer)],[np.sin(steer), np.cos(steer)]])#yaw旋轉(zhuǎn)矩陣rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],[np.sin(yaw), np.cos(yaw)]])fr_wheel = np.dot(rot1, fr_wheel)fl_wheel = np.dot(rot1, fl_wheel)fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])fr_wheel = np.dot(rot2, fr_wheel)fr_wheel[0, :] += xfr_wheel[1, :] += yfl_wheel = np.dot(rot2, fl_wheel)fl_wheel[0, :] += xfl_wheel[1, :] += yrr_wheel = np.dot(rot2, rr_wheel)rr_wheel[0, :] += xrr_wheel[1, :] += yrl_wheel = np.dot(rot2, rl_wheel)rl_wheel[0, :] += xrl_wheel[1, :] += yvehicle_outline = np.dot(rot2, vehicle_outline)vehicle_outline[0, :] += xvehicle_outline[1, :] += yax.plot(fr_wheel[0, :], fr_wheel[1, :], color)ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)ax.axis('equal')

path_generator.py

"""
路徑軌跡生成器
"""import math
import numpy as npclass Path:def __init__(self):self.ref_line = self.design_reference_line()self.ref_yaw = self.cal_yaw()self.ref_s = self.cal_accumulated_s()self.ref_kappa = self.cal_kappa()def design_reference_line(self):rx, ry = [], []step_curve = 0.005 * math.pistep_line = 0.25for ix in np.arange(5, 80, step_line):rx.append(ix)ry.append(60)cx, cy, cr = 80, 45, 15theta = np.arange(math.pi/2, -math.pi/2, -step_curve)for itheta in theta:rx.append(cx + cr * math.cos(itheta))ry.append(cy + cr * math.sin(itheta))for ix in np.arange(80, 15, -step_line):rx.append(ix)ry.append(30)cx, cy, cr = 15, 15, 15theta = np.arange(math.pi/2, math.pi * 1.5, step_curve)for itheta in theta:rx.append(cx + cr * math.cos(itheta))ry.append(cy + cr * math.sin(itheta))for ix in np.arange(15, 90, step_line):rx.append(ix)ry.append(0)return np.column_stack((rx, ry))def cal_yaw(self):yaw = []for i in range(len(self.ref_line)):if i == 0:yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i, 1],self.ref_line[i + 1, 0] - self.ref_line[i, 0]))elif i == len(self.ref_line) - 1:yaw.append(math.atan2(self.ref_line[i, 1] - self.ref_line[i - 1, 1],self.ref_line[i, 0] - self.ref_line[i - 1, 0]))else:yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i -1, 1],self.ref_line[i + 1, 0] - self.ref_line[i - 1, 0]))return yawdef cal_accumulated_s(self):s = []for i in range(len(self.ref_line)):if i == 0:s.append(0.0)else:s.append(math.sqrt((self.ref_line[i, 0] - self.ref_line[i-1, 0]) ** 2+ (self.ref_line[i, 1] - self.ref_line[i-1, 1]) ** 2))return sdef cal_kappa(self):# 計算曲線各點的切向量dp = np.gradient(self.ref_line.T, axis=1)# 計算曲線各點的二階導數(shù)d2p = np.gradient(dp, axis=1)# 計算曲率kappa = (d2p[0] * dp[1] - d2p[1] * dp[0]) / ((dp[0] ** 2 + dp[1] ** 2) ** (3 / 2))return kappadef get_ref_line_info(self):return self.ref_line[:, 0], self.ref_line[:, 1], self.ref_yaw, self.ref_s, self.ref_kappa

main.py

from kinematic_bicycle_model import Vehicle, VehicleInfo, draw_trailer
from rear_wheel_feedback import rear_wheel_feedback_control
from path_generator import Path
import numpy as np
import matplotlib.pyplot as plt
import imageio.v2 as imageioMAX_SIMULATION_TIME = 200.0  # 程序最大運行時間200*dtdef main():# 設(shè)置跟蹤軌跡rx, ry, ref_yaw, ref_s, ref_kappa = Path().get_ref_line_info()ref_path = np.column_stack((rx, ry, ref_yaw, ref_s, ref_kappa))# 假設(shè)車輛初始位置為（5，55），航向角yaw=pi/6，速度為2m/s，時間周期dt為0.1秒vehicle = Vehicle(x=5.0,y=55.0,yaw=np.pi/6,v=2.0,dt=0.1,l=VehicleInfo.L)time = 0.0  # 初始時間target_ind = 0# 記錄車輛軌跡trajectory_x = []trajectory_y = []lat_err = []  # 記錄橫向誤差i = 0image_list = []  # 存儲圖片plt.figure(1)last_idx = ref_path.shape[0] - 1  # 跟蹤軌跡的最后一個點的索引while MAX_SIMULATION_TIME >= time and last_idx > target_ind:time += vehicle.dt  # 累加一次時間周期# rear_wheel_feedbackdelta_f, target_ind, e_y = rear_wheel_feedback_control(vehicle, ref_path)# 橫向誤差lat_err.append(e_y)# 更新車輛狀態(tài)vehicle.update(0.0, delta_f, np.pi / 10)  # 由于假設(shè)縱向勻速運動，所以加速度a=0.0trajectory_x.append(vehicle.x)trajectory_y.append(vehicle.y)# 顯示動圖plt.cla()plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)plt.plot(trajectory_x, trajectory_y, "-r", label="trajectory")plt.plot(ref_path[target_ind, 0], ref_path[target_ind, 1], "go", label="target")plt.axis("equal")plt.grid(True)plt.pause(0.001)plt.savefig("temp.png")#     i += 1#     if (i % 50) > 0:#         image_list.append(imageio.imread("temp.png"))## imageio.mimsave("display.gif", image_list, duration=0.01)plt.figure(2)plt.subplot(2, 1, 1)plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)plt.plot(trajectory_x, trajectory_y, 'r')plt.title("actual tracking effect")plt.subplot(2, 1, 2)plt.plot(lat_err)plt.title("lateral error")plt.show()if __name__ == '__main__':main()

運行效果：

控制效果和橫向誤差：

以上內(nèi)容僅是個人理解，如有漏誤歡迎批評指正!

文章首發(fā)公眾號：iDoitnow如果喜歡話，可以關(guān)注一下