LQG-MP: Optimized Path Planning for Robots with Motion Uncertainty and Imperfect State Information

Author:Jur van den Berg, Pieter Abbeal, Ken Goldberg
Year:2010
Link:link
Accepted:The International Journal of Robotics Research

Abstract

この論文で提案されるLQG-MPは経路追従に用いられるセンサやコントローラーの不確実性を考慮に入れた動作計画器であり，具体的には与えられた経路に対するロボットの事前分布を求めることができるので，それに基づいて衝突確率を評価することなどが可能になるとのこと．不確実性はガウス分布で，そしてそれに基づいた線形二次レギュレータに基づいている，つまりカルマンフィルタとLQRね．ロボット系の人って結構軽くカルマンフィルタを使ってしまっているイメージがある．時間領域でアルゴリズムを論じているから仕方ないのかもしれない．

最適な経路の候補となる経路を生成する手法はたくさんあるが，この論文ではRRTを用いる．この論文で提案されている手法はカルマンスムーザーの適用により得られた経路を \(C_{k}\) 級にすることができる，その例としてダイクストラ法に類似した手法により得られたロードマップに対してそれを適用することで滑らかな(high-quality)経路を得ることができる．

Introduction

ロボットの不確実性は

  • モデル化されていない外乱
  • センサの雑音によるimperfect state information

に起因する．これから自分が論文を書くために書いておくと，アプリケーションは

  • guiding mobile robot over uneven terrain
  • robotic surgery with high-degree-of freedom (6-DOF)

などがある．

ロボットの不確実性はロボットがとる行動やセンシングに依存する．そして各経路に対してその不確実性は異なるであろう．そこで予めその不確実性を考慮できるようにすることでロボットにとって最良の経路が選ばれるようにしたい．

多くの伝統的な手法は決定論的に環境に関する知識を保有していると仮定している．そして環境の不確実性は実際にその経路を追従する(execute)する制御の段階になって初めてフィードバックの形で考慮されている．

計画と制御は相互に関係しているものの，異なる領域である．

Planning and control are realted but distinct fields.

近年のmotion planningに関する研究の中には不確実性を考慮に入れているものが存在しているが，そのほとんどは経路を計画する際に制御を考慮に入れていない．そしてほとんどの制御手法は経路を与えられたものとしている．LQG-MPはそれらの間の架け橋となる．

LQG-MP builds a bridge between these desciplines and draws from results in both．

LQG-MPの鍵となるポイントは，経路の追従の際に用いられるコントローラーとセンサーの不確実性の事前分布は，経路生成の際の最適化に利用できるということである．

本論文では与えられた確率的な(雑音を含む)システムのダイナミクスとセンサの確率的な(雑音を含む)観測モデルを用いて，(大雑把な)経路が与えられた際に，実際にその経路を追従する(execute the path)際のロボットの状態と制御入力の不確実性を見積もることができる．それにより例えば，ロボットが障害物と衝突する確率やゴールに到着する確率，その他経路に関する定量的な評価を行うことができる．

本論文では任意の次元のホロノミック・非ホロノミックなキネマティクス・ダイナミクスの系に対して適用できる．本手法を適用するには，ロボットの確率的ダイナミクスモデル・確率的観測モデルが明示的に与えられ，その雑音がガウスモデルに従う必要がある．本手法は線形な系に対して設計されているが，線形化により局所的に十分近似されるのであれば，非線形な系に対しても適用できる．

本論文では候補となる経路を生成するのにRRTが用いられている．経路の良し悪しを評価する際に経路長や障害物からのmaximum clearanceの基準が同じでもダイナミクス・観測モデルの不確実性が異なると得られる経路のqualityは著しく異なることが示される．

一般にRRTにより得られる経路は滑らかではない．標準的なスムージングのアルゴリズムは経路をshort-cutしたりdynamically unfeasibleな結果を招くことがある(splineの使用などにより)．本論文ではカルマンスムーザーを用いることにより経路を \(C_{k}\) 級の滑らかさにすることが可能であることを示し，提案手法のLQG-MPと併用することでその効果を実証する．

一方提案手法は直接にはロードマップベースの経路計画手法には向いていない．そこでinformation spaceにおけるplanningを取り扱った先行研究において用いられている近似手法を用いることで，予め求められたロードマップに対してもLQG-MPを適用することができる．

Problem Definition

ここからはありがちな文章が続くが，将来自分も書くときのためにここで簡単に書いておこう．まずロボットの状態変数が属する空間を \(\mathcal{X}\) ，そして制御入力の属する空間を \(\mathcal{U}\) とする．時間軸は一定の刻み幅で離散化されており \(u_t\) は \(x_t\) を \(x_{t+1}\) にbringするとする．ここでダイナミクスも不確実性があるので

\begin{align*} x_t = f(x_{t-1}, u_{t-1}, m_t), \quad m_t \sim \mathcal{N}(0, M_t) \end{align*}

