参考网址

路径规划与轨迹跟踪系列算法学习_第10讲_纯跟踪法_哔哩哔哩_bilibili

(116条消息) 无人车系统（五）：轨迹跟踪Pure Pursuit方法_windSeS的博客-CSDN博客

AtsushiSakai/PythonRobotics: Python sample codes for robotics algorithms. (github.com)

算法流程:

1647753144_1_.png

代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
def search_target_index(self, state):

    # To speed up nearest point search, doing it at only first time.
    if self.old_nearest_point_index is None:
        # search nearest point index
        dx = [state.rear_x - icx for icx in self.cx]
        dy = [state.rear_y - icy for icy in self.cy]
        d = np.hypot(dx, dy)
        ind = np.argmin(d)
        self.old_nearest_point_index = ind
    else:
        ind = self.old_nearest_point_index
        distance_this_index = state.calc_distance(self.cx[ind],
                                                    self.cy[ind])
        while True:
            distance_next_index = state.calc_distance(self.cx[ind + 1],
                                                        self.cy[ind + 1])
            if distance_this_index < distance_next_index:
                break
            ind = ind + 1 if (ind + 1) < len(self.cx) else ind
            distance_this_index = distance_next_index
        self.old_nearest_point_index = ind

    Lf = k * state.v + Lfc  # update look ahead distance #更新前瞻距离,与速度有关

    # search look ahead target point index # 搜索前瞻目标点索引
    while Lf > state.calc_distance(self.cx[ind], self.cy[ind]):
        if (ind + 1) >= len(self.cx):
            break  # not exceed goal
        ind += 1

    return ind, Lf
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
def pure_pursuit_steer_control(state, trajectory, pind):
    ind, Lf = trajectory.search_target_index(state)

    if pind >= ind:
        ind = pind

    if ind < len(trajectory.cx):
        tx = trajectory.cx[ind]
        ty = trajectory.cy[ind]
    else:  # toward goal
        tx = trajectory.cx[-1]
        ty = trajectory.cy[-1]
        ind = len(trajectory.cx) - 1
	
    alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw

    delta = math.atan2(2.0 * WB * math.sin(alpha) / Lf, 1.0)

    return delta, ind