Global Path Planning & Local Path Planning (MORAI 시뮬레이터)

국토부 대회에서 erp를 사용하는거 말고도 시뮬레이션 대회도 같이 진행한다고 했다. 자세한건 귀찮으니 바로 로직 설명으로 ㄱㄱ

 

1. Global Path Planning

예전에 쓰던 A* 알고리즘을 사용했다. 다익스트라 알고리즘과 유사하지만, 목표 노드와의 추정거리(휴리스틱) 까지 고려한다. 따라서 목표 노드에 더 가까운 노드를 우선적으로 탐색할 수 있다는 뜻. 그리고 morai에는 교차로마다 node가 있고 갈 수 있는 경로가 link로 이어져 있다.

 

노드와 링크가 있으니 global path는 노드를 몇개 추출하여, 그 노드들 잇는 하나의 path를 생성한다. 이 path가 global path가 되는것이다.

 

2. Local Path Planning

local path는 lattice planner를 사용했다. 우선 /global path에서 현재 차량위치로부터 일정 거리 만큼 경로를 잘라내어 /local path 토픽으로 발행한다. lattice planner 노드는 /local_path, /Ego_topic(차량 정보), /Object_topic(장애물 정보)를 구독하고, 다음 checkObject 함수를 통해 /local_path 상에 장애물이 있는지 확인한다.

    def checkObject(self, ref_path, object_data):
        is_crash = False
        for obstacle in object_data.obstacle_list:
            for path in ref_path.poses:
                dis = sqrt(pow(path.pose.position.x - obstacle.position.x, 2) + pow(path.pose.position.y - obstacle.position.y, 2))
                if dis < 5:
                    is_crash = True
                    break
        return is_crash

 

장애물이 없는 경우엔 원래 발행하던 /local_path를 그대로 발행하지만, 장애물이 있는경우 횡방향 오프셋을 사용하여 6개의 후보 회피 경로를 생성한다. 그리고 다음 collision_check 함수를 통해 각 후보 경로의 충돌 위험을 계산하여 가장 가중치가 낮은 안전한 경로를 선택한다. 그리고 선택된 최적의 회피 경로를 /lattice_path 토픽으로 발행한다.

 

    def collision_check(self, object_data, out_path):
        selected_lane = -1
        lane_weight = [3, 2, 1, 1, 2, 3]
 
        for obstacle in object_data.obstacle_list:
            for path_num in range(len(out_path)):
                for path_pos in out_path[path_num].poses:
                    dis = sqrt(pow(obstacle.position.x - path_pos.pose.position.x, 2) + pow(obstacle.position.y - path_pos.pose.position.y, 2))
                    
                    if dis < 1.5:
                        weight_increase = 200
                    elif dis < 4:
                        weight_increase = 100
                    elif dis < 7:
                        weight_increase = 50
                    else:
                        weight_increase = 0
                    lane_weight[path_num] += weight_increase
                    
        selected_lane = lane_weight.index(min(lane_weight))
        return selected_lane

 

다음은 결과 동영상이다.