[논문 리뷰] Batch and Incremental Kinodynamic Motion Planning using Dynamic Factor Graphs

Mandy Xie, Frank Dellaert|arXiv (Cornell University)|2020. 05. 26.

Robotic Path Planning Algorithms참고 문헌 42인용 수 4

한 줄 요약

이 논문은 중력, 관성, 토크 한계를 포함한 전체 로봇 역학을 최적화하여 에너지 효율적인 로봇 궤적을 생성하기 위해 동적 인자 그래프(Dynamic Factor Graphs, DFG)를 사용하는 배치 및 인크리멘탈 운동 계획 기법을 제안한다. GTSAM을 통한 희소 비선형 최적화 및 인크리멘탈 베이즈 트리 솔버를 활용함으로써, 기존의 배치 방법 대비 최대 20배 빠른 상태의 최적화 속도를 달성하고, 최소한의 재계산으로 효율적인 재계획이 가능하다.

ABSTRACT

This paper presents a kinodynamic motion planner that is able to produce energy efficient motions by taking the full robot dynamics into account, and making use of gravity, inertia, and momentum to reduce the effort. Given a specific goal state for the robot, we use factor graphs and numerical optimization to solve for an optimal trajectory, which meets not only the requirements of collision avoidance, but also all kinematic and dynamic constraints, such as velocity, acceleration and torque limits. By exploiting the sparsity in factor graphs, we can solve a kinodynamic motion planning problem efficiently, on par with existing optimal control methods, and use incremental elimination techniques to achieve an order of magnitude faster replanning.

연구 동기 및 목표

• 관성, 중력, 토크 한계를 포함한 전체 로봇 역학을 통합하여 에너지 효율적인 로봇 운동을 생성하는 데 도전한다.
• 복잡한 동적 제약 조건과 고차원 상태 공간을 다루는 데 어려움을 겪는 샘플링 기반 방법의 한계를 극복한다.
• 희소성과 인크리멘탈 최적화 기법을 활용하여, 동적 환경에서의 신속한 인크리멘탈 재계획을 가능하게 한다.
• 인자 그래프의 표현력과 희소 비선형 솔버의 효율성을 결합함으로써 기존 최적 제어 방법을 초월한다.
• 배치 최적화 방법과 동등한 성능를 달성하면서도, 인크리멘탈 업데이트를 통해 재계획 속도를 수십 배 빠르게 한다.

제안 방법

• 로봇 역학을 인자 모델에 포함시켜 동적 인자 그래프(DFG)를 사용해 최적 제어 문제로 운동 계획을 공식화한다.
• 궤적 최적화 목표 및 동적 제약 조건(예: 속도, 가속도, 토크 한계)을 인자 그래프의 인자로 표현하고, 구성 및 속도 상태를 변수로 사용한다.
• 자동 미분를 지원하는 GTSAM 라이브러리를 사용해 희소 비선형 최적화를 수행함으로써 효율적인 계산과 기울기 기반 수렴을 가능하게 한다.
• 기존 계산을 재사용할 수 있는 인크리멘탈 베이즈 트리 솔버를 통해 인크리멘탈 재계획을 구현한다.
• 재계획을 위한 두 가지 초기화 전략을 적용한다: 직선 가속도를 부드럽게 만든 경로(DFGP-L)와 이전 해를 온난 스타트로 사용하는 방법(DFGP-W).
• 에너지 효율성을 향상시키기 위해 목적 함수에 최소 토크 제약 조건을 통합한다. 특히 무거운 하중을 지닌 작업이나 동적 이동이 필요한 작업에서 효과적이다.

실험 결과

연구 질문

• RQ1 전체 로봇 역학을 고려한 운동 계획 문제를 효과적으로 모델링하고 해결하는 데 동적 인자 그래프를 활용할 수 있는가?
• RQ2 기존 최적 제어 및 샘플링 기반 접근법과 비교할 때, 제안된 방법의 계획 효율성과 해 품질은 어떠한가?
• RQ3 인크리멘탈 최적화 기법을 통해 동적 환경에서의 재계획 시간을 얼마나 줄일 수 있으며, 해 품질에 영향을 주지 않으면서 가능한가?
• RQ4 동적 제약 조건과 기하 제약 조건을 만족시키면서도 토크 사용을 최소화함으로써 에너지 효율적인 궤적을 얼마나 효과적으로 생성할 수 있는가?
• RQ5 다양한 초기화 전략이 인크리멘탈 재계획의 성공률과 수렴 속도에 어떤 영향을 미치는가?

주요 결과

• 제안된 DFGP 알고리즘은 최신 기술 수준의 배치 최적 제어 방법과 동등한 계획 성능를 보이며, 평균 계산 시간은 작업 1에서 1.98초, 작업 2에서 1.68초를 기록한다.
• 인크리멘탈 iDFGP 알고리즘은 평균 재계획 시간을 작업 1에서 0.119초, 작업 2에서 0.074초로 줄여, 온난 스타트 초기화 시 기존 배치 방법 대비 최대 20배 빠른 속도를 달성한다.
• iDFGP는 높은 성공률(작업 1: 80%, 작업 2: 96%)을 유지하며, DFGP-W(작업 2: 98%)와 비교해 미미한 감소만을 보이며, 목표 변경에 대한 강건성을 입증한다.
• 최소 토크 제약 조건을 통합함으로써 iDFGP의 평균 토크가 1489.03 N·m로 크게 감소하여 에너지 효율성이 향상됨을 확인한다.
• 목표가 변경되었을 때, 인크리멘탈 솔버는 영향을 받는 궤적 부분만 업데이트하며, 그림 8(f)–(j) 및 9(k)–(o)에서 원래 경로의 구조를 가능한 한 유지하는 것을 확인할 수 있다.
• 최적 궤적 형상화를 통해 관절 토크를 최소화함으로써, 동적 제약 조건과 에너지 효율성이 중요한 복잡한 작업(예: 무거운 블록 들어 올리기, 밀기)에서 뛰어난 성능를 발휘한다.