GPS를 활용한 자율 운항 선박의 네비게이션

참고 블로그

🐢 아래 내용은 위 블로그에서 설명하고자 하는 내용을 학습하고자 제 블로그에 정리하는 내용들입니다. 🐢

오토파일럿 호핑투어

알고리즘 설계

호핑투어는 주어진 GPS 좌표의 2개 이상의 지점 위치를 순차적으로 방문해야 하는 미션이다. 사전에 각 waypoint를 입력하고, 순서대로 현 시점 t의 목표 지점(goal)으로 삼는다. 한 목표 지점에 도착하면 다음 순서의 waypoint를 목표 지점으로 교체하여 이를 추종하도록 한다.

호핑투어의 보트 제어에는 PID 제어기를 사용하여 회전각과 전진 속도를 결정한다. 회전각의 측면에서는 다음 지정 위치로 선수 방향을 위치 시키기 위해 $ \psi _e $를 제어기에 입력하고 연산을 통해 서보 모터의 출력을 얻는다. 전진 속도의 측면에서는 지정 위치까지 떨어진 거리에 따른 선속을 제어하는데 PID 제어를 사용하며, 거리가 멀 때는 높은 선속으로 운항하고 가까울 때는 선속을 줄인다.

아래는 해당 알고리즘의 Pseudo Code 이다.

while 모든 waypoints를 다 방문하지 않음
    if 현재 추종하는 waypoint에 도착했음 then
        추종점을 다른 waypoint로 변경함
    else
        waypoint의 위치와 현재 선수각 간의 에러를 계산함
        PID 제어기 에러각을 입력하여 서보 모터 제어값을 출력함
        waypoint로부터 현재 위치 간의 거리를 계산
        PID 제어기에 거리를 입력하여 추진기 제어값을 출력함
        각각 구한 두 제어값을 이용해 실제로 보트를 이동시킴
    endif
endwhile

자율운항 임무 중 장애물 통과

알고리즘 구상

목표 지점까지 이동하며 도중에 장애물을 회피하는 경로를 탐색하는 알고리즘에는 Dijkstra, A, D 등 유명한 알고리즘이 존재한다. 그러나 대부분의 기존 경로 탐색 알고리즘의 경우 그리드 기반의 방식으로, 이동체의 non-holonomic한 특성을 고려하지 못한다.

지면에서 움직이는 이동체와 물 속에서 움직이는 선박은 동일한 알고리즘을 사용하는 데에는 무리가 있다.

그래서 위 블로그에서는 다음 시점 t + 1에 위치하길 원하는 특정 위치 ($(x_{t+1}, y_{t+1})$)나 몇개의 timestamp 이후의 예상 경로를 결정하는 것이 아닌, 다음 시점 t + 1에 가지길 바라는 선수각의 각도와 목표각 $ \psi_d $를 탐색하는 방식을 사용한다. 현 시점 t에서의 센싱 데이터를 바탕으로 제어기를 조정하여 t + 1 시점의 선수각을 만들었다.

자율운항 알고리즘

Fuzzy 방식으로 사용을 하여 목표각을 계산하는 방식에서의 단점을 보안하고자 했다.

해당 방식은 Rule base로 작동하여 장애물 회피를 하기 위한 Local path plan에 해당한다. 장애물의 위치에 따라 선속과 회전각을 선택하여, 만약 보트 전방(탐색 범위) 내에 장애물이 존재하지 않을 경우 호핑투어와 마찬가지로 목표 지점을 추종한다.

빠르고 정확한 장애물 회피를 보였으나, LiDAR의 스캐닝 결과를 가공하지 않고 개별의 ‘점’으로 사용하는 것과 LiDAR 역시 노이즈 문제로 과한 회피각을 산출할 수도 있는 위험성이 있다.

알고리즘 설계

Fuzzy 방식의 문제점을 인지하고 위 블로그에서는 2가지의 알고리즘 방식을 설계 했다. 1. 점수 기반 목표각 산출 그리고 2. 선별 기반 목표각 산출이다. 목표각은 $ \psi_d + \psi + \pso_e $이므로 각 알고리즘은 동체 좌표계를 기준으로 받아들인 LiDAR 데이터 및 항법 좌표계를 기준으로 받아들인 선수각 등을 사용하여 $ \psi_e $를 계산한다.

• 점수 기반 목표각 산출

점수 기반 목표각 산출 방식은 이동 가능한 각도 모두에 일종의 점수를 부여하여 가장 좋은 점수의 각도를 목표각으로 삼는다. 점수 부여의 기준은 아래 세 가지이며, 각 기준별 계수를 곱해 점수로 할당한다. 최종적으로 $ \psi_e $는 각 기준에서의 점수를 모두 합산하여 최소 점수를 가지는 각도로 삼는다.

– 거리 위험도 : 장애물과 거리가 얼마나 가까운가? > 보트로부터 더 가까운 장애물을 우선적으로 회피해야 한다.
– 각도 위험도 : 현재 선수로부터 장애물이 얼마나 가까운가? > 회피 방향을 결정할 때는 선수를 최소한으로 회전하여 불필요한 이동을 최소화해야 한다.
– $\psi_e$ 목표 위험도 : 목표 지점까지 얼마나 선수를 회전해야 하는가? > 회피를 하는 동시에 목표점 역시 추종하도록 한다.

점수표의 역할을 하는 리스트는 아래와 같은 형태를 띈다.

현 시점 t에서 $ \psi_e $를 결정하는 의사코드는 아래와 같다.

