lattice planner 规划2_lattice planner代码

书接上回

local planner

继续说local planner

local planner下主要有这些函数：
add emergency stop 车辆的紧急制动功能，这个优先级最高，也是为了保证行驶安全，当察觉设定最小距离内有车辆就行驶紧急制动功能
set target speed 设置目标速度，车辆在路上不是无限制的加速行驶，此函数来高速车辆当前道路最高限速，保证车辆不会超速
follow speed limit 跟车速度
get local planner 是局部规划，因此lattice 算法就是与此部分进行结合
get global planner 是全局规划，在给车辆起始点和目的地时候已经确定了，carla的全局规划是规划出以队列形式的点集合，用pid去寻迹这些点，因此如果局部规划要更改全局规划时候，需要把障碍物那块的点集合删除，用新的点集合去代替，这样就实现了实时规划路径。
set destination是设置目标点

lattice 模块

lattice 模块主要有两个文件

collision checker 是当检测到前方有障碍物之后，选择最接近中心线和最远的路径索引，远离碰撞路径。

 def collision_check(self, paths, obstacles): 
         collision_check_array = np.zeros(len(paths), dtype=bool)
        for i in range(len(paths)):
            collision_free = True
            path           = paths[i]

            # Iterate over the points in the path.
            for j in range(len(path[0])):
                # 将障碍物中心为圆心画圆，这个圆表示碰撞区域，
                # 圆偏移量由 self._circle_offsets 给出。
                # 沿路径的每个点都需要放置圆偏移量、
                # 偏移量根据车辆偏航方向旋转。
                # 每条路径的形式为 [[x_values], [y_values]、
                # [θ_值]]的形式，其中每个 x_值、y_值和
                # 偏航角度是按顺序排列的。
                 因此，我们需要计算
                # circle_x = point_x + circle_offset*cos(yaw)
                # circle_y = point_y circle_offset*sin(yaw)
                # 沿着路径的每个点。
                # point_x 由 全局规划给出，
                 # 假定每个障碍物都是由一组
                # 形状为 [x, y] 的点的集合。
                # 在这里，我们将遍历障碍物点，并检查
                # 是否有任何一个障碍点位于我们的任何一个圆内。
                # 如果是，那么路径将与障碍物发生碰撞，并且
                # 对于此标志，应将 collision_free 标志设为 false
                for k in range(len(obstacles)):
                    collision_dists = \
                        scipy.spatial.distance.cdist(obstacles[k], 
                                                     circle_locations)
                    collision_dists = np.subtract(collision_dists, 
                                                  self._circle_radii)
                    collision_free = collision_free and \
                                     not np.any(collision_dists < 0)

                    if not collision_free:
                        break
                if not collision_free:
                    break

            collision_check_array[i] = collision_free

        return collision_check_array
    #根据路径与车道中心线的紧密程度以及与其他路径的距离，选择路径集中的最佳路径。
    # 选择路径集中的最佳路径。
    # 碰撞的路径的距离。
    # 取消与障碍物碰撞的路径的选择资格。
    # 过程。
    如果 paths[i] 是 # 无碰撞的，则碰撞检查数组在索引 i 处包含 True。
    # 则在索引 i 处包含 True，否则包含 False。
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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

def select_best_path_index(self, paths, collision_check_array, goal_state)：
        “””
        选择最接近中心线和最远的路径索引
        远离碰撞路径。
        返回最适合车辆穿越的路径索引。
        的路径索引。

        选择最接近中心线和最远的路径碰撞索引远离路径。
        远离碰撞路径的路径索引。
        collision_check_array： 布尔值列表，用于区分
                路径是否无碰撞（true）或无碰撞（false）。碰撞数组中的
                碰撞检查数组列表中的第
                路径列表中的第 i 条路径。
            goal_state（目标状态）： 车辆要达到的目标状态（中心线目标）。
                格式为 [x_goal，y_goal，v_goal]，单位： [米、米、米/秒］
        有用变量
            self._weight： 与最佳指数得分相乘的权重。
        返回：
            best_index： 最适合车辆导航的路径索引。
                导航的路径索引。
        best_index = None
        best_score = float('Inf')
        for i in range(len(paths))：
            # 处理无碰撞路径的情况。
            if collision_check_array[i]：
                # 计算 “距中心线距离 ”得分。
                # 中心线目标由 goal_state 给出。
                # 目标函数的具体选择由您决定。
                # 分数越低，表示路径越合适
                score = np.sqrt((goal_state[0] - paths[i][0][len(paths[i][0])-1])**2 + (goal_state[1] - paths[i][1][len(paths[i][0])-1])**2)
                 # 计算 “与其他碰撞路径的距离 ”得分，并将其
                # 将其与 “与中心线的距离 ”得分相加。
                # 目标函数的具体选择由您决定。
                for j in range(len(paths))：
                    if j == i：
                        继续
                    else：
                        if not collision_check_array[j]：
                            # todo: 在虚线之间插入代码
                      
                            print(“Adding score”)
                            score += self._weight * paths[i][2][j]
            # 处理路径碰撞的情况。
            else：
                score = float('Inf')

            print(“score = %f” % score)
                
            # 设置最佳索引为得分最低的路径索引

            如果 score < best_score：
                best_score = score
                best_index = i

        print(“--------------------”)

        return best_index
        到此搜索出最佳路径
        
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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59

但此时最佳路径可能就包含2个点，只有足够多的点才能让车去光滑无摆动的去寻迹
这时通过路径优化将在这两个点中优化出连续的多个点，车辆换道才能顺滑
for goal_state in goal_state_set:
path = self._path_optimizer.optimize_spiral(goal_state[0],
goal_state[1],
goal_state[2])
if np.linalg.norm([path[0][-1] - goal_state[0],
path[1][-1] - goal_state[1],
path[2][-1] - goal_state[2]]) > 0.1:
path_validity.append(False)
else:
paths.append(path)
path_validity.append(True)
return paths, path_validity
这就是路径优化的功能

最后将产生的路径点去替代全局规划的点，就完成了避障。
到此结束，如果各位想要完全代码的，可以私信我18734916009（小偿）

编写不易，希望点个赞