2024 IROS 문제 상황 모델링이 잘 된 논문인 것 같지만 이것도 약간 용두사미랄지...
Abstract
본 논문에서는 신경망을 Kinodynamic motion planning에 통합하는 Learning for KinoDynamic Tree Expansion(L4KDE) 방법을 제시한다. RRT와 같은 트리 기반 계획 접근법은 연속적인 상태 공간 모션 플래닝에서 전역 최적 경로를 찾는 주요한 방식이다. 이 접근법의 핵심은 새로운 노드가 지속적으로 확장되는 트리에 추가되는 Tree Expansion 과정이다.
우리는 tree-based planning에서의 Kinodynamic variant를 연구하였다. 기존 방법들은 새로 샘플링된 좌표에 연결할 노드를 빠르게 선택하는 과정에서 샘플링된 좌표로의 전이 비용이 낮은 노드를 최적화하지 못한다. 대신 Search tree에 연결할 후보 노드를 선택할 때 좌표 간 유클리디안 거리같은 휴리스틱을 사용하게 된다.
우리는 이 문제를 해결하기 위해 L4KDE를 제안한다. L4KDE는 신경망을 사용하여 쿼리된 상태 간의 전이 비용을 예측하며, 이는 Batch를 통해 효율적으로 계산될 수 있다. 우리는 다양한 도전적인 시스템 동역학에서 L4KDE가 제공하는 성능 향상을 실증적으로 보여줬으며, 동일한 모델 클래스의 다른 인스턴스에서도 일반화가 가능하고 최신 트리 기반 모션 플래너들과 함께 사용할 수 있음을 입증한다.
Kinodynamic Tree-based Motion Planning
A. Problem Setup
먼저 Kinodynamic motion planning 문제를 개괄적으로 살펴보자면, 상태 공간을 X, 제어 제약 조건 내의 제어 집합을 U, 운동학적 제약 조건이 만족되는 상태 집합을 K, 충돌이 없는 상태 집합을 O라고 할 수 있다.
정의 1 : 실행 가능한 Kinodynamic 플래닝 문제는 초기 상태 x0, 목표 상태 xgoal, 시스템 동역학 f가 주어졌을 때, 다음 조건을 만족하는 궤적 x(t): [0,tmax] → X와 제어 시퀀스 u(t): [0,tmax] → U를 생성하는 것이다 :
Optimal kinodynamic planning 문제는 식 (1)에서부터 (5) 까지 제약 조건 하에서 objective cost를 최소화하는 궤적을 찾는 것이다. 이러한 목적 함수는
이렇게 기술될 수 있고 여기서 L은 incremental cost이고 Φ는 terminal cost이다.
이 내용은 로봇 키노다이나믹 플래닝을 할 때 고려해야 할 제약 조건이 다음과 같음을 설명하고 있다.
물리적 한계 (운동학적 제약)
장애물 회피
로봇의 움직임 법칙 (동역학)
여기서 Incremental cost는 경로를 따라서 이동하면서 발생하는 비용 (자동차의 연료 소비량, 이동 시간 등)이고, Terminal cost는 최종 목적이 도달 시 발생하는 일회성 비용 (최종 목표 지점과의 거리 차이, 목표 자세와의 오차 등)을 말한다.
따라서 전체 비용은 "이동 중 발생하는 모든 비용의 합(L)" + "도착 시 발생하는 비용(Φ)"이다.
B. Tree Expansion
RRT*와 같은 트리 기반 모션 플래너는 무작위로 좌표를 샘플링하고 검증한 후 유효한 샘플과 연결하여 공간을 채우는 트리를 반복적으로 구축한다. 이는 일반적으로 장애물이 없는 상황에서 두 좌표 간의 최적 경로를 찾는 Steering function을 필요로 한다. (Kinodynamic 제약이 없는 경우 이는 단순히 직선을 찾는 것이다.) 일반적으로 트리 노드와 샘플링된 점 사이의 Optimal kinodynamic plan 문제를 풀지 않고는 최적 경로를 찾을 수 없다.
이를 해결하기 위해서 트리 기반 방법들에 Kinodynamic 변형이 도입되었다. 이 방법들은 유효하다고 판단된 샘플링된 좌표를 직접 연결하는 대신, 최적화를 단순화하고 기존 트리의 가장 가까운 노드에서 샘플에 '충분히 가까운' 실행 가능한 좌표로 키노다이나믹적으로 실현 가능한 방식으로 forward propagate를 시도한다.
L4KDE
A. Euclidean-based Heuristics : Fast but Flawed
Nearest 휴리스틱은 Kinematic 제약 하에서 샘플링된 점과 가장 가까운 기존 트리의 노드를 찾는 것을 목표로 한다. 이는 충돌 검사와는 분리되어 있다. 트리 확장 중 Nearest를 호출할 때, 기존의 키노다이나믹 트리 기반 Planning 방법들은 유클리드 거리를 사용하여 트리 T에서 가장 가까운 노드 좌표를 선택한다.
키노다이나믹 제약이 없는 경우에는 유클리드 거리를 최소화하여 노드를 선택하는 것이 최저 비용 경로를 보장하지만, 키노다이나믹 제약 하에는 그렇지 않은 경우가 있다. 예를 들어 그림 1a에서는 유클리드 거리는 가깝지만 자동차 동역학 하에서는 도달하기에 상대적으로 비용이 많이 드는 좌표들이 있다.
B. Transition Cost Prediction for Tree Expansion
전이 비용에 기반하여 가장 가까운 노드를 찾으려면 다음과 같은 constrained optimisation problem을 풀어야 한다.
일반적으로 기존 트리의 노드와 새로 샘플링된 노드 사이의 feasible한 궤적의 비용을 찾는 것은 온라인에서는 효율적으로 해결할 수 없는 문제이다. 대신 식 10의 장애물이 없는 trainsition cost 함수를 오프라인 학습을 통한 function approximator로 근사하고, 활성 노드를 선택하기 위해 온라인에서 근사 비용을 최소화할 것을 제안한다. .
중요한 점은, cost function이 environment-specific collision-avoidance constraint가 아닌 키노다이나믹 제약만을 고려하므로 훈련된 모델은 시스템 동역학에 특화되어 있다는 점이다. 우리는 Function approximator로 신경망 gθ를 사용한다.
따라서 문제는 다음과 같이 모델링 될 수 있다.
C. Training the Function Approximator
Kinematic constraints는 특정 환경에 종속되어 있지 않으므로, 우리는 transifion cost를 근사하기 위한 신경망을 오프라인으로 훈련할 수 있다. 우리는 n개의 데이터 예제 D = {(x_i_start, x_i_goal), c_i}_n_i=1를 수집하였다. 여기서 c는 시작점 x_start에서 목표점 x_goal까지의 비용이다.
L(θ) = ||c − gθ(x_start ⊕ x_goal)||2
여기서 x ⊕ x는 시작과 목표 좌표의 벡터 연결을 나타낸다.
Experimental Results
우리는 여러가지 동역학 모델을 사용하여 L4KDE의 트리 확장을 광범위하게 검증하였다. 검토된 모델 동역학은 다음과 같다.
두빈스 자동차(Dubins car) 동역학: - 상태 x = [x, y, θ] - 제어 u = [us, uφ] - 방정식: x ̇ = us cos θ, y ̇ = us sin θ, θ ̇ = us/Laxles tan uφ - Laxles는 전후 차축 간 거리
이중 진자(Double Pendulum) 동역학:
- 상태 x = [θ1, ω1, θ2, ω2] - 제어 u = [uα1, uα2] - 방정식: θ ̇i = ωi, ω ̇i = uαi/(mL2i) - g sin θi/Li - Li는 i번째 팔의 길이
구스넥 트레일러(Gooseneck Trailer) 동역학:
- 상태 x = [x, y, θ1, θ2] - 제어 u = [us, uφ] - 방정식: x ̇ = us cos θ, y ̇ = us sin θ, θ ̇ = u/L tan u, θ ̇ = u/L sin(θ-θ) - L1, L2는 차축 거리
L4KDE 트리 확장 방법은 다음과 같은 널리 사용되는 트리 기반 키노다이나믹 모션 플래너들과 함께 사용될 수 있다. RRT - Anytime RRT - SST - SST* - AO-RRT
신경망의 구조는 다음과 같다. - 5 Fully connected layers (input : state space) - 훈련 중 목표 비용을 시그모이드 층의 출력 범위에 맞게 조정
평가 지표 : 1. 해결 시간: 실현 가능한 해결책을 찾는 데 걸리는 시간 (초) 2. 성공 비율 : 실현 가능한 해결책을 찾은 실행의 비율
