ホーム > 特許ランキング > 東京ロボティクス株式会社
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024057369
(43)【公開日】2024-04-24
(54)【発明の名称】移動型ロボット、システム、方法及びプログラム
(51)【国際特許分類】
B25J 9/10 20060101AFI20240417BHJP
【ＦＩ】
B25J9/10 A
【審査請求】未請求
【請求項の数】15
【出願形態】ＯＬ
(21)【出願番号】P 2022164066
(22)【出願日】2022-10-12
(71)【出願人】
【識別番号】315014671
【氏名又は名称】東京ロボティクス株式会社
(74)【代理人】
【識別番号】100098899
【弁理士】
【氏名又は名称】飯塚 信市
(74)【代理人】
【識別番号】100163865
【弁理士】
【氏名又は名称】飯塚 健
(72)【発明者】
【氏名】加藤 健太
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707BS10
3C707BS13
3C707BS14
3C707CS08
3C707KS12
3C707KS16
3C707KS20
3C707LT06
3C707LV19
3C707MS09
3C707MS10
3C707WA16
(57)【要約】 （修正有）
【課題】逆運動学を用いた制御を採用しつつも、安全に運用可能なロボットを提供する。
【解決手段】目標領域情報を取得する目標領域情報取得部と、前記目標領域に対応する前記参照領域を取得する参照領域情報取得部と、前記参照領域に対応する前記参照停止情報に基づいて前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する目標停止情報生成部と、前記移動型ロボットの移動制御を行う、移動制御部と、前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する目標制御点情報取得部と、前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する参照関節角度ベクトル情報取得部と、逆運動学を解き目標関節角度ベクトル情報を生成する目標関節角度ベクトル情報生成部と、前記移動型ロボットを制御する関節制御部と、を備える移動型ロボットが提供される。
【選択図】図９
【解決手段】目標領域情報を取得する目標領域情報取得部と、前記目標領域に対応する前記参照領域を取得する参照領域情報取得部と、前記参照領域に対応する前記参照停止情報に基づいて前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する目標停止情報生成部と、前記移動型ロボットの移動制御を行う、移動制御部と、前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する目標制御点情報取得部と、前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する参照関節角度ベクトル情報取得部と、逆運動学を解き目標関節角度ベクトル情報を生成する目標関節角度ベクトル情報生成部と、前記移動型ロボットを制御する関節制御部と、を備える移動型ロボットが提供される。
【選択図】図９
【特許請求の範囲】
【請求項1】
多リンク機構と、制御部と、記憶部と、を備えた、移動型ロボットであって、
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部は、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得部と、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成部と、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御部と、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得部と、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成部と、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御部と、を備える、移動型ロボット。
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部は、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得部と、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成部と、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御部と、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得部と、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成部と、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御部と、を備える、移動型ロボット。
【請求項2】
目標関節角度ベクトル情報生成部は、前記参照関節角度ベクトルを初期値として、前記制御点の前記目標位置及び姿勢までの逆運動学を解くものである、請求項１に記載の移動型ロボット。
【請求項3】
目標関節角度ベクトル情報生成部は、前記参照関節角度ベクトルと前記移動型ロボットの現在関節角度との差分を最小化する優先度付き逆運動学を解くものである、請求項１に記載の移動型ロボット。
【請求項4】
前記参照制御点は１つの前記参照領域に対して複数設定され、
前記目標関節角度ベクトル情報生成部は、
前記目標制御点情報に係る前記制御点の目標位置及び／又は姿勢に最も近い前記参照制御点に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、請求項１に記載の移動型ロボット。
前記目標関節角度ベクトル情報生成部は、
前記目標制御点情報に係る前記制御点の目標位置及び／又は姿勢に最も近い前記参照制御点に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、請求項１に記載の移動型ロボット。
【請求項5】
前記目標関節角度ベクトル情報生成部は、さらに、
前記参照関節角度ベクトルに一致するよう前記移動型ロボットの関節を制御する、第１関節制御部と、
前記第１関節制御部の動作の後に、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、第２関節制御部と、を備える、請求項１に記載の移動型ロボット。
前記参照関節角度ベクトルに一致するよう前記移動型ロボットの関節を制御する、第１関節制御部と、
前記第１関節制御部の動作の後に、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、第２関節制御部と、を備える、請求項１に記載の移動型ロボット。
【請求項6】
前記移動型ロボットは、さらに、
前記移動型ロボットの外部環境を認識する、センサと、
前記センサの出力に基づいて、前記移動制御後の前記移動型ロボットの位置又は姿勢を前記目標停止情報と一致するよう補正する、補正制御部と、を備える、請求項１に記載の移動型ロボット。
前記移動型ロボットの外部環境を認識する、センサと、
前記センサの出力に基づいて、前記移動制御後の前記移動型ロボットの位置又は姿勢を前記目標停止情報と一致するよう補正する、補正制御部と、を備える、請求項１に記載の移動型ロボット。
【請求項7】
前記参照領域は、その重心位置が鉛直方向に異なるように前記空間内に複数配置される、請求項１に記載の移動型ロボット。
【請求項8】
鉛直方向に重心位置が異なるよう配置された複数の前記参照領域は、その鉛直方向の位置に応じて鉛直方向の厚みが異なるよう設定されている、請求項７に記載の移動型ロボット。
【請求項9】
前記参照領域の鉛直方向の厚みは、鉛直上方又は下方に近付く程、小さくなる、請求項８に記載の移動型ロボット。
【請求項10】
前記参照領域は、３次元領域である、請求項１に記載の移動型ロボット。
【請求項11】
前記参照領域は、２次元領域である、請求項１に記載の移動型ロボット。
【請求項12】
前記移動型ロボットの関節角度ベクトルが所定条件を満たす場合に前記関節制御部における制御を無効化する、制御無効化部をさらに備える、請求項１に記載の移動型ロボット。
【請求項13】
多リンク機構を備えた移動型ロボットと、記憶部と、制御部と、を含むシステムであって、
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部は、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得部と、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成部と、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御部と、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得部と、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成部と、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御部と、を備える、システム。
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部は、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得部と、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成部と、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御部と、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得部と、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得部と、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成部と、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御部と、を備える、システム。
【請求項14】
多リンク機構と、制御部と、記憶部と、を備えた、移動型ロボットの制御方法であって、
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部において、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得ステップと、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成ステップと、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御ステップと、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得ステップと、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成ステップと、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御ステップと、を実行する、制御方法。
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部において、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得ステップと、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成ステップと、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御ステップと、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得ステップと、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成ステップと、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御ステップと、を実行する、制御方法。
【請求項15】
多リンク機構と、制御部と、記憶部と、を備えた、移動型ロボットの制御プログラムであって、
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部において、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得ステップと、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成ステップと、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御ステップと、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得ステップと、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成ステップと、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御ステップと、を実行する、制御プログラム。
前記記憶部は、
所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、
前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、
前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、
前記制御部において、
目標領域に関する情報である目標領域情報を取得する、目標領域情報取得ステップと、
前記目標領域に対応する前記参照領域を取得する、参照領域情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成ステップと、
目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御ステップと、
前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得ステップと、
前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得ステップと、
前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成ステップと、
前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御ステップと、を実行する、制御プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、ロボット、特に、モバイルマニピュレータ等に関する。
【背景技術】
【0002】
近年、ロボットアーム等の多リンク機構で成るマニピュレータを備えた移動型のロボット（以下、モバイルマニピュレータ）の現場への導入が進められている。例えば、モバイルマニピュレータには、ピッキング作業や搬送作業等を担うことが期待されている。
【0003】
この種の多リンク機構を備えたロボットの動作制御にあたって、逆運動学が用いられることがある。逆運動学とは、ロボットに設定された所望の制御点を目標位置又は姿勢に到達させるための動きを方程式等により決定するものである。例えば、ロボットアームの手先等の位置を制御点として、当該制御点を目標位置又は姿勢に到達させるための各関節の角度等を算出し、それを目標に各関節を制御すること等が行われる（例えば、特許文献１）。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開２０１７－４７５１８号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
ところで、多リンク機構等の自由度の大きいロボットの場合、制御点を所望の位置・姿勢とするための逆運動学の解は複数存在し得り、すなわち、ロボットは様々な姿勢を採り得る。しかしながら、制御点の最終的な位置・姿勢は同一であっても、一部の解に相当する姿勢を採用すべきでない場合がある。例えば、周囲の環境と衝突してしまうおそれのある姿勢や、所謂特異点等の不安定性をもたらす姿勢に近い姿勢等は好ましくない。従前の構成ではそのような姿勢を採ってしまうおそれがあった。
【0006】
例えば、特許文献１の構成にあっては、ピッキング可能位置を定義することにより、ピッキング動作にあたって逆運動学の解が存在することは保証されていても、いずれの解を採用すべきかに関する配慮はなされていない。従って、同構成にあっては、上述の好ましくない姿勢を採ってしまうおそれがある。
【0007】
本発明は上述の技術的背景に鑑みてなされたものであり、その目的は、逆運動学を用いた制御を採用しつつも、安全に運用可能なロボットを提供することにある。
【課題を解決するための手段】
【0008】
上述の技術的課題は、以下の構成を有する移動型ロボット等により解決することができる。
【0009】
すなわち、本発明に係る移動型ロボットは、多リンク機構と、制御部と、記憶部と、を備えた、移動型ロボットであって、前記記憶部は、所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、前記制御部は、目標領域に関する情報である目標領域情報を取得する、目標領域情報取得部と、前記目標領域に対応する前記参照領域を取得する、参照領域情報取得部と、前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成部と、目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御部と、前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得部と、前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得部と、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成部と、前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御部と、を備えている。
【0010】
このような構成によれば、逆運動学を用いて目標関節角度ベクトル情報を生成する際に、参照領域毎に予め設定された参照関節角度ベクトル情報を用いるので、逆運動学を用いて関節制御を行っても移動型ロボットが想定外の動作を行うおそれが低減される。すなわち、逆運動学を用いた制御を採用しつつも、安全に運用可能なロボットを提供することができる。なお、移動型ロボットとは、移動可能に構成されたロボットであり、例えば、モバイルマニピュレータである。
【0011】
目標関節角度ベクトル情報生成部は、前記参照関節角度ベクトルを初期値として、前記制御点の前記目標位置及び姿勢までの逆運動学を解くものであってもよい。
【0012】
このような構成によれば、逆運動学を用いて関節制御を行っても移動型ロボットが想定外の動作を行うおそれが低減される。
【0013】
目標関節角度ベクトル情報生成部は、前記参照関節角度ベクトルと前記移動型ロボットの現在関節角度との差分を最小化する優先度付き逆運動学を解くものであってもよい。
【0014】
このような構成によれば、逆運動学を用いて関節制御を行っても移動型ロボットが想定外の動作を行うおそれが低減される。また、参照関節角度ベクトルに係る姿勢に近い姿勢をとる可能性をより高めることができる。
【0015】
前記参照制御点は１つの前記参照領域に対して複数設定され、前記目標関節角度ベクトル情報生成部は、前記目標制御点情報に係る前記制御点の目標位置及び／又は姿勢に最も近い前記参照制御点に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、ものであってもよい。
【0016】
このような構成によれば、参照位置との差分が最も少ない参照制御点を選択することができ、移動型ロボットにおける想定外の動作の可能性をより低減することができる。
【0017】
前記目標関節角度ベクトル情報生成部は、さらに、前記参照関節角度ベクトルに一致するよう前記移動型ロボットの関節を制御する、第１関節制御部と、前記第１関節制御部の動作の後に、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、第２関節制御部と、を備えるものであってもよい。
【0018】
このような構成によれば、まず、参照関節角度ベクトルの通りに関節角度が制御されるので初期動作が統一される。これにより、想定外の事態の発生をより低減することができる。
【0019】
前記移動型ロボットは、さらに、前記移動型ロボットの外部環境を認識する、センサと、前記センサの出力に基づいて、前記移動制御後の前記移動型ロボットの位置又は姿勢を前記目標停止情報と一致するよう補正する、補正制御部と、を備えるものであってもよい。
【0020】
このような構成によれば、移動型ロボットの停止誤差を補正することができるので、移動型ロボットの制御精度を向上させることができる。
【0021】
前記参照領域は、その重心位置が鉛直方向に異なるように前記空間内に複数配置される、ものであってもよい。
【0022】
このような構成によれば、鉛直方向に複数の参照領域が設定されることとなるので、例えば、様々な高さの対象物に対して作業を実行すること等が可能となる。
【0023】
鉛直方向に重心位置が異なるよう配置された複数の前記参照領域は、その鉛直方向の位置に応じて鉛直方向の厚みが異なるよう設定されている、ものであってもよい。
【0024】
このような構成によれば、参照領域の厚みが高さによって変化するのでより安定的に作業を実行することができる。
【0025】
前記参照領域の鉛直方向の厚みは、鉛直上方又は下方に近付く程、小さくなる、ものであってもよい。
【0026】
このような構成によれば、適切なロボット位置が大きく変化する鉛直上方又は下方の領域において参照領域の設定が細かくなるので、より安定的に作業を実行することができる。
【0027】
前記参照領域は、３次元領域であってもよい。
【0028】
このような構成によれば、３次元的な物体を想定して参照情報を生成することができる。
【0029】
前記参照領域は、２次元領域であってもよい。
【0030】
このような構成によれば、２次元的な物体又は３次元的な物体の１側面等を想定して参照情報を生成することができる。
【0031】
前記移動型ロボットの関節角度ベクトルが所定条件を満たす場合に前記関節制御部における制御を無効化する、制御無効化部をさらに備える、ものであってもよい。
【0032】
このような構成によれば、移動型ロボットの動作の安全性をより高めることができる。
【0033】
別の側面から見た本発明は、システムである。すなわち、本発明に係るシステムは、多リンク機構を備えた移動型ロボットと、記憶部と、制御部と、を含むシステムであって、前記記憶部は、所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、前記制御部は、目標領域に関する情報である目標領域情報を取得する、目標領域情報取得部と、前記目標領域に対応する前記参照領域を取得する、参照領域情報取得部と、前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成部と、目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御部と、前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得部と、前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得部と、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成部と、前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御部と、を備えている。
【0034】
別の側面から見た本発明は、方法である。すなわち、本発明に係る制御方法は、多リンク機構と、制御部と、記憶部と、を備えた、移動型ロボットの制御方法であって、前記記憶部は、所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、前記制御部において、目標領域に関する情報である目標領域情報を取得する、目標領域情報取得ステップと、前記目標領域に対応する前記参照領域を取得する、参照領域情報取得ステップと、前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成ステップと、目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御ステップと、前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得ステップと、前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得ステップと、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成ステップと、前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御ステップと、を実行する。
【0035】
別の側面から見た本発明は、プログラムである。すなわち、本発明に係る制御プログラムは、多リンク機構と、制御部と、記憶部と、を備えた、移動型ロボットの制御プログラムであって、前記記憶部は、所定空間内に設定された１又は複数の参照領域に関する情報である参照領域情報と、前記参照領域に対応して設定される前記移動型ロボットの参照停止位置及び姿勢に関する情報である参照停止情報と、前記参照領域に対応して設定される前記移動型ロボットの参照制御点の位置及び姿勢に関する情報である参照制御点情報と、前記参照停止位置及び姿勢にある前記移動型ロボットがその制御点を前記参照制御点の位置及び姿勢へと配置した条件下において前記移動型ロボットが採り得る所望の姿勢を示す参照関節角度ベクトルに関する情報である参照関節角度ベクトル情報と、を記憶しており、前記制御部において、目標領域に関する情報である目標領域情報を取得する、目標領域情報取得ステップと、前記目標領域に対応する前記参照領域を取得する、参照領域情報取得ステップと、前記目標領域に対応する前記参照領域に対応する前記参照停止情報に基づいて、前記移動型ロボットの目標となる停止位置及び姿勢に関する情報である目標停止情報を生成する、目標停止情報生成ステップと、目標となる前記停止位置において前記移動型ロボットが目標となる姿勢となるよう前記移動型ロボットの移動制御を行う、移動制御ステップと、前記移動型ロボットの制御点の目標位置及び姿勢に関する情報である目標制御点情報を取得する、目標制御点情報取得ステップと、前記目標領域に対応する前記参照領域に対応する前記参照関節角度ベクトル情報を取得する、参照関節角度ベクトル情報取得ステップと、前記目標制御点情報と、前記参照関節角度ベクトル情報に基づいて、逆運動学を解き、目標関節角度ベクトル情報を生成する、目標関節角度ベクトル情報生成ステップと、前記目標関節角度ベクトル情報に基づいて前記移動型ロボットを制御する、関節制御ステップと、を実行する。
【発明の効果】
【0036】
本発明によれば、逆運動学を用いた制御を採用しつつも、安全に運用可能なロボットを提供することができる。
【図面の簡単な説明】
【0037】
【発明を実施するための形態】
【0038】
以下、本発明の好適な実施の形態について添付の図面を参照しつつ詳細に説明する。
【0039】
（１．第１の実施形態）
第１の実施形態として、本発明を多リンク機構３０を備えた移動式のロボット、すなわち、モバイルマニピュレータ１００に対して適用した例について説明する。
第１の実施形態として、本発明を多リンク機構３０を備えた移動式のロボット、すなわち、モバイルマニピュレータ１００に対して適用した例について説明する。
【0040】
（１．１ モバイルマニピュレータの構成）
図１は、モバイルマニピュレータ１００の概略構成を示す外観図である。同図から明らかな通り、モバイルマニピュレータ１００は、略直方体の本体部１０を備えている。本体部１０の内部には、モバイルマニピュレータ１００を制御するための情報処理装置等が設けられている。
図１は、モバイルマニピュレータ１００の概略構成を示す外観図である。同図から明らかな通り、モバイルマニピュレータ１００は、略直方体の本体部１０を備えている。本体部１０の内部には、モバイルマニピュレータ１００を制御するための情報処理装置等が設けられている。
【0041】
本体部１０の底面側には、移動機構が備えられている。本実施形態においては、移動機構として差動二輪が採用されている。モバイルマニピュレータ１００は、例えば、環境地図情報と環境のセンシング結果に基づいて、自律移動する。
【0042】
なお、移動機構はこのような構成に限定されない。従って、例えば、全方向移動機構であってもよいし、クローラや脚機構等であってもよい。また、移動機構は、床面上を移動するものに限定されず、例えば、空中を移動するドローン等であってもよい。
【0043】
本体部１０の天面には、多数のリンクが関節を介して接続された多リンク機構３０が備えられている。同図において、多リンク機構３０は、複数のリンクを接続したシリアルリンクアームである。なお、同図においては４つのリンクが例示されているものの、これは例示であって、他の個数のリンクを接続したものであってもよい。多リンク機構３０の先端には、例えば、カメラや把持機構等が備えられている。後述するように、本実施形態において、制御点は、多リンク機構３０の先端近傍に設定される。
【0044】
多リンク機構３０は、関節部とリンクとから構成されるものであればよく、様々な形態を採り得る。例えば、６軸又は７軸のシリアルリンクアーム、胴体部とそれに取り付けられた１又は複数のアームから構成される機構、水平多関節アーム等、様々な構成を採り得る。なお、多リンク機構３０の語は、様々に言い換えることができ、例えば、多関節機構等と称呼してもよい。
【0045】
図２は、モバイルマニピュレータ１００の制御に係るハードウェア構成図である。同図から明らかな通り、モバイルマニピュレータ１００は、制御部１１、記憶部１３、通信部１５、関節駆動部１６、移動機構駆動部１７、センサ１８及びＩ／Ｏ部１９を備え、これらの構成はバス等を介して互いに接続されている。なお、記載を簡潔なものとするため、電源系統等は図示されていない点に留意されたい。
【0046】
制御部１１は、ＣＰＵ等の制御装置であり、後述の種々のプログラムの実行処理を行う。記憶部１３は、ＲＯＭ、ＲＡＭ、ハードディスク、フラッシュメモリ等の記憶装置であり、後述の各種のデータやプログラムを記憶している。通信部１５は、外部装置との間の通信処理を行う通信ユニットである。
【0047】
関節駆動部１６は、制御部１１からの指令に応じて、多リンク機構３０の駆動関節に係るモータ制御を行う。移動機構駆動部１７は、制御部１１からの指令に応じて、移動機構に係るアクチュエータ（例えば、モータ）の制御を行う。センサ１８は、関節角度等を検出するセンサ、ＬｉＤＡＲ、外界を撮影するカメラ等の各種のセンサである。Ｉ／Ｏ部１９は、外部装置との間での情報の入出力を行うインターフェースである。
【0048】
図３は、移動処理を行うモバイルマニピュレータ１００の機能ブロック図である。同図から明らかな通り、記憶部１３は、参照領域情報、参照停止位置・姿勢情報、参照制御点位置・姿勢情報、参照関節角度ベクトル情報を記憶している。また、制御部１１は、目標領域情報取得部１１１、参照領域情報取得部１１２、参照停止位置・姿勢情報取得部１１３、目標停止位置・姿勢情報生成部１１５、及び移動制御部１１６を備えている。
【0049】
目標領域情報取得部１１１は、目標領域情報を取得して、目標停止位置・姿勢情報生成部１１５へと提供する。参照領域情報取得部１１２は、記憶部１３から参照領域情報を取得して、目標停止位置・姿勢情報生成部１１５へと提供する。参照停止位置・姿勢情報取得部１１３は、記憶部１３から参照停止位置・姿勢情報を取得し、目標停止位置・姿勢情報生成部１１５へと提供する。
【0050】
目標停止位置・姿勢情報生成部１１５は、提供された目標領域情報、参照領域情報、及び参照停止位置・姿勢情報に基づいて、目標停止位置・姿勢情報を生成し、移動制御部１１６へと提供する。移動制御部１１６は、目標停止位置・姿勢情報に基づいて、移動機構駆動部１７を制御する。これにより、移動機構が制御される。
【0051】
図４は、関節制御処理を行うモバイルマニピュレータ１００の機能ブロック図である。同図から明らかな通り、記憶部１３は、図３と同様に、参照領域情報、参照停止位置・姿勢情報、参照制御点位置・姿勢情報、参照関節角度ベクトル情報を記憶している。また、制御部１１は、目標制御点位置・姿勢情報取得部１１７、参照停止位置・姿勢情報取得部１１８、参照関節角度ベクトル情報取得部１１９、逆運動学演算部１２０を備えている。
【0052】
目標制御点位置・姿勢情報取得部１１７は、制御点の目標位置・姿勢情報である目標制御点位置・姿勢情報を取得して、逆運動学演算部１２０へと提供する。現在停止位置・姿勢情報取得部１１８は、センサ１８において検出された種々の情報を取得してモバイルマニピュレータ１００の実際の停止位置及び姿勢情報を推定し、推定情報を現在停止位置・姿勢情報として逆運動学演算部１２０へと提供する。なお、推定にあたりその他必要な情報があれば記憶部１３を参照してもよい。例えば、地図情報等を記憶部１３から取得してもよい。参照関節角度ベクトル情報取得部１１９は、記憶部１３から参照関節角度ベクトル情報を取得し、逆運動学演算部１２０へと提供する。
【0053】
逆運動学演算部１２０は、目標制御点位置・姿勢情報、現在停止位置・姿勢情報及び参照関節角度ベクトル情報に基づいて逆運動学演算を行い、目標関節角度ベクトル情報を生成する。逆運動学演算部１２０は、生成された目標関節角度ベクトル情報に基づいて関節駆動部１６を制御する。これにより、多リンク機構３０が制御される。
【0054】
（１．２ モバイルマニピュレータの動作）
（事前設定処理）
まず、事前設定処理について説明する。この事前設定処理は、ＰＣ等の不図示の外部情報処理装置上で行われ、Ｉ／Ｏ部１９を介してモバイルマニピュレータ１００に記憶される。
（事前設定処理）
まず、事前設定処理について説明する。この事前設定処理は、ＰＣ等の不図示の外部情報処理装置上で行われ、Ｉ／Ｏ部１９を介してモバイルマニピュレータ１００に記憶される。
【0055】
図５は、事前設定処理のゼネラルフローチャートである。同図から明らかな通り、まず、事前設定処理を行う者は、情報処理装置上で参照領域を設定する（Ｓ１１）。参照領域とは、作業対象となる物体等に応じて所定の空間上に設定される領域であり、参照座標系で表される。この領域は、３次元領域であってもよいし、２次元領域であってもよい。
【0056】
図６は、３次元の参照領域に関する説明図である。同図の例にあっては、３段の棚上にそれぞれ配置される箱よりやや大きい３つの直方体形状の参照領域５１（５１ａ～５１ｃ）が設定されている。なお、同図の例にあっては、各参照領域５１は、床面上に原点を持つ参照座標系で表現されている。
【0057】
図７は、２次元の参照領域に関する説明図である。同図の例にあっては、所定空間内に配置されている箱の前面よりやや大きい範囲に平面状に参照領域５１ｄが設定されている。このように参照領域を２次元とすることもできる。
【0058】
図５に戻り、参照領域を設定した後、設定者は、情報処理装置上で、参照領域に対応するモバイルマニピュレータ１００の参照制御点の位置及び姿勢の設定処理を行う（Ｓ１２）。参照制御点とは、各参照領域の内部又はその近傍に設定される点である。
【0059】
図８は、参照制御点の概念図である。同図から明らかな通り、本実施形態において参照制御点（Ｒ）は、３次元の参照領域の中心に設定される。なお、参照制御点の位置は床面上に原点を持つ参照座標系で表現される。
【0060】
図５に戻り、制御点の位置及び姿勢の設定処理の後、設定者は、情報処理装置上で、参照領域に対応する参照停車位置及び姿勢の設定処理を行う（Ｓ１３）。参照停車位置及び姿勢とは、参照制御点に制御点が到達することを保証するモバイルマニピュレータ１００の停車位置及び姿勢であり、床面上に原点を持つ参照座標系で表現される。すなわち、モバイルマニピュレータ１００が、この停止位置及び姿勢に停止する限り、逆運動学の計算の解が存在することが保証される。
【0061】
参照領域に対応する参照制御点の位置及び姿勢の設定処理が完了すると、設定者は、情報処理装置上で、参照関節角度ベクトルを算出する処理を行う（Ｓ１５）。参照関節角度ベクトルは、参照停車位置及び姿勢にあるモバイルマニピュレータ１００がその制御点を参照制御点位置及び姿勢へと到達させるときの、各関節の理想的な角度を表している。理想的な関節角度とは、例えば、周囲の環境と衝突してしまうおそれが少ない姿勢や、所謂特異点等の不安定性をもたらさない姿勢である。後述の通り、この参照関節角度ベクトルを参照して、逆運動学に関する演算が行われる。なお、制御点とは、モバイルマニピュレータ１００の一部又は近傍に設定される点であり、例えば、本実施形態においては、制御点は多リンク機構３０の先端近傍に設定される。
【0062】
参照関節角度ベクトルの設定処理が完了すると、設定者は、情報処理装置上で設定した各情報、すなわち、参照領域毎に、参照制御点の位置及び姿勢、参照停車位置及び姿勢、参照関節角度ベクトルに関する情報を、モバイルマニピュレータ１００の記憶部１３へと記憶させる処理を行う（Ｓ１６）。記憶させる処理は、例えば、通信部１５を介して行ってもよいしＩ／Ｏ部１９を介して行ってもよい。記憶処理の後、事前設定処理は終了する。
（作業例）
次に、事前設定処理完了後のモバイルマニピュレータ１００を用いてピッキング作業を行う例について説明する。本実施形態においては、モバイルマニピュレータ１００が、初期位置から移動して棚上に配置された箱の中の物体をピッキングするまでの一連の動作について説明する（ピースピッキング作業）。
（作業例）
次に、事前設定処理完了後のモバイルマニピュレータ１００を用いてピッキング作業を行う例について説明する。本実施形態においては、モバイルマニピュレータ１００が、初期位置から移動して棚上に配置された箱の中の物体をピッキングするまでの一連の動作について説明する（ピースピッキング作業）。
【0063】
なお、本発明に係る作業は、このような作業に限定されることはなく、例えば、把持した物体を目標領域内に載置するような作業であってもよいし、物体ではなく箱自体に対する作業（ケースピッキング作業）を行ってもよい。
【0064】
図９は、モバイルマニピュレータ１００の行う作業の一例について説明するゼネラルフローチャートである。同図から明らかな通り、処理が開始すると、目標領域情報取得部１１１は、目標となる領域の位置に関する情報を取得する処理を行う。本実施形態において、目標領域情報は、通信部１５を介して外部から取得するものとするが、そのような構成に限定されない。従って、例えば、予め記憶部１３に記憶しておき、それを読み出してもよい。
【0065】
図１０は、目標領域６１に関する説明図である。同図（Ａ）は、本実施形態に係る目標領域の位置を示しており、同図（Ｂ）は、目標領域の配置イメージ図である。同図（Ａ）から明らかな通り、本実施形態において目標領域６１は、例として「左から２列目の棚６０の、下から３段目の棚板上にあって、奥からＮ番目の箱」に設定されている。また、同図（Ｂ）から明らかな通り、目標領域６１に相当する箱の内部にはピッキング対象となる目標物体（目標となるピース）が入っている。
【0066】
図９に戻り、目標領域の取得の後、モバイルマニピュレータ１００は、目標停車位置及び姿勢へと遷移するよう制御される（Ｓ２２）。
【0067】
図１１は、目標停止位置及び姿勢への遷移処理（Ｓ２２）の詳細フローチャートである。同図から明らかな通り、処理が開始すると、参照領域情報取得部１１２は、記憶部１３から目標領域に対応する参照領域に関する情報を取得する処理を行う（Ｓ２２１）。ここで、読み出される参照領域情報は、例えば、上述の３段目の棚板に配置された箱に対応する参照領域に関する情報である。
【0068】
参照領域情報の取得処理の後、参照停止位置・姿勢情報取得部１１３は、記憶部１３から参照領域に対応する参照停止位置及び参照停止姿勢を取得する処理を行う（Ｓ２２２）。
【0069】
目標停止位置・姿勢情報生成部１１５は、取得した参照停止位置及び姿勢に基づいて、地図座標系における目標停止位置及び姿勢を生成する処理を行う（Ｓ２２３）。これにより、地図座標系におけるモバイルマニピュレータ１００の目標位置・姿勢が定まる。
【0070】
その後、移動制御部１１６は、移動機構を駆動させる移動機構駆動部１７を制御して、モバイルマニピュレータ１００を目標停止位置及び姿勢へと移動（又は遷移）させる。この移動処理には、公知の種々の手法を採用することができる。
【0071】
図９に戻り、目標停車位置及び姿勢への遷移処理の後、目標制御点位置・姿勢情報取得部１１７は、制御点の目標位置・姿勢情報を取得する処理を行う（Ｓ２３）。本実施形態において、制御点の目標位置・姿勢情報は、通信部１５を介して外部から取得するものとするが、そのような構成に限定されない。従って、例えば、予め記憶部１３に記憶しておき、それを読み出してもよい。
【0072】
制御点位置及び姿勢の取得処理の後、逆運動学演算部１２０は、逆運動学を解くことにより、目標関節角度ベクトルを生成する処理を行う（Ｓ２５）。
【0073】
図１２は、目標関節角度ベクトルの生成処理に関する詳細フローチャートである。同図から明らかな通り、処理が開始すると、現在停止位置・姿勢情報取得部１１８は、センサ１８から検出した情報等を取得してモバイルマニピュレータ１００の現在の実際の停止位置・姿勢を推定する処理を行う（Ｓ２５１）。この推定した現在停止位置・姿勢情報は、逆運動学演算部１２０へと提供される。なお、モバイルマニピュレータ１００の停止位置・姿勢について十分な精度が見込まれる場合には、現在停止位置・姿勢情報に代えて参照停止位置・姿勢情報を取得してもよい。次に、推定処理の後、参照関節角度ベクトル情報取得部１１９は、参照領域に対応する参照関節角度ベクトルを取得して逆運動学演算部１２０へと提供する処理を行う（Ｓ２５２）。
【0074】
その後、逆運動学演算部１２０は、現在停止位置・姿勢にあるモバイルマニピュレータ１００の制御点の目標位置及び姿勢について、参照関節角度ベクトルを初期値として、逆運動学を演算する処理を行うことにより、目標関節角度ベクトルを生成する（Ｓ２５３）。
【0075】
本実施形態において、逆運動学の演算は、以下数式の通り、参照関節角度ベクトルを初期値として、E（ｑ）を反復的計算により最小化する演算である。ただし、ｅは制御点の目標位置及び姿勢（＝ｐ＊）と、順運動学モデル（＝ｆ（ｑ））との誤差ベクトル（＝ｐ＊－ｆ（ｑ））を表している。また、Ｗｅは位置及び姿勢の各次元の誤差に対する重み行列である。
【0076】
【数1】
【0077】
このような最小化問題を解くには、例えば、Newton-Raphson法やLevenberg-Marquardt法等を用いることができる。なお、参照関節角度ベクトルを参照する逆運動学演算は本手法に限定されず、他の手法であってもよい。
【0078】
このような構成によれば、参照関節角度ベクトルを参照するので、逆運動学を用いて関節制御を行ってもモバイルマニピュレータ１００が想定外の動作を行うおそれが低減される。
【0079】
図９に戻り、目標関節角度ベクトルの生成処理の後、関節制御部１２１は、逆運動学演算により得られた目標関節角度ベクトルを目標値として関節駆動部１６を介して各関節を制御する。
【0080】
各関節の駆動処理の後、処理は終了する。なお、一連の動作終了後に、多リンク機構３０の先端に備えられた把持機構等を用いて箱の内部の物体の把持動作（ピースピッキング）を行ってもよいことは勿論である。また、その後に移動動作等を実行してもよい。
【0081】
このような構成によれば、逆運動学を用いて目標関節角度ベクトル情報を生成する際に、参照領域毎に予め設定された参照関節角度ベクトル情報を用いるので、逆運動学を用いて関節制御を行ってもモバイルマニピュレータ１００が想定外の動作を行うおそれが低減される。すなわち、逆運動学を用いた制御を採用しつつも、安全に運用可能なモバイルマニピュレータ１００を提供することができる。
【0082】
（２．変形例）
本発明は、様々に変形して実施することができる。
本発明は、様々に変形して実施することができる。
【0083】
上述の実施形態においては、参照関節角度ベクトルを初期値として逆運動学を計算する構成について説明したが、本発明はそのような構成に限定されない。すなわち、逆運動学計算にあたっての参照関節ベクトルの参照はそのような手法に限定されない。例えば、逆運動学の演算処理（Ｓ２５２）の際に、参照関節角度ベクトルとの関節角度の誤差を最小化するように優先度付き逆運動学を解いてもよい。優先度付き運動学とは、制御点を目標の位置及び姿勢に到達させた状態で、冗長自由度を用いて他のコスト関数を最小化する計算である。
【0084】
すなわち、この場合の逆運動学の演算は、以下数式の通り、誤差ｅ１＝０の制約条件の下で、左辺の数式を最小化する演算である。ただし、誤差ｅ１は、制御点の目標位置及び姿勢（＝ｐ＊）と順運動学モデル（＝ｆ（ｑ））との差分（＝ｐ＊－ｆ（ｑ））である。また、誤差ｅ２は、目標とする参照関節角度ベクトル（＝ｑｄ）と関節角度ベクトル（＝ｑ）との差分（＝ｑｄ－ｑ）であり、Ｗは重み行列（正定値行列）である。
【0085】
【数2】
【0086】
なお、この数式は、ラグランジュの未定定数法と勾配法を用いて解くことができる。
【0087】
このような構成によれば、逆運動学を用いて関節制御を行ってもモバイルマニピュレータ１００が想定外の動作を行うおそれが低減される。また、本手法によれば、参照関節角度ベクトルに係る姿勢に近い姿勢をとる可能性をより高めることができる。
【0088】
上述の実施形態においては、参照制御点は参照領域に１つ設けられる構成としたが、本発明はそのような構成に限定されない。従って、例えば、参照領域に複数の参照制御点を設けてもよい。
【0089】
図１３は、１つの参照領域に複数の参照制御点を設ける場合を示す説明図である。同図から明らかな通り、本変形例にあっては、１つの参照領域に対して複数の制御点（Ｒ１～Ｒ４）が設けられる。
【0090】
なお、このとき、使用する参照制御点を、制御点の目標位置又は姿勢に応じて選択してもよい。例えば、制御点の目標位置に最も近い参照制御点を選択して対応する参照関節角度ベクトルを取得してもよい。なお、近さは、制御点の目標位置と参照制御点の位置との間のノルムに基づいて判定してもよいし、位置に加えて姿勢も考慮した誤差ベクトルの大きさに基づいて判定してもよい。
【0091】
このような構成によれば、参照位置との差分が最も少ない参照制御点を選択することができ、モバイルマニピュレータ１００における想定外の動作の可能性をより低減することができる。
【0092】
上述の実施形態においては、参照関節角度ベクトルを逆運動学計算の際にのみ参照する構成としたが、参照関節ベクトルの参照のタイミングはそのような構成に限定されない。従って、例えば、目標停止位置への移動の後に参照関節角度ベクトルに基づいて関節制御処理を行ってもよい。
【0093】
図１４は、モバイルマニピュレータ１００の行う作業の一例について説明するゼネラルフローチャート（変形例１）である。同図から明らかな通り、本変形例においては、目標停止位置への移動（Ｓ２２）後に、参照関節ベクトルを目標として関節角度を制御する処理が行われる（Ｓ２１１）。その後の処理（Ｓ２３～Ｓ２６）は、上述の実施形態と同様である。
【0094】
このような構成によれば、まず、参照関節角度ベクトルの通りに関節角度が制御されるので初期動作が統一される。これにより、想定外の事態の発生をより低減することができる。
【0095】
上述の実施形態においては、目標停止位置・姿勢への遷移処理（Ｓ２２）の後に、制御点に関する処理（Ｓ２３～Ｓ２６）を行う構成とした。しかしながら、モバイルマニピュレータ１００においては、床面との間のスリップや移動に用いる地図情報の精度等により、停止位置・姿勢について誤差が生じることがある。そのため、この停止位置・姿勢に関する誤差を補正する処理を行ってもよい。
【0096】
図１５は、モバイルマニピュレータ１００の行う作業の一例について説明するゼネラルフローチャート（変形例２）である。同図から明らかな通り、本変形例に係るフローチャートでは、目標停止位置及び姿勢への遷移処理（Ｓ２２）の後、モバイルマニピュレータ１００は、移動機構等を制御して、その停止位置及び姿勢の補正処理を行い、その後に、制御点に関する制御等を行う（Ｓ２３～Ｓ２６）。
【0097】
より具体的には、モバイルマニピュレータ１００は、カメラ等のセンサ１８を用いて外界の認識を行い、当該認識結果に基づいて目標停止位置及び姿勢との誤差を算出し、当該誤差を減少させるように移動機構等を制御する。
【0098】
このような構成によれば、停止誤差を補正することができるので、モバイルマニピュレータ１００の制御精度を向上させることができる。
【0099】
上述の実施形態において、参照領域は、同一の大きさの直方体形状を規則的に配置する構成としたが、本発明はそのような構成に限定されない。従って、参照領域の位置、大きさ、又は形状等を様々に変更してもよい。
【0100】
図１６は、参照領域５２の設定に関する変形例を示す説明図である。同図の例にあっては、参照領域５２は鉛直方向に複数配置されている（５２ａ～５２ｉ）。特に、同図の例にあっては、多リンク機構３０の付け根からの距離が大きい程、換言すれば、鉛直上下方向に近づく程、参照領域５２の鉛直上下方向の高さ（又は厚さ）は小さくなるよう設定されている。例えば、中心付近の参照領域５２ｅよりも上下端に近い参照領域（５２ａ～５２ｄ、５２ｆ～５２ｉ）の鉛直上下方向の高さ（又は厚さ）は端部に近付く程小刻みとなるように設定されている。
【0101】
このような構成によれば、鉛直方向に複数の参照領域５２が設定されることとなるので、例えば、様々な高さの対象物に対して作業を実行すること等が可能となる。また、モバイルマニピュレータ１００の停止位置の大幅な変動が必要となる鉛直上方又は下方の領域において参照領域５２の設定が細かくなるので、より安定的に作業を実行することができる。
【0102】
上述の実施形態においては、目標関節角度ベクトルとなるように各関節の制御を行うものとして説明したが、本発明はそのような構成に限定されない。従って、例えば、関節角度を検出するセンサ１８の出力に基づいて、関節角度が所定条件を満たすか否かを判定し、所定条件を満たす場合には制御を無効化する制御無効化部（リミッタ）を設けてもよい。所定条件とは、例えば、特異姿勢に近い姿勢を表す関節角度ベクトル等であり、具体的には、関節角度が所定値以下又は以上となることである。
【0103】
このような構成によれば、モバイルマニピュレータ１００の動作の安全性をより高めることができる。
【0104】
上述の実施形態においては、モバイルマニピュレータ１００が、制御部１１、記憶部１３等を備える構成として説明したが、本発明はそのような構成に限定されない。従って、例えば、必ずしも制御部１１、記憶部１３がモバイルマニピュレータ１００内に配置されるわけではないシステムとして構成してもよい。
【0105】
以上、本発明の実施形態について説明したが、上記実施形態は本発明の適用例の一部を示したに過ぎず、本発明の技術的範囲を上記実施形態の具体的構成に限定する趣旨ではない。また、上記の実施形態は、矛盾が生じない範囲で適宜組み合わせ可能である。
【産業上の利用可能性】
【0106】
本発明は、ロボット等を製造する産業において利用可能である。
【符号の説明】
【0107】
１０ 本体部
１１ 制御部
１３ 記憶部
１５ 通信部
１６ 関節角度駆動部
１７ 移動機構駆動部
１８ センサ
１９ Ｉ／Ｏ部
３０ 多リンク機構
５１ 参照領域
５２ 参照領域（変形例）
６０ 棚
６１ 目標領域
１００ モバイルマニピュレータ
１１１ 目標領域情報取得部
１１２ 参照領域情報取得部
１１３ 参照停止位置・姿勢情報取得部
１１５ 目標停止位置・姿勢情報生成部
１１６ 移動制御部
１１７ 目標制御点位置・姿勢情報取得部
１１８ 参照停止位置・姿勢情報取得部
１１９ 参照関節角度ベクトル情報取得部
１２０ 逆運動学演算部
１２１ 関節制御部
１１ 制御部
１３ 記憶部
１５ 通信部
１６ 関節角度駆動部
１７ 移動機構駆動部
１８ センサ
１９ Ｉ／Ｏ部
３０ 多リンク機構
５１ 参照領域
５２ 参照領域（変形例）
６０ 棚
６１ 目標領域
１００ モバイルマニピュレータ
１１１ 目標領域情報取得部
１１２ 参照領域情報取得部
１１３ 参照停止位置・姿勢情報取得部
１１５ 目標停止位置・姿勢情報生成部
１１６ 移動制御部
１１７ 目標制御点位置・姿勢情報取得部
１１８ 参照停止位置・姿勢情報取得部
１１９ 参照関節角度ベクトル情報取得部
１２０ 逆運動学演算部
１２１ 関節制御部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】