と推移する．もちろんガウス雑音である．

注釈

ああ，確率ロボティクスは偉大なあ．

ロボットの出発点となる \(x_{start}\) ，ゴール領域となる \(\mathcal{X}_{goal}\) が与えられるので，求める経路とは \(x_{0}^{*}, u_{0}^{*}, \cdots, x_{l}^{*}, u_{l}^{*}\) なる系列であり，もちろん \(x_{0}^{*} = x_{start}, x_{l}^{*} \in \mathcal{X}_{goal}, x_{t}^{*} = f(x_{t-1}^{*}, u_{t-1}^{*}, 0)\) に従う．つまり雑音がなければ経路は完全にダイナミクスにconsistentである．

execution of the path の間にロボットはノミナルの経路からずれるので，これを補償するために以下のようなコスト関数を最小化するようにフィードバック制御により経路がexecuteされるとする．

\begin{align*} \mathbb{E} \sum_{t=0}^{l}(x_t - x_{t}^{*})^{\text{T}}C(x_t - x_{t}^{*}) + (u_t - u_{t}^{*})^{\text{T}}D(u_t - u_{t}^{*}) \end{align*}

もちろんC，Dは正定値行列である．

そして経路のexecutionの間，センサーによりロボットの状態に関して以下のように部分的な情報を得られるとしよう．

\begin{align*} z_t = h(x_t, n_t), \quad n_t \sim \mathcal{N}(0, N_t) \end{align*}

問題提起を2つ行う．

  1. stochastic dynamics model と stochastic observation model, cost function が与えられたときに，特定の経路についてそれに沿ったロボットの状態と制御入力の事前分布を評価する．
  2. 候補の中から最良の経路を選択する．

A-priori Probability Distributions

ここでは与えられた(候補の)経路について，それに沿った状態変数と入力変数の事前分布を求める方法について述べる．一応，経路をexecuteする際にどのような制御器を用いるかは予め決まっているとする．線形なダイナミクスとガウス雑音を受ける観測モデルに対しては，LQR-feedbackをKalman-filterと並行して用いる，つまりLQG制御に基づくexecutionがoptimal approachである．

注釈

この人たちって，可制御性とか可観測性についてはどれくらい知っているのだろうか？ 少し軽率さを感じてしまう．まあ間違いではないのだけれど．

Linear(ized) Dynamics and Observation Model

ロボットは経路のexecutionの間参照経路にstay closeするので，ロボットの状態は \(x_{t}^{*}, u_{t}^{*}\) 周辺で線形近似することが可能である．よって以下のようにテイラー展開できる．

\begin{align*} x_t &= f(x_{t-1}^{*}, u_{t-1}^{*}, 0) + A_t(x_{t-1} - x_{t-1}^{*}) \\ &+ B_t(u_{t-1} - u_{t-1}^{*}) + V_t m_t \\ z_t &= h(x_{t}^{*}, 0) + H_t(x_t - x_{t}^{*}) + W_t n_t \end{align*}

ここで各行列は近似された動作点において評価されたヤコビ行列である．

\begin{align*} A_t &= \dfrac{\partial f}{\partial x}(x_{t}^{*}, u_{t-1}^{*}, 0) \\ B_t &= \dfrac{\partial f}{\partial u}(x_{t}^{*}, u_{t-1}^{*}, 0) \\ V_t &= \dfrac{\partial h}{\partial m}(x_{t}^{*}, u_{t-1}^{*}, 0) \\ H_t &= \dfrac{\partial h}{\partial x}(x_{t}^{*}, 0) \\ W_t &= \dfrac{\partial h}{\partial n}(x_{t}^{*}, 0) \end{align*}

ここで各々を動作点からのdeviationで表現し直すと，つまり \(\bar{x}_t = x_t - x_{t}^{*}\) ， \(\bar{u}_t = u_t - u_{t}^{*}\) ， \(\bar{z}_t = z_t - h(x_{t}^{*}, 0)\) ととれば

\begin{align*} \bar{x}_t &= A_{t-1} \bar{x}_t + B_t \bar{u}_{t-1} + V_t m_t \\ \bar{z}_t &= H_t \bar{x}_t + W_t n_t \end{align*}

そしてコスト関数は

\begin{align*} \mathbb{E}( \sum_{t=0}^{l} \bar{x}_t^{\text{T}}C\bar{x}_t + \bar{u}_t^{\text{T}}D\bar{u}_t) \end{align*}

Kalman Filter for Optimal State Estimation

ここも復習がてらきちんと書いておこう．カルマンフィルタは \(\bar{x}_t\) の真値の推定値とその誤差共分散 \(P_t\) を追跡する．そのアルゴリズムは2段階からなる．

  1. applyした制御入力 \(\bar{u}_t\) をpropagateするダイナミクスのupdate
  2. 得られた測定値 \(\bar{z}_t\) をincorporateする測定のupdate