单车模型下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)

        # trajectory
        trajectory_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.8
        v = 10.0 # 初始速度

        # 初始化单车模型，仿真模拟步长0.1s
        car = 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 = 0
        ld = 1.0 * v + 2.0  # 前视距离选取关于速度的线性函数，k=1.0,b=2.0
        for 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 = i
                    break

            # pure pursuit算法
            dist = np.linalg.norm(car_pos - trajectory[idx])
            dx, dy = trajectory[idx] - car_pos
            alpha = 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)

跑出来的效果如下：