1
import math
2

3
def pure_pursuit(state, path, v, Ld_min=0.3, k=1.0):
4
    """
5
    state: (x, y, theta) 当前位姿
6
    path: [(x1,y1), (x2,y2), ...] 路径点列表
7
    v: 当前线速度
8
    """
9
    x, y, theta = state
10

11
    # 1. 找到路径上离车最近的点
12
    min_dist = float('inf')
13
    nearest_idx = 0
14
    for i, (px, py) in enumerate(path):
15
        d = math.hypot(px - x, py - y)
16
        if d < min_dist:
17
            min_dist = d
18
            nearest_idx = i
19

20
    # 2. 从最近点往前搜索，找到距离 >= Ld 的前瞻点
21
    Ld = max(Ld_min, k * v)
22
    target = path[-1]  # 默认用终点
23
    for i in range(nearest_idx, len(path)):
24
        px, py = path[i]
25
        d = math.hypot(px - x, py - y)
26
        if d >= Ld:
27
            target = (px, py)
28
            break
29

30
    # 3. 把前瞻点转换到车体坐标系
31
    dx = target[0] - x
32
    dy = target[1] - y
33
    local_x =  dx * math.cos(theta) + dy * math.sin(theta)
34
    local_y = -dx * math.sin(theta) + dy * math.cos(theta)
35

36
    # 4. 算曲率和角速度
37
    Ld_actual = math.hypot(local_x, local_y)
38
    if Ld_actual < 0.01:
39
        return 0.0
40
    curvature = 2.0 * local_y / (Ld_actual ** 2)
41
    omega = v * curvature
42

43
    return omega

1
import heapq
2

3
def astar(grid, start, end):
4
    """grid: 2D 数组, 0=可通行, 1=障碍"""
5
    rows, cols = len(grid), len(grid[0])
6
    open_set = [(0, start)]
7
    came_from = {}
8
    g_score = {start: 0}
9

10
    while open_set:
11
        _, current = heapq.heappop(open_set)
12
        if current == end:
13
            path = [current]
14
            while current in came_from:
15
                current = came_from[current]
16
                path.append(current)
17
            return path[::-1]
18

19
        for dx, dy in [(-1,0),(1,0),(0,-1),(0,1),(-1,-1),(-1,1),(1,-1),(1,1)]:
20
            nx, ny = current[0]+dx, current[1]+dy
21
            if 0 <= nx < rows and 0 <= ny < cols and grid[nx][ny] == 0:
22
                new_g = g_score[current] + (1.414 if dx*dy != 0 else 1)
23
                if new_g < g_score.get((nx,ny), float('inf')):
24
                    g_score[(nx,ny)] = new_g
25
                    f = new_g + abs(nx-end[0]) + abs(ny-end[1])
26
                    heapq.heappush(open_set, (f, (nx,ny)))
27
                    came_from[(nx,ny)] = current
28
    return []

1
def bezier_curve(points, n=100):
2
    """n 阶贝塞尔曲线，points 是控制点列表"""
3
    n_pts = len(points)
4
    curve = []
5
    for t_i in range(n + 1):
6
        t = t_i / n
7
        # de Casteljau 算法
8
        temp = list(points)
9
        for k in range(n_pts - 1):
10
            temp = [
11
                ((1-t) * temp[i][0] + t * temp[i+1][0],
12
                 (1-t) * temp[i][1] + t * temp[i+1][1])
13
                for i in range(len(temp) - 1)
14
            ]
15
        curve.append(temp[0])
16
    return curve

1
class LowPassFilter:
2
    def __init__(self, alpha=0.3):
3
        self.alpha = alpha
4
        self.value = None
5

6
    def update(self, new_value):
7
        if self.value is None:
8
            self.value = new_value
9
        else:
10
            self.value = self.alpha * new_value + (1 - self.alpha) * self.value
11
        return self.value
12

13
# 用法
14
filter_x = LowPassFilter(alpha=0.3)
15
filter_y = LowPassFilter(alpha=0.3)
16

17
while running:
18
    raw_x, raw_y = get_position()
19
    x = filter_x.update(raw_x)
20
    y = filter_y.update(raw_y)
21
    omega = pure_pursuit((x, y, theta), path, v)