单车模型下pure pursuit循迹

本文主要是介绍单车模型下pure pursuit循迹，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

前置：单车模型及其线性化

1 pure pursuit方法

  单车模型下的状态更新为，详细的变量含义在前置链接中
$$\dot{S}=\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{c}vcos(\psi )\\ vsin(\psi )\\ v\frac{tan(\psi )}{L}\end{array}\right]$$
pure pursuit方法能够做到：在给定轨迹下，控制单车模型的前轮转角${\delta }_f$，从而让单车模型能够跟随轨迹运动。如下图所示，对于给定目标点$(g_x,g_y)$，pure pursuit方法输出特定的${\delta }_f$使其后轴中心能够通过目标点，从而达到轨迹跟踪的作用。

根据几何关系，可以得到：
$${\delta }_f=arctan(\frac{2Lsin\alpha }{l_d})$$
其中$l_d$被称为前视距离，在实际应用中，我们根据$l_d$去选择一个时间步下的预瞄点，前视距离一般设定为速度的线性函数

2 实现例子

  这是一个通过manim动画系统实现的Pure pursuit例子。

class FollowTraj(Scene):def construct(self) -> None:self.frame.set_width(80)axes = Axes(x_range=[-500, 500], y_range=[-20, 20])self.play(ShowCreation(axes))trajectory = np.zeros((150, 2))trajectory[:, 0] = np.linspace(0, 150, 150)trajectory[:, 1] = 5 * np.sin(trajectory[:, 0] / 20)# trajectorytrajectory_dot = VGroup()for i in range(len(trajectory)):dot = Dot().move_to(axes.c2p(trajectory[i, 0], trajectory[i, 1]))dot.set_color(BLUE)trajectory_dot.add(dot)self.play(ShowCreation(trajectory_dot))L = 2.8v = 10.0 # 初始速度# 初始化单车模型，仿真模拟步长0.1scar = KinematicModelBackCenter(0, 2.0, 0, v, L, 0.1)# 动画相关car_center = Dot(color=RED).move_to(axes.c2p(car.x, car.y))car_polygon = get_polygon(car.x, car.y, car.psi, 3.0, 1.0, 2.1).set_color(RED)idx = 0ld = 1.0 * v + 2.0  # 前视距离选取关于速度的线性函数，k=1.0,b=2.0for i in range(150):self.frame.move_to(axes.c2p(car.x, car.y))car_pos = np.array([car.x, car.y])# 找到距离前视距离范围外第一个点，作为预瞄点for i in range(idx, len(trajectory)):dis = np.linalg.norm(car_pos - trajectory[i])if dis >= ld:idx = ibreak# pure pursuit算法dist = np.linalg.norm(car_pos - trajectory[idx])dx, dy = trajectory[idx] - car_posalpha = math.atan2(dy, dx)delta = math.atan2(2.0 * L * math.sin(alpha - car.psi) / dist, 1.0)# 更新车辆状态car.update_state(0, delta)# 动画相关cur_car_center = Dot(color=RED).move_to(axes.c2p(car.x, car.y))cur_car_polygon = get_polygon(car.x, car.y, car.psi, 3.0, 1.0, 2.1).set_color(RED)# 动画相关self.play(Transform(car_center, cur_car_center),Transform(car_polygon, cur_car_polygon), run_time=0.02)

跑出来的效果如下：

这篇关于单车模型下pure pursuit循迹的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！

---