初めに

2017/06/23
ソフトバンクロボティクス主催
【特別開催】パリ・日本の専門家から直接SLAMを学ぼう！移動、演出、モーションまで
に参加したので、その内容をまとめた。
https://pepper.doorkeeper.jp/events/61547
また、筆者が以前ハッカソンにてSLAMアプリを作成した知見についても追加している。
Naoqi OS 2.2.5で提供されるSLAM機能は、β版であり、正式版では仕様が変更される可能性があるためご注意ください。

概要

第1部　SLAM技術トレーニング
講師：ソフトバンクロボティクスヨーロッパのエンジニア
内容：SLAM技術仕様・デモ
第2部　SLAM演出ワークショップ
　　　 講師:株式会社ヘッドウォータース　渡部知香氏
　　　 内容：Pepperアプリにおける演出の重要性
　　　 　　　モーション作成方法
　　　 　　　

第1部 SLAM技術トレーニング

SLAMとは

Simultaneous Localization and Mappingの略
自己位置推定と環境地図作成を同時に実行

簡単に言うと適当にPepperが動いて地図を作成してくれる。地図上で、今自分がどこにいるかも把握する。
※Pepperは同時に実行せず、別々に実行する。そのため厳密な意味での
SLAMではない

SLAMと既存自己位置推定技術の比較

SLAMが確立される以前は、ロボットに搭載されているジャイロや
ホイールといった内界センサでの自己位置推定が一般的であった

※内界センサとは、ロボット自身の状態を知るためのセンサ
　例：加速度センサ、温度センサ等
外界センサとは、ロボット外の状態を知るためのセンサ
例：タッチセンサ、超音波センサ、カメラ等

内界センサを用いた地図作成&自己位置推定は、以下の要因により大きく実環境とずれることがあった。
・地面が平面ではない
・強い磁場の発生

特にPepperの場合は、ホイールオドメトリという足についている車輪の回転から、地図作成＆自己位置推定は可能であるのだが、精度があまり良くないためあまり実用的ではなかった。
（その場で１回転させただけでも、Pepper上では±20°の誤差が生じる）

PepperにおけるSLAM活用事例

Pepper world
2017年2月に開催したPepper worldにて、Pepperの歴史を移動しながら紹介を行う展示を実施
2日間10時間連続稼働を行ったが、１度も機体交換及び故障が発生しなかった。

SLAM実装方法

ALNavigation APIの以下メソッドを用いて実現する。
Pepper(Naoqi OS 2.5.5で提供されるβ版)は、上述の通り自己位置推定と環境地図作成を同時に実行できないため、別々に実行する。
今回は、こちらのサンプルアプリを参考に講義を進めた。
https://github.com/aldebaran/naoqi_navigation_samples
ドキュメントは、下記
http://doc.aldebaran.com/2-5/naoqi/motion/exploration-api.html

地図作成

explore(float)
Pepeprが、周辺を適当に動き地図を作成する。
引数の指定した数値を半径として
Pepperが円状に探索を始めるので要注意

saveExploration()
exploreで作成した地図を、Pepperのローカルへと保存する。
返り値として、保存した地図のフルパスが返る。
この際　~.exploという特殊な拡張子で保存される。

ここでのフルパスは、忘れると探すのが困難なため必ず覚えておくこと

自己位置推定

startLocalization()
自己位置推定を開始する。
自己位置推定に限らず、MAPを読み込み移動する場合は、必ず実行すること

navigateToInMap(x,y,θ)
作成したMAPに対し、指定した位置へと移動する。
(0,0,0)と指定すると探索開始位置へと移動する。
Naoqi OS 2.5.5では引数3つ目のθは非対応

stopLocalization()
自己位置推定の終了

stopLocalization()をせずに
次のstartLocalization()を実行するとエラーとなるので要注意

以上が、MAP作成及び自己位置推定の一連の流れとなる。
簡潔にまとめると下記のようになる。
１、exploreで、地図作成
２、saveExplorationで保存
３、startLocalizationで自己位置推定開始
　　→今自分が地図上のどこにいるのか
４、navigateToInMapで移動
５、stopLocalizationで自己位置推定終了

ここまでは、地図作成し、そのまま所定の位置へを動かす手順となる。
しかし、実際はそのような手順を行っていては、exploreの時点でPepperがどこに行くかわからないため現実的ではないだろう。
ここからは、実際の場面を想定し、地図作成と移動を分けたプロセスを紹介する。

地図読み込み&移動

loadExploration(name)
地図作成で作成した、地図の読み込みを行う。
saveExploration()で取得したフルパスを引数に指定すること。

relocalizeInMap(x,y,θ)
自己位置推定開始時に、今Pepperがいる地点を教える。
必須ではないが、これをやらないとずれる可能性があるため
なるべく自己位置推定開始時は、Pepperを地図作成の初期地点へと移動し、relocalizeInMap(0,0,0)と指定すると良い。

以降の動きは、上記と同様の流れ
１、startLocalizationで自己位置推定開始
　　→今自分が地図上のどこにいるのか
２、navigateToInMapで移動
３、stopLocalizationで自己位置推定終了

その他のメソッド

getRobotPositionInMap()
現在のMAPに対する自己位置(x,y)を返却
デバッグの際に必須となる

SLAM利用の注意

上記の実装で、すぐにSLAMが動くというわけではない
やはりPepperもロボットということで、幾つか注意点があるので
読んでいただきたい。

地図作成時の注意

Naoqi OS 2.5.5で利用されているSLAMでは、Pepperの足元のレーザーセンサーを用いて地図作成を行っている。
そのため、机のような障害物は検知できない。
なので、ぶつからないようにするためにあらかじめ高さが30cm程度のブックエンドのような障害物を配置する必要がある。

初期地点に戻す

作成地図を読み込み、移動させたい地点にPepperを移動
そして初期地点に戻る

というのが、これを読んでいる皆さんも想定するPepperの動きだろう。
しかしながら、初期地点にうまく戻れない場合がある。
その改善として以下記載するので試してみてほしい。

navToCurrent = almath.Pose2D(navigation.getRobotPositionInMap()[0])
navToReference = almath.Pose2D(0,0,0)
currentToReference = mavToCurrent.inverse() * navToReference
motion.moveTo(currenttoReference.x,currentToReference.y,currenttoReference.theta)