1 Stanley方法

  Stanley与pure pursuit方法都是基于几何的路径跟踪方法，pure pursuit的思想是要让车辆的后轴中心经过目标点，从而计算车辆的前轮转角。Stanley则除了利用横向跟踪误差外，还利用车辆的航向角偏差来计算车辆的前轮转角。因此相对于pure pursuit方法，stanley的计算起来要更复杂一些，它还需要轨迹线的角度作为输入，同时在弯路上的跟踪效果精度要优于pure pursuit

  Stanley基于航向角误差${\theta }_e$与横向误差$e$，如上图。其中航向角误差是当前车辆航向角与距离前轮中心最近的轨迹点所在的切线方向的夹角。在忽略航向角误差的情况下，横向误差$e$越大，前轮转角应该越大（很符合直觉，车要想尽力开到轨迹线上就会努力往那个方向打轮）。$d(t)$是最近轨迹点所在的切线方向与前轮预期的行进方向的交点产生，则有：
$${\delta }_e(t)=arctan\frac{e(t)}{d(t)}$$
由于$d(t)$与车速相关，可以写成$v(t)$的形式

$${\delta }_e(t)=arctan\frac{ke(t)}{v(t)}$$
最后：
$$\delta (t)={\theta }_e(t)+arctan\frac{ke(t)}{v(t)}$$

2 实现

  同样利用manim动画系统演示一下

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))
		
		# 轨迹线，sin函数
        trajectory = np.zeros((1000, 2))
        trajectory[:, 0] = np.linspace(0, 1000, 1000)
        trajectory[:, 1] = 5 * np.sin(trajectory[:, 0] / 20)
        # 轨迹点的切线方向，sin的导数
        trajectory_head = 0.25 * np.cos(trajectory[:, 0] / 20)

        refer_tree = KDTree(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 = 5.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
        pidx = 0
        k = 0.5
        for i in range(1000):
            self.frame.move_to(axes.c2p(car.x, car.y))
            car_pos = np.array([car.x, car.y])
            car_fwheel = np.array([car.x + car.L * np.cos(car.psi),
                                   car.y + car.L * np.sin(car.psi)])

            _, idx = refer_tree.query(car_fwheel)
            if idx < pidx:
                idx = pidx
            else:
                pidx = idx

            # 前轮中心到当前最近的轨迹线的距离
            dist = np.linalg.norm(car_fwheel - trajectory[idx])
            # 前轮中心到最近的轨迹点的向量，以及该向量的角度
            dx, dy = trajectory[idx] - car_fwheel
            alpha = math.atan2(dy, dx)
            # 这个是航向角误差
            dtheta = normalize_angle(trajectory_head[idx] - car.psi)
    		# 横向误差
            e = np.sign(np.sin(alpha - car.psi)) * dist
            theta_d = np.arctan2(k*e, car.v)
            # 前轮转角
            delta = dtheta + theta_d
            #  限制车轮转角 [-30, 30]
		    if delta > np.pi / 6.0:
		        delta = np.pi / 6.0
		    elif delta < - np.pi / 6.0:
		        delta = - np.pi / 6.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.01)

运行效果如下

3 参考资料

• https://blog.csdn.net/renyushuai900/article/details/98460758
• https://windses.blog.csdn.net/article/details/103518011
• manim动画系统