점수를 저장하는 리스트 angle_risk를 모두 0으로 초기화함

for 모든 장애물에 대해
    if 장애물이 탐지 범위 바깥에 위치함 then
        continue
    endif
    
    장애물의 양쪽에 여유 각도를 더하여 시작/끝 각도를 계산함
    시작/끝 각도의 angle_risk 리스트에서 인덱스를 구함
    보트에서부터 장애물까지의 거리를 구함
    for 장애물의 시작 각도부터 끝 각도까지 모든 각도에 대해
        [각도 위험도]
        angle_risk의 각도 위험도 점수를 더함
        [거리 위험도]
        angle_risk의 거리 위험도 점수를 더함
    endfor
endfor

[목표 위험도]
if 최소 탐지 범위 <= 최종 목표 위치 =< 최대 탐지 범위 then
    for angle_risk의 모든 각도에 대하여
        해당 각도에서 목표점까지 회전해야 할 각도를 계산함
        회전해야할 각도의 범위에 따라 angle_risk의 목표 위험도 점수를 더함
    endfor
else if 최소 탐지 범위 > 최종 목표 위치 then
    0 ~ 최대 탐지 범위의 각도 모두(우현 방향)에 angle_risk의 목표 위험도 점수를 더함
else if 최대 탐지 범위 < 최종 목표 위치 then
    최소 탐지 범위 ~ 0 모두(좌현 방향)에 angle_risk의 목표 위험도 점수를 더함
endif

[최종 점수 집계]
angle_risk에서 가장 작은 점수를 가지는 각도로 설정함

angle_risk에서 특정 각도를 가지는 인덱스를 구하는 코드는 아래와 같다.

idx = int((angle - angle_min) / angle_increment)

angle_risk에서 특정 인덱스에서의 각도를 구하는 방식은 아래와 같다.

angle = idx * angle_increment + angle_min

• 선별 기반 목표각 산출 알고리즘

선별 기반 목표각 산출 방식은 다수의 후보 각도 중에서 이동 불가능한 각도를 제거한 뒤 최선의 각도들만 선택해나간다.

점수 기반 선별 방식은 더 많이 고려해야 할 사항을 계수 조정을 통해서 조절할 수 있다는 장점이 있지만 그만큼 하이퍼 파라미터의 개수가 많아 최적 파라미터값을 찾는데 오랜 시간이 걸리는 단점이 있다.

선별 기반 알고리즘은 이를 보완하기 위해 하였고, 점수 기반 알고리즘의 단순환 버전이라고 볼 수 있다.

두 방식의 차이점은 아래와 같이 정리할 수 있다.

• 선박으로부터 일정한 거리 이내, 즉 거리 범위 내측의 장애물은 모두 같은 취급을 한다. 점수 기반 방식에서 ‘거리 위험도’가 같은 값이거나 없는 셈이다.
• 범위 내 장애물이 위치한 각도는 모두 이동이 물가능한 각도로 본다. 이동이 불가능한 각도를 아예 제거함으로써 연산량을 줄일 수 있다. ‘각도 위험도’가 없는 셈이다.
• 점수 기반 방식의 ‘목표 위험도’처럼 특정 범위마다 가중치를 부여하지 않고, 단순히 목표와 가장 가까운 각 하나만 선택한다.

선별 기반 알고리즘의 작동 방식을 다시 정리하면 아래와 같다.

• 장애물이 위치한 각도 및 그 주변 일정 부분을 위험 구역이라 판단하며(a) $ \psi_e $의 후보군에서 제외함으로써 이동 가능한 영역만 남겨둔다.(b)
• 이동 가능한 영역 중에서는 최대한 최종 게이트 방향에 가까운 각도를 $ \psi_e $로 삼는다. 회피 중에서도 최종 게이트를 추종하도록 하기 위함이다.(c)
• 최종 게이트 방향까지 동일한 각도로 떨어져 있다만, 현재 선수각에서 최대한 덜 회전하는 방향을 $ \psi_e $로 삼는다. 불필요하거나 급격한 회전을 줄이기 위함이다.(d)

현 시점 t에서 $ \psi_e $를 결정하는 의사코드는 아래와 같다.

[장애물 회피]
for 모든 장애물에 대해
    장애물의 양쪽에 여유 각도를 더하여 시작/끝 각도를 계산함
    보트에서부터 장애물까지의 거리를 구함
    if 장애물이 탐지 각도 범위와 탐지 거리 범위 내부에 있음 then
        for 장애물의 시작 각도부터 끝 각도까지 모든 각도에 대하여
            장애물이 존재하는 각도 리스트인 danger_angles 리스트에 추가
        endfor
    endif
endfor
danger_angles에서 중복된 값을 제거함

[최적 에러각 계산]
현 선수각으로부터 목표점까지 회전해야 할 각도 ψ_g를 계산함
if 탐지 각도 범위 내 장애물이 없거나, 모두 장애물이 있음 then
    ψ_e = ψ_g
else
    for 탐지 각도 범위의 모든 각도에 대하여
        if danger_angles에 해당함 then
            continue
        endif
        ψ_e = ψ_g와 가장 가까운 각도, 혹은 0과 가장 가까운 각도로 한다.
    endfor
endif

🐢 현재 공부하고 있는 자율주행/자율운항을 학습하며 기록 및 정리를 하기위한 내용들입니다. 🐢

개발 과정

감사합니다.😊

Autonomous_Concept 카테고리 내 다른 글 보러가기