Where we should go?
Draw a route and navigate along
地図が描け，その中のどこにいるかわかったら，次はどこへ行けるか，行くべきかを見つけ（地形評価とプランニング），さらにそのプランに沿って走行制御（コントロール）しなければなりません．これには幾つかのバリエーションやステップがあります．例えば，3次元地形を対象にする場合は，単なる平地と障害物とに切り分けることはできず，その場その場の傾斜角，ロボットがそこにいられるだけのスペースがあるかどうか，また，平地であったとしてもそこにたどり着けるだけのルートが存在するかどうかなどを多角的に評価しなければなりません．
Fig.1 Terrain Modeling, Evaluation and Planning
Fig. 2 Local Planning and Control with Dynamic Window Algorithm
Fig. 1は，3次元地形をステレオビジョンによりモデリングし，グリッドセル毎の高さ，傾斜，隣接するセルとの段差をそれぞれ評価してパスプランニングを行っています． Fig. 2は，もっと簡単に，2次元のモデリング環境の中でダイナミックウィンドウ・アプローチにより走行すべき方向を決定しています．
Planning & Control in Dynamic Space
環境に不特定多数の移動障害物がある場合は，プランニングは容易ではありません．静的な物体と，移動体とを切り分けた上で，移動体の速度ベクトルを求め予測をした上でプランニングをする必要があります．
Fig. 3 Planning and Control on the Field with Moving Obstacles
上の動画は，複数の移動障害物が存在する空間で，その動きを予測してそれぞれの時空間における存在可能性をプロットすることによりプランニングを行っています．プランニング結果には当然，予測が入るので，予測時間が長いと現実との誤差が発生します．ロボットがゴール近傍（左端）で一旦停止しているのは，予測とは異なった場所に移動障害物を発見したからで，その場合，緊急停止して安全を確保し，自動的にリプランニングしてからゴールまで誘導制御しています．
