[F1TENTH] Lane Obstacle Predictor (작성중)

Kim JuYoung·2024년 3월 30일

F1TENTH

목록 보기

1/3

🎨 공부하면서 작성하는 글입니다. 잘못된 정보, 오류 등은 댓글로 말씀해주시면 감사하겠습니다.

개요

맵 기반으로 로봇이 이동할 경로를 생성하는 알고리즘인 Trajectory Generator 를 통해 경로를 먼저 생성하고 해당 경로를 기준으로 양옆에 일정 간격을 두고 경로를 감싸는 라인을 생성한다. 이 라인의 범위를 자동차 폭을 고려해서 생성하면 로봇이 이동할 경로 위에 장애물이 있는지 판단할 수 있다.

내용

Trajectory Generator

Trajectory Generator는 ICRA'22 F1TEHTH Stack code 에 포함되어 있는 코드이다.

코드를 실행하면 맵을 기반으로 inner_bound, outer_bound를 생성하는데 이 때 config 폴더의 paramas.yaml에서 opp_safe_dist를 조절하여 패딩처럼 값을 설정할 수 있다.

생성된 bound를 기준으로 lane_0, lane_1, centerline을 생성하고 가속, 감속 구간 등을 정해서 최적의 경로 계획을 생성한다.

장애물이 없는 일반적인 상황이라면 계획된 경로를 따라서 원활하게 이동하는 것을 볼 수 있다.

그러나 기존 경로 계획에 없던 장애물이 맵에 나타나면 문제가 발생한다.

이 때문에 장애물을 판단하는 알고리즘을 통해 적절한 대응이 필요하다.

Opponent Predictor

Opponent Predictor는 상대차와 장애물을 판별하는 알고리즘의 코드이다. 먼저 LiDAR /scan 토픽을 통해 장애물을 판단한다.

그런데 라이다에 반사되어 돌아온 모든 결과가 장애물이라고 판단되면 안되기 때문에 Trajectory Generator에서 생성되는 inner_bound, outer_bound을 기준으로 두 라인 안에 존재 하면 장애물이라고 판단한다.

# Find obstacle that is on the track
inside_outer_bound = cv2.pointPolygonTest(self.outer_bound, (x, y), False)
outside_inner_bound = cv2.pointPolygonTest(self.inner_bound, (x, y), False)

그래서 위 영상 처럼 진행 경로 밖에 위치하는 장애물을 판단할 수 있다.

lane_follow.py 에는 opponent_predictor와 연계하여 현재 라인에 장애물이 있을 경우 천천히 감속하는 코드가 있다.

단순히 감속만 하기 위함이면 현재 이 상태로도 괜찮지만 장애물에 꼬라박으면 의미가 있을까 하는생각에 나는 이 기능을 일부 수정하여 천천히 감속하는게 아니라 follow the gap method로 전환하는 코드로 개조를 했었다.

장애물이 감지되면 fgm 알고리즘으로 주행하는 간단한 방법인데 fgm의 원초적 문제점 때문에 벽에 꼬라박는 문제점은 나중에 해결해야한다.

Lane Obstacle Predictor

장애물을 회피하는 알고리즘은 나중에 개선한다치고 불필요한 장애물까지 감지를 하게 되면 불필요한 주행 알고리즘 변환 과정으로인해 최적의 경로 주행을 하지 못할 수 있다. 그래서 주행하는 라인에 장애물이 위치하는지만 판단한다면 불필요한 주행 알고리즘 전환이 줄어들 수 있다는게 나의 생각이다.

위 사진처럼 벽이 있고 그 사이에 로봇이 주행해야할 좌표가 있다고 할 때 그 좌표를 중심으로 로봇 자동차의 크기를 고려한 범위 만큼만 장애물이 있는지 판단하는것이다.

현재 opponent predictor의 장애물 판단 알고리즘과 얼추 비슷하지만 벽과 벽 사이의 간격을 주고 inner_bound, outer_bound를 정하는데 위 사진 처럼 실제 optimal_lane 밖에 있지만 이 bound 안에 존재하는 모든건 장애물로 인식한다.

위 사진의 경우에는 문제 없이 지나가겠지만 만약 장애물의 존재에 따라 주행 알고리즘을 전환해야한다면? 그럼 어디까지 장애물이 존재해야 장애물로 판단하고 주행알고리즘을 전환할 것인지 정해야할것이다.

