企业网站怎么做百度,网站模板套用湖南岚鸿,网站架构图一般包括什么,珠海商城网站文章目录 1. 后轮反馈控制2. 算法原理3. 算法和仿真实现 1. 后轮反馈控制
后轮反馈控制#xff08;Rear wheel feedback#xff09;算法是利用后轮中心的跟踪偏差来进行转向控制量计算的方法#xff0c;属于Frenet坐标系的一个应用。通过选择合适的李雅普诺夫函数设计控制率… 文章目录 1. 后轮反馈控制2. 算法原理3. 算法和仿真实现 1. 后轮反馈控制
后轮反馈控制Rear wheel feedback算法是利用后轮中心的跟踪偏差来进行转向控制量计算的方法属于Frenet坐标系的一个应用。通过选择合适的李雅普诺夫函数设计控制率利用后轮中心的跟踪偏差来进行转向控制量计算的方法。
2. 算法原理 后轮反馈控制算法原理如上图所示其中 P P P当前距离车辆最近的路经点 e y e_y ey P P P点与车辆后轮中心点的横向偏差 A P AP AP实际上对应的就是frenet坐标下的 l l l φ \varphi φ车辆朝向与 X X X轴正方向的夹角即航向角 φ r \varphi_{r} φr P P P点切线与 X X X轴正方向的夹角 φ e \varphi_e φe车辆航向角误差即 φ − φ r \varphi-\varphi_{r} φ−φr n τ ⃗ \vec{n_\tau} nτ P P P点法线的单位向量 τ r ⃗ \vec{\tau_r} τr P P P点切线的单位向量 L L L轴距 δ f \delta_f δf前轮转角 v v v车辆的速度
由前面的文章frenet坐标与cartesian坐标相互转换与代码实现和上图的几何关系可得车辆在参考轨迹上的投影点 P P P处的线速度 s ˙ \dot{s} s˙和横向误差 e y e_y ey即对应frenet坐标系下的 l ˙ \dot{l} l˙的表达式为 s ˙ v ⃗ τ r ⃗ 1 − k r l ∣ v ⃗ ∣ ∣ τ r ⃗ ∣ c o s φ e 1 − k r e y ∣ v ⃗ ∣ c o s φ e 1 − k r e y (1) \dot{s}\frac{\vec{v} \vec{\tau_r}}{1-k_rl} \frac{|\vec{v}| |\vec{\tau_r}|cos{\varphi_e}}{1-k_re_y}\frac{|\vec{v}|cos{\varphi_e}}{1-k_re_y} \tag{1} s˙1−krlv τr 1−krey∣v ∣∣τr ∣cosφe1−krey∣v ∣cosφe(1) e y ˙ l ˙ v ⃗ n r ⃗ ∣ v ⃗ ∣ ∣ n r ⃗ ∣ c o s ( π 2 − φ e ) ∣ v ⃗ ∣ s i n φ e (2) \dot{e_y}\dot{l} \vec{v} \vec{n_r} |\vec{v}| |\vec{n_r}| cos({\frac{\pi}{2}-\varphi_e}) |\vec{v}| sin{\varphi_e} \tag{2} ey˙l˙v nr ∣v ∣∣nr ∣cos(2π−φe)∣v ∣sinφe(2)
由图中几何关系可得航向误差为 φ e φ − φ r (3) \varphi_e \varphi-\varphi_{r} \tag{3} φeφ−φr(3) 则航向误差变化率为 φ e ˙ φ ˙ − φ r ˙ φ ˙ − s ˙ R r φ ˙ − s ˙ k r (4) \dot{\varphi_e} \dot{\varphi}-\dot{\varphi_{r}} \dot{\varphi}-\frac{\dot{s}}{R_r } \dot{\varphi}-\dot{s}k_r \tag{4} φe˙φ˙−φr˙φ˙−Rrs˙φ˙−s˙kr(4) 注一个刚体的角速度 线速度/线速度到速度瞬心的距离上式中 R r R_r Rr为点 P P P处的瞬时圆心半径。 将(1)代入(4)可得 φ e ˙ φ ˙ − s ˙ k r φ ˙ − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y (4) \dot{\varphi_e} \dot{\varphi}-\dot{s}k_r \dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}\tag{4} φe˙φ˙−s˙krφ˙−1−kreykr∣v ∣cosφe(4) 综上可得后轮反馈控制算法对应模型的微分方程为 { s ˙ ∣ v ⃗ ∣ c o s φ e 1 − k r e y e y ˙ ∣ v ⃗ ∣ s i n φ e φ e ˙ φ ˙ − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y (5) \begin{cases} \dot{s}\quad \frac{|\vec{v}|cos{\varphi_e}}{1-k_re_y} \\ \dot{e_y}\quad |\vec{v}| sin{\varphi_e}\\ \dot{\varphi_e} \quad \dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y} \end{cases} \tag{5} ⎩ ⎨ ⎧s˙ey˙φe˙1−krey∣v ∣cosφe∣v ∣sinφeφ˙−1−kreykr∣v ∣cosφe(5)
有了模型的微分方程我们可以根据李雅普诺夫第二方法设计该模型的车辆航向变化率 φ ˙ \dot{\varphi} φ˙。 李雅普诺夫第二方法 稳定的系统能量总是不断被耗散的李雅普诺夫通过定义一个标量函数 V ( x ) V(x) V(x) 通常能代表广义能量来分析稳定性。这种方法的避免了直接求解方程也没有进行近似线性化所以也一般称之为直接法。 V ( x ) V(x) V(x)需满足 V ( x ) 0 V(x)0 V(x)0 当且仅当 x 0 x 0 x0 V ( x ) 0 V(x)0 V(x)0 当且仅当 x ≠ 0 x\neq0 x0 V ˙ ( x ) ≤ 0 \dot{V}(x)\leq0 V˙(x)≤0 当 x ≠ 0 x \neq0 x0 则称系统在李雅普诺夫意义下是稳定的特别的若 x ≠ 0 x \neq0 x0时有 V ˙ ( x ) 0 \dot{V}(x)0 V˙(x)0则系统是渐进稳定的。 这里我们比较关心的就是横向误差 e y e_y ey和 φ e \varphi_e φe期望他们随着时间的增长逐渐收敛于0我们可以定义李雅普诺夫函数形式如下 V ( e y , φ e ) 1 2 e y 2 1 2 k e φ e 2 (6) V(e_y,\varphi_e)\frac{1}{2}e_y^2\frac{1}{2k_e}\varphi_e^2 \tag{6} V(ey,φe)21ey22ke1φe2(6) 其中 k e 0 k_e0 ke0。
为了使 ( e y , φ e ) (e_y,\varphi_e) (ey,φe)在平衡点 ( 0 , 0 ) (0,0) (0,0)处稳定根据李雅普诺夫第二方法的稳定判据 V ( x ) V(x) V(x)需满足 V ( e y , φ e ) 0 V(e_y,\varphi_e)0 V(ey,φe)0 当且仅当 e y 0 , φ e 0 e_y0,\varphi_e 0 ey0,φe0 V ( e y , φ e ) 0 V(e_y,\varphi_e)0 V(ey,φe)0 当且仅当 e y ≠ 0 , φ e ≠ 0 e_y\neq0,\varphi_e\neq0 ey0,φe0 V ˙ ( e y , φ e ) ≤ 0 \dot{V}(e_y,\varphi_e)\leq0 V˙(ey,φe)≤0 当 e y ≠ 0 , φ e ≠ 0 e_y\neq0,\varphi_e\neq0 ey0,φe0
对于1、2两条我们选取的李亚普函数 V ( e y , φ e ) V(e_y,\varphi_e) V(ey,φe)显然满足。所以满足李雅普诺夫第二方法的前两条。
对于 V ˙ ( e y , φ e ) \dot{V}(e_y,\varphi_e) V˙(ey,φe)我们结合(5)可得 V ˙ ( e y , φ e ) e y e y ˙ 1 k e φ e φ e ˙ e y ∣ v ⃗ ∣ s i n φ e 1 k e φ e ( φ ˙ − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y ) (7) \begin{aligned} \dot{V}(e_y,\varphi_e) e_y\dot{e_y}\frac{1}{k_e}\varphi_e \dot{\varphi_e}\\ e_y|\vec{v}| sin{\varphi_e} \frac{1}{k_e}\varphi_e(\dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}) \end{aligned} \tag{7} V˙(ey,φe)eyey˙ke1φeφe˙ey∣v ∣sinφeke1φe(φ˙−1−kreykr∣v ∣cosφe)(7) 令 V ˙ ( e y , φ e ) 0 \dot{V}(e_y,\varphi_e)0 V˙(ey,φe)0结合(7)式可得 e y ∣ v ⃗ ∣ s i n φ e 1 k e φ e ( φ ˙ − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y ) 0 k e e y ∣ v ⃗ ∣ s i n φ e φ e φ ˙ − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y 0 (8) \begin{aligned} e_y|\vec{v}| sin{\varphi_e} \frac{1}{k_e}\varphi_e(\dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y})0\\ k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e} \dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}0 \end{aligned} \tag{8} ey∣v ∣sinφeke1φe(φ˙−1−kreykr∣v ∣cosφe)keey∣v ∣φesinφeφ˙−1−kreykr∣v ∣cosφe00(8) 由上式可以得到零界航向变化率 φ 0 ˙ \dot{\varphi_0} φ0˙等于 φ 0 ˙ k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y − k e e y ∣ v ⃗ ∣ s i n φ e φ e (9) \dot{\varphi_0}\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e} \tag{9} φ0˙1−kreykr∣v ∣cosφe−keey∣v ∣φesinφe(9) 为了使 V ˙ ( e y , φ e ) \dot{V}(e_y,\varphi_e) V˙(ey,φe)满足要求3我们可以设计一个调节函数 g ( e y , φ e ) k φ ∣ v ⃗ ∣ φ e 0 g(e_y,\varphi_e) k_\varphi |\vec{v}|\varphi_e0 g(ey,φe)kφ∣v ∣φe0其中 k φ 0 k_{\varphi}0 kφ0我们可以将车辆航向的变化率 φ ˙ \dot{\varphi} φ˙设置为 φ ˙ φ 0 ˙ − g ( e y , φ e ) k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y − k e e y ∣ v ⃗ ∣ s i n φ e φ e − k φ ∣ v ⃗ ∣ φ e (10) \dot{\varphi}\dot{\varphi_0}-g(e_y,\varphi_e)\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}-k_\varphi |\vec{v}|\varphi_e \tag{10} φ˙φ0˙−g(ey,φe)1−kreykr∣v ∣cosφe−keey∣v ∣φesinφe−kφ∣v ∣φe(10) 将(10)代入(7)得 V ˙ ( e y , φ e ) e y ∣ v ⃗ ∣ s i n φ e 1 k e φ e ( φ ˙ − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y ) e y ∣ v ⃗ ∣ s i n φ e 1 k e φ e ( k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y − k e e y ∣ v ⃗ ∣ s i n φ e φ e − k φ ∣ v ⃗ ∣ φ e − k r ∣ v ⃗ ∣ c o s φ e 1 − k r e y ) e y ∣ v ⃗ ∣ s i n φ e 1 k e φ e ( − k e e y ∣ v ⃗ ∣ s i n φ e φ e − k φ ∣ v ⃗ ∣ φ e ) − k φ k e ∣ v ⃗ ∣ φ e 2 0 (11) \begin{aligned} \dot{V}(e_y,\varphi_e) e_y|\vec{v}| sin{\varphi_e} \frac{1}{k_e}\varphi_e(\dot{\varphi}-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y})\\ e_y|\vec{v}| sin{\varphi_e} \frac{1}{k_e}\varphi_e(\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}-k_\varphi |\vec{v}|\varphi_e-\frac{k_r|\vec{v}|cos{\varphi_e}}{1-k_re_y}) \\ e_y|\vec{v}| sin{\varphi_e}\frac{1}{k_e}\varphi_e(-k_ee_y|\vec{v}| \frac{sin{\varphi_e}}{\varphi_e}-k_\varphi |\vec{v}|\varphi_e) \\ -\frac{k_\varphi}{k_e} |\vec{v}|\varphi_e^2 0 \end{aligned} \tag{11} V˙(ey,φe)ey∣v ∣sinφeke1φe(φ˙−1−kreykr∣v ∣cosφe)ey∣v ∣sinφeke1φe(1−kreykr∣v ∣cosφe−keey∣v ∣φesinφe−kφ∣v ∣φe−1−kreykr∣v ∣cosφe)ey∣v ∣sinφeke1φe(−keey∣v ∣φesinφe−kφ∣v ∣φe)−kekφ∣v ∣φe20(11) 因此我们设计的公式(10)即车辆航向的变化率 φ ˙ \dot{\varphi} φ˙满足李雅普诺夫的稳定性条件。
根据前面文章介绍过的车辆运动学模型下的几何关系 t a n δ f L R (12) tan\delta_f\frac{L}{R} \tag{12} tanδfRL(12) 车辆的航向变化率与车速的转弯半径关系 φ ˙ v / R \dot{\varphi}v/R φ˙v/R结合上式可得 δ f a r c t a n φ ˙ L v (13) \delta_farctan\frac{\dot{\varphi}L}{v} \tag{13} δfarctanvφ˙L(13)
3. 算法和仿真实现
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和索引indexrx, 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,x0.0,y0.0,yaw0.0,v0.0,dt0.1,l3.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_steernp.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 # 最大前轮转角TR 0.5 # 轮子半径TW 0.5 # 轮子宽度WD W #轮距LENGTH LB LF # 车辆长度def draw_trailer(x, y, yaw, steer, ax, vehicle_infoVehicleInfo, colorblack):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#方向盘旋转rot1 np.array([[np.cos(steer), -np.sin(steer)],[np.sin(steer), np.cos(steer)]])#yaw旋转矩阵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, axis1)# 计算曲线各点的二阶导数d2p np.gradient(dp, axis1)# 计算曲率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_kappamain.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():# 设置跟踪轨迹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))# 假设车辆初始位置为555航向角yawpi/6速度为2m/s时间周期dt为0.1秒vehicle Vehicle(x5.0,y55.0,yawnp.pi/6,v2.0,dt0.1,lVehicleInfo.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)# 更新车辆状态vehicle.update(0.0, delta_f, np.pi / 10) # 由于假设纵向匀速运动所以加速度a0.0trajectory_x.append(vehicle.x)trajectory_y.append(vehicle.y)# 显示动图plt.cla()plt.plot(ref_path[:, 0], ref_path[:, 1], -.b, linewidth1.0)draw_trailer(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)plt.plot(trajectory_x, trajectory_y, -r, labeltrajectory)plt.plot(ref_path[target_ind, 0], ref_path[target_ind, 1], go, labeltarget)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, duration0.01)plt.figure(2)plt.subplot(2, 1, 1)plt.plot(ref_path[:, 0], ref_path[:, 1], -.b, linewidth1.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()运行效果 控制效果和横向误差 以上内容仅是个人理解如有漏误欢迎批评指正! 文章首发公众号iDoitnow如果喜欢话可以关注一下