기존의 opponent predictor 처럼 inner_bound와 outer_bound를 만드는 간격을 벽에서 더 멀리 떨어트려 lane_optimal에 가깝게 bound를 만들어도 되지만 이게 곧 lane_0, lane_1이 되고 이 사이를 lane_optimal으로 만들기 때문에 그냥 맵의 중앙을 optimal_lane으로 만들 수 있기 때문에 최적과는 거리가 멀어질 것이다.

그래서 장애물을 덜 인식하는 방법으로 기존의 inner_bound와 outer_bound 말고 또 다른 inner_bound와 outer_bound를 따로 만들어서 실제 주행에 있어서 방해되지 않는 장애물은 무시하는 방법으로 장애물 인식 알고리즘을 만든다면 불필요한 주행 알고리즘 전환이 일어나지 않을 수 있다.

inner_bound와 outer_bound를 만드는 방법에는 두 가지 방법이 있다. 벽과 벽 사이의 간격을 기준으로 만드는 방법이 있고 다른 방법은 주행 경로를 기준으로 일정 범위를 만드는 방법이 있다. 이 글에서는 두 번째 방법을 설명하려고 한다.

lane_optimal.csv
-68.973	54.336	8.5
-69.173	54.327	8.5
-69.372	54.317	8.5
-69.572	54.306	8.5
-69.771	54.294	8.5
-69.971	54.282	8.5
-70.17	54.268	8.5
-70.37	54.253	8.411
-70.569	54.237	8.219
-70.768	54.219	8.024
-70.967	54.2	7.825
-71.166	54.18	7.617
-71.365	54.159	7.402
-71.563	54.136	7.178

주행 경로는 수많은 (x, y) 좌표로 이뤄진 경우가 많다. 당장 csv 폴더 들어가서 lane_0, lane_1, centerline, lane_optimal을 다 열어봐도 이런 식일것이다. (세 번째 열은 속도로 추정됨)

주행 경로 기준으로 일정 범위를 만드는 방법은 간단하다.

이 x, y 좌표를 서로 연결하여 삼각함수를 통해 이에 수직하는 선분을 그으면 그게 범위가 된다.

우리는 이 방법을 사용하기 전에 현재 좌표 D, 다음 좌표 A, 그리고 범위가될 길이 w를 알고 있다.

우선 직각삼각형을 생각해보자

직각삼각형을 마우스로 그렸는데 아무튼 직각이니까 직각삼각형이라고 생각하자

현재 주행중인 좌표가 D, 다음 이동할 좌표가 A, 그리고 선분 BC의 길이가 w, 선분 AD의 길이가 h가 될것이다.

여기서 점 B와 C를 구하는 방법은 다음과 같다.

먼저 점 A와 D의 기울기 ${\theta}$ 를 구한다.
수직선을 구하기 위해 기울기의 곱이 -1이 되어야 하므로 $\cfrac{-1}{\theta}$ 변수를 저장한다.
이 변수에 아크탄젠트 함수로 각도를 라디안 단위인 각도로 변환한다.

만약 여기서 3번 작업인 라디안 단위로 각도를 변환하지 않으면 방향을 제대로 찾지 못한다.

변환된 각도에 cos, sin함수를 x, y를 각각 적용하고 w/2만큼 곱해줘서 dx, dy변수에 저장한다.
마지막으로 점 D 좌표에서 왼쪽 좌표는 dx, dy만큼 빼고 오른쪽 좌표는 dx, dy만큼 더하면 된다.

def find_perpendicular_line_endpoints(A, B, L):
    x1, y1 = A
    x2, y2 = B
    if x2 - x1 == 0:  # 선분 AB가 수직일 경우
        dx = L / 2
        dy = 0
    elif y2 - y1 == 0:  # 선분 AB가 수평일 경우
        dx = 0
        dy = L / 2
    else:
        slope = (y2 - y1) / (x2 - x1)
        angle = math.atan(-1 / slope)
        dx = (L / 2) * math.cos(angle)
        dy = (L / 2) * math.sin(angle)

    endpoint1 = (x2 + dx, y2 + dy)
    endpoint2 = (x2 - dx, y2 - dy)
    return endpoint1, endpoint2