特開 2022-135512 移動マニピュレータの姿勢最適化方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022135512

(43)【公開日】2022-09-15

(54)【発明の名称】移動マニピュレータの姿勢最適化方法

(51)【国際特許分類】

   B25J 9/10 20060101AFI20220908BHJP

【ＦＩ】

B25J9/10 A

【審査請求】未請求

【請求項の数】2

【出願形態】ＯＬ

(21)【出願番号】P 2021035364

(22)【出願日】2021-03-05

【新規性喪失の例外の表示】特許法第３０条第２項適用申請有り ２０２０年１０月１１日に第３８回ロボット学会学術講演会にて発表

(71)【出願人】

【識別番号】000005197

【氏名又は名称】株式会社不二越

(71)【出願人】

【識別番号】504180239

【氏名又は名称】国立大学法人信州大学

(74)【代理人】

【識別番号】100120400

【弁理士】

【氏名又は名称】飛田 高介

(74)【代理人】

【識別番号】100124110

【弁理士】

【氏名又は名称】鈴木 大介

(72)【発明者】

【氏名】遠藤 大輔

(72)【発明者】

【氏名】山崎 公俊

(72)【発明者】

【氏名】鈴木 聡史

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707AS01

3C707BS10

3C707CS08

3C707DS01

3C707ES03

3C707ET01

3C707KS20

3C707LT17

3C707LV05

3C707LV07

3C707LW08

3C707WA16

(57)【要約】

【課題】物体やハンドの形状、すなわち把持する際の手先の方向を考慮して、把持動作の実現しやすさを用いて到着時の台車の姿勢（位置と方向）を評価する。
【解決手段】把持対象物を把持するための移動マニピュレータの姿勢を設定し、確率的な誤差を含む台車およびマニピュレータの移動後の手先姿勢の推定値（Ｘｈｅ）を算出し、把持対象物を把持することが可能な手先姿勢である把持可能領域（Ｚｇ）を算出し、移動後の手先姿勢の推定値（Ｘｈｅ）と把持対象物の物体姿勢（Ｘｏ）を境界の両端とする修正領域（Ｚｍ）を算出し、把持可能領域（Ｚｇ）と修正領域（Ｚｍ）の重複範囲を余裕領域（Ｚ）とし、余裕領域（Ｚ）の中に手先を移動修正した場合の修正後の手先姿勢（Ｘｈｃ）を設定し、移動後の手先姿勢の推定値（Ｘｈｅ）と余裕領域の中の手先姿勢（Ｘｈｃ）の確率的な距離の積分から移動マニピュレータの姿勢を最適化する。
【選択図】図１

【特許請求の範囲】

【請求項1】

台車にマニピュレータを搭載した移動マニピュレータの姿勢最適化方法であって、
少なくとも位置と方位を含む状態量を姿勢と呼ぶとき、
把持対象物を把持するための移動マニピュレータの姿勢を設定し、
確率的な誤差を含む台車およびマニピュレータの移動後の手先姿勢の推定値（Ｘｈｅ）を算出し、
把持対象物を把持することが可能な手先姿勢である把持可能領域（Ｚｇ）を算出し、
移動後の手先姿勢の推定値（Ｘｈｅ）と把持対象物の物体姿勢（Ｘｏ）を境界の両端とする修正領域（Ｚｍ）を算出し、
把持可能領域（Ｚｇ）と修正領域（Ｚｍ）の重複範囲を余裕領域（Ｚ）とし、
余裕領域（Ｚ）の中に手先を移動修正した場合の修正後の手先姿勢（Ｘｈｃ）を設定し、
移動後の手先姿勢の推定値（Ｘｈｅ）と余裕領域の中の手先姿勢（Ｘｈｃ）の確率的な距離の積分から移動マニピュレータの姿勢を評価することを特徴とする移動マニピュレータの姿勢最適化方法。

【請求項2】

台車にマニピュレータを搭載した移動マニピュレータの姿勢最適化方法であって、
少なくとも位置と方位を含む状態量を姿勢と呼ぶとき、
把持対象物を把持するための移動マニピュレータの姿勢を設定し、
確率的な誤差を含む台車およびマニピュレータの移動後の手先姿勢の推定値（Ｘｈｅ）を算出し、
把持対象物を把持することが可能な手先姿勢である把持可能領域（Ｚｇ）を算出し、
前記把持可能領域（Ｚｇ）のうち把持対象物の物体中心から移動後の手先姿勢の推定値（Ｘｈｅ）側の領域を余裕領域（Ｚ）とし、
余裕領域（Ｚ）の中に手先を移動修正した場合の修正後の手先姿勢（Ｘｈｃ）を設定し、
移動後の手先姿勢の推定値（Ｘｈｅ）と余裕領域の中の手先姿勢（Ｘｈｃ）の確率的な距離の積分から移動マニピュレータの姿勢を評価することを特徴とする移動マニピュレータの姿勢最適化方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、台車にマニピュレータを搭載した移動マニピュレータの姿勢最適化方法に関する。

【背景技術】

【0002】

移動マニピュレータは移動能力と物体操作能力を併せ持つロボットである。その特性から、物体運搬などの作業の自動化が期待される。しかし、台車の位置決め誤差が比較的大きく、マニピュレータの高い位置決め精度を生かせないという問題点がある。よって、移動マニピュレータによる物体操作では、台車の姿勢誤差を最適化することが求められている。

【0003】

特許文献１には、移動体に設置されたカメラが把持対象物体を検出することが可能な領域である物体検出可能領域と、移動体に設置されたロボットアームが把持対象物体に到達することが可能な領域である物体到達可能領域と、に基づいて、ロボットアームにより把持対象物体を把持可能な領域である物体把持可能領域を設定するステップと、移動体の自己位置ばらつきと、追従ずれと、に基づいて物体把持可能領域への到達可能性を判定するステップと、移動体を物体把持可能領域内に到達させる経路計画を生成するステップと、を備える移動体の移動制御方法が開示されている。

【0004】

非特許文献１には、ロボット手先と把持対象物との相対位置の誤差楕円体の主軸方向と、マニピュレータの可操作性楕円体の主軸方向とが一致するように動作を計画することが提案されている。そして、台車の姿勢誤差を確率分布としてモデル化し、手先が目標とする姿勢へ到達できる確率を定義し、誤差の修正に必要なマニピュレータの動作量の期待値を求め、それを評価指標とし、台車姿勢を評価している。

【先行技術文献】

【特許文献】

【0005】

【特許文献1】特開２０１３－２３７１２５号公報

【非特許文献】

【0006】

【非特許文献1】K. Yamazaki, M. Tomono and T. Tsubouchi: “Pose Planning for a Mobile Manipulator based on Joint Motions for Posture Adjustment to End-Effector Error,” Advanced Robotics, vol.22, no.4, pp.411.431, 2008.

【発明の概要】

【発明が解決しようとする課題】

【0007】

しかしながら、従来の技術においては、台車が移動後に物体を「把持できるかどうか」までしか検討されていない。これに対し発明者らは、さらに、把持動作の実現しやすさを評価する必要があることに着眼した。ここで、把持動作の実現しやすさとは、台車の移動に伴う手先姿勢誤差が入りにくく、もし入った場合にもそれを修正しやすいこととする。そのため、「計画時に得たマニピュレータの姿勢をできるだけ変えずに、物体把持が実現できること」を指標とする方法を提案する。

【0008】

特許文献１に記載の技術は、台車の移動制御は記載されているものの、台車の到達位置の演算には検出可能領域および把持可能領域しか考慮していない。すなわち、台車の到達位置を評価するためにロボットアームの誤差は考慮されていない。また、物体をどの方位からでも把持できることを想定しており、物体やグリッパの形状や向きによる影響を考慮していない。

【0009】

非特許文献１では、物体をどの方位からでも把持できることを想定しており、物体やグリッパの形状や向きによる影響を考慮していない。したがって、四角や六角のような多面体、シャフトなどの棒材などのように把持する方向に異方性がある場合には、マニピュレータの姿勢をさらに多く修正する必要がある場合がある。

【0010】

そこで本発明では、物体やハンドの形状、すなわち把持する際の手先の方向を考慮して、把持動作の実現しやすさを用いて到着時の台車の姿勢（位置と方向）を評価することが可能な移動マニピュレータの姿勢最適化方法を提案することを目的としている。

【課題を解決するための手段】

【0011】

上記課題を解決するために、本発明の代表的な構成は、台車にマニピュレータを搭載した移動マニピュレータの姿勢最適化方法であって、少なくとも位置と方位を含む状態量を姿勢と呼ぶとき、把持対象物を把持するための移動マニピュレータの姿勢を設定し、確率的な誤差を含む台車およびマニピュレータの移動後の手先姿勢の推定値（Ｘｈｅ）を算出し、把持対象物を把持することが可能な手先姿勢である把持可能領域（Ｚｇ）を算出し、移動後の手先姿勢の推定値（Ｘｈｅ）と把持対象物の物体姿勢（Ｘｏ）を境界の両端とする修正領域（Ｚｍ）を算出し、把持可能領域（Ｚｇ）と修正領域（Ｚｍ）の重複範囲を余裕領域（Ｚ）とし、余裕領域（Ｚ）の中に手先を移動修正した場合の修正後の手先姿勢（Ｘｈｃ）を設定し、移動後の手先姿勢の推定値（Ｘｈｅ）と余裕領域の中の手先姿勢（Ｘｈｃ）の確率的な距離の積分から移動マニピュレータの姿勢を評価することを特徴とする。

【0012】

本発明の他の代表的な構成は、台車にマニピュレータを搭載した移動マニピュレータの姿勢最適化方法であって、少なくとも位置と方位を含む状態量を姿勢と呼ぶとき、把持対象物を把持するための移動マニピュレータの姿勢を設定し、確率的な誤差を含む台車およびマニピュレータの移動後の手先姿勢の推定値（Ｘｈｅ）を算出し、把持対象物を把持することが可能な手先姿勢である把持可能領域（Ｚｇ）を算出し、前記把持可能領域（Ｚｇ）のうち把持対象物の物体中心から移動後の手先姿勢の推定値（Ｘｈｅ）側の領域を余裕領域（Ｚ）とし、余裕領域（Ｚ）の中に手先を移動修正した場合の修正後の手先姿勢（Ｘｈｃ）を設定し、移動後の手先姿勢の推定値（Ｘｈｅ）と余裕領域の中の手先姿勢（Ｘｈｃ）の確率的な距離の積分から移動マニピュレータの姿勢を評価することを特徴とする。

【発明の効果】

【0013】

本発明によれば、物体把持における手先姿勢の許容誤差を定式化し、誤差の修正先を許容誤差の範囲内に設定する。そしてマニピュレータの修正量の期待値を計算し、評価指標とする。これによって、把持動作の実現しやすさを用いて到着時の台車の姿勢（位置と方向）を評価することが可能な移動マニピュレータの姿勢最適化方法を提供することができる。

【図面の簡単な説明】

【0014】

【図1】本実施形態にかかる移動マニピュレータの構成を示す図である。

【図2】物品把持タスクの全体的な流れを説明するフローチャートである。

【図3】探索の段取りを詳細に説明するフローチャートである。

【図4】評価の手法について説明する図である。

【図5】平行グリッパの場合の把持可能領域および余裕領域を説明する図である。

【図6】グリッドマップを用いた環境表現を説明する図である。

【0015】

以下に添付図面を参照しながら、本発明の好適な実施形態について詳細に説明する。かかる実施形態に示す寸法、材料、その他具体的な数値などは、発明の理解を容易とするための例示に過ぎず、特に断る場合を除き、本発明を限定するものではない。なお、本明細書及び図面において、実質的に同一の機能、構成を有する要素については、同一の符号を付することにより重複説明を省略し、また本発明に直接関係のない要素は図示または説明を省略する。

【0016】

［用語の定義］
ここで、本出願において、「姿勢」という語は位置と方位を含む状態量とし、以下のように定義する。
・台車姿勢：台車の現在の状態
・手先姿勢：マニピュレータの先端に取り付けられたエンドエフェクタの現在の状態
・マニピュレータ姿勢：マニピュレータへの関節角度指令から実現されるマニピュレータの現在の状態
・ロボット姿勢：台車姿勢とマニピュレータ姿勢を合わせた移動マニピュレータの現在の状態
・物体姿勢：把持対象物（以下、単に「物体」という。）の現在の状態。

【0017】

［全体構成］
図１は本実施形態にかかる移動マニピュレータの構成を示す図である。移動マニピュレータ１００は、台車１１０にマニピュレータ１２０を搭載している。マニピュレータ１２０の先端には物体１４０を把持する手先１３０（エンドエフェクタ）が取り付けられている。手先１３０は、本実施形態では平行二指グリッパを例に用いているが、把持に方向があるものであればよく、例えば三指や四指のグリッパ、もしくは吸引カップを有する真空吸着グリッパであってもよい。物体１４０は、本実施形態において重要なことであるが、四角や六角のような多面体、シャフトなどの棒材などのように把持する方向に異方性があるものを想定する。

【0018】

移動マニピュレータ１００によって物体１４０を把持するときの問題点は、移動後の手先姿勢が必ずしも目標手先姿勢と一致しないことである。本発明では、この主な要因を台車１１０の姿勢誤差によるものと考える。一般に、台車姿勢はオドメトリを利用して推定できるが、ここには初期位置のずれやスリップなどの要因で誤差が含まれる。すなわち課題は、この誤差の影響をなるべく受けないロボット姿勢を探すことである。一方で別の問題として、マニピュレータ１２０の冗長自由度がある。本実施形態では、台車１１０とマニピュレータ１２０が持つ自由度の合計が、物体１４０を把持するために必要な自由度より多いと仮定する。

【0019】

マニピュレータ１２０に冗長自由度を持たせているのは、主に、把持した物体１４０を台車１１０の上に乗せて搬送することを想定するため、把持するためだけの自由度では不足するからである。一方、冗長自由度があることにより、物体１４０を把持可能な台車１１０の姿勢が複数存在する。そこで、複数の台車１１０の姿勢の中からどれを選ぶかをどのような評価によって行う（最適化する）かが課題となる。

【0020】

図２は物品把持タスクの全体的な流れを説明するフローチャートである。まず移動マニピュレータ１００は移動前の台車姿勢、手先指定、マニピュレータ姿勢、物体姿勢を取得する（Ｓ１００）。この初期値の取得については、センサ等によって取得してもよいし、コンピュータ等から入力した設定値を用いてもよい。

【0021】

次にＳ１００で取得した情報に基づいて、目標手先姿勢を決定する。そして、それを実現するための台車姿勢およびマニピュレータ姿勢を探索する（Ｓ１０２）。

【0022】

そして、決定した台車姿勢およびマニピュレータ姿勢に達するまで移動させる（Ｓ１０４）。その後、目標手先姿勢と実際の手先姿勢にずれがあった場合は、新しいマニピュレータ姿勢を再計算する（Ｓ１０６）。それから再計算の結果に従ってマニピュレータ１２０を動かし、物体を把持する（Ｓ１０８）。

【0023】

ここで、Ｓ１０２において評価がよい台車姿勢を探索してあれば、Ｓ１０６で再計算されるマニピュレータ姿勢では、マニピュレータ１２０の動き量が少ないはずである。本発明では、手先姿勢の誤差が小さく、さらにその誤差に対して手先の操作性が高く保てる台車姿勢およびマニピュレータ姿勢を探す。ここでの「操作性が高い」とは、マニピュレータ１２０が現在の姿勢から別の姿勢に変移する場合に、各関節がより少ない動きで目標姿勢に到達できることと定義する。

【0024】

［探索の手法］
本実施形態で利用する姿勢変数を次のように定義する。
ＸＧ:世界座標系での現在の台車姿勢
ＸＨ:世界座標系での現在の手先姿勢
Ｘｈ：台車基準の座標系での手先姿勢
ＸＯ：世界座標系での現在の物体姿勢（把持対象物の物体姿勢）
Ｘｈｅ:移動後の手先姿勢の推定値
Ｘｈｃ：修正後の手先姿勢
添え字が大文字の場合は世界座標系、小文字の場合は台車を基準とした座標系での変数である。それそれの姿勢に（ｅ）がついている場合は推定値(estimated value)を意味し、（ｃ）がついている場合は修正値(corrected value)を意味する。本実施形態では簡単のため、ＸＯはロボットにとっての把持目標姿勢であるとする。

【0025】

図３は探索（Ｓ１０２）の探索の段取りを詳細に説明するフローチャートである。以下に説明する誤差の算出や推定などの演算は、台車１１０に搭載した演算部（コンピュータ）で行ってもよいし、台車１１０と接続された外部コンピュータで行ってもよい。

【0026】

まず、物体１４０を把持するための移動マニピュレータ１００の姿勢を設定する（Ｓ２００）。詳しくは、手先１３０が物体１４０に届く範囲内に台車１１０の目標姿勢を設定し、同時にその台車目標姿勢から物体１４０を把持するためのマニピュレータ姿勢（想定マニピュレータ姿勢）を算出しておく。

【0027】

台車１１０が台車目標姿勢へ到達するための走行経路を生成する（Ｓ２０２）。台車１１０およびマニピュレータ１２０の移動後の、確率的な誤差を含む手先姿勢の推定値Ｘｈｅを算出する（Ｓ２０４）。詳しくは、走行経路にしたがって台車１１０が移動した場合に、台車目標姿勢における姿勢誤差（確率的な誤差）を算出する。その姿勢誤差がある状態でマニピュレータ１２０が想定マニピュレータ姿勢をとった場合に、手先１３０が有する誤差（確率的な誤差）を算出する。ここで確率的な誤差とは、このくらいの確率でこのくらいの誤差を有するというマトリクス値である。

【0028】

それから、手先姿勢の誤差を「物体１４０を問題なく把持できる程度に」修正するためのマニピュレータ１２０の動作量を算出する（Ｓ２０６）。この動作量が小さい場合、そのときのロボット姿勢はよい姿勢であると評価する（Ｓ２０８）。本発明はこの評価の部分において特徴を有している。

【0029】

ロボット姿勢の評価は、マニピュレータ１２０の関節変数を軸とした空間でおこなう。以後、この空間を関節空間とよび、ロボットが実際に作業をおこなう空間を作業空間と呼ぶ。姿勢の評価式を以下のように定義する。評価値Ｃは、移動後の手先姿勢の推定値（Ｘｈｅ）と余裕領域Ｚの中の手先姿勢（Ｘｈｃ）の確率的な距離の積分である。評価値Ｃが小さいほど、良い姿勢である。

【数1】

ここで、Ｘ（ｘ）は、手先姿勢に対してマニピュレータの逆キネマティクスを解き、各関節角を求める関数である。またＰ（ｘ）はｘの確率密度分布である。

【0030】

図４は評価の手法について説明する図であって、図４（ａ）は評価のフローチャート、図４（ｂ）は概念図である。

【0031】

まず把持対象物の物体姿勢Ｘｏから、物体１４０を把持することが可能な手先姿勢である把持可能領域（Ｚｇ）を算出する（Ｓ３００）。誤差がない理想的な状況では、マニピュレータ１２０の手先１３０はＺｇに到達するので、物体１４０を把持できるはずである。しかし実際には台車１１０の自己位置推定などに誤差が伴い、手先の位置にも誤差が伴う。したがって移動後の手先姿勢の推定値Ｘｈｅは、把持可能領域Ｚｇにあるとは限らない。そこで、マニピュレータ１２０を動かして手先姿勢を修正することを考える。

【0032】

そこで図４（ｂ）に示すように、移動後の手先姿勢の推定値Ｘｈｅと把持対象物の物体姿勢Ｘｏを境界の両端とする修正領域Ｚｍを算出する（Ｓ３０２）。修正領域Ｚｍは、手先姿勢を修正するにあたり、修正後の手先姿勢の存在範囲を絞り込むための領域である。把持可能領域Ｚｇは、手先１３０の最大開き幅や物体１４０の形状などから決まる、本来の把持目標姿勢からずれてもよい範囲を表す領域である。

【0033】

そして把持可能領域Ｚｇと修正領域Ｚｍの重複範囲を余裕領域Ｚとして算出する（Ｓ３０４）。すなわち、余裕領域は次式（２）で表すことができる。

【数2】

結果として余裕領域Ｚは、把持可能領域Ｚｇのうち、物体１４０の中心から手先姿勢の推定値Ｘｈｅ側の領域に生成される。すなわち、手先姿勢を修正する際、無駄に遠い場所に修正先の姿勢を設定することを防ぐ。これにより、期待値を求める式（１）が不要に大きくなることを防ぐことができる。

【0034】

そして、余裕領域Ｚの中に手先を移動修正した場合の修正後の手先姿勢Ｘｈｃを設定する（Ｓ３０６）。このとき、手先１３０で物体１４０を挟み込む際には、クリアランスがあり、手先１３０と物体１４０が理想的な姿勢関係にならなくとも、多少のズレであれば把持動作時に吸収できるとする。この前提では、修正後の手先姿勢Ｘｈｃは必ずしも物体姿勢Ｘｏと一致させる必要はない。すなわち、クリアランスを許す範囲に収まる位置まで手先を動かせば、充分と言える。したがって修正後の手先姿勢Ｘｈｃは、必ずしも物体姿勢Ｘｏではなく、余裕領域の内部から推定値Ｘｈｅ寄りの位置に設定することができる。

【0035】

本発明の指標では、ＸｈｅとＸｈｃの間の姿勢変化で、マニピュレータ１２０の各関節の動作量が少なければ良いと考える。これは、マニピュレータ１２０の関節空間において、修正前の座標と修正後の座標の間の距離が小さくなることと等しい。上記の式（１）は、手先姿勢の誤差を確率密度分布で表現し、その誤差を修正するのに必要な各関節角の動作量の期待値を求めるものである。

【0036】

最後に、手先姿勢の推定値Ｘｈｅと修正後の手先姿勢Ｘｈｃの確率的な距離の積分から、式（１）を用いて移動マニピュレータの姿勢を評価する（Ｓ３０８）。台車１１０を基準とした座標系での手先姿勢の確率密度分布Ｐ（Ｘｈ）は、次の式（３）から求まる。

【数3】

ここで、Ｐ（ＸＧ）は世界座標系での台車姿勢の確率密度分布であり、台車の初期状態および走行経路に依存して決まる。

【0037】

まず、ある余裕領域Ｚが得られたとする。この余裕Ｚに、Ｘｈｃに関する確率分布を当てはめる。以上の手順を経て、修正後の手先姿勢に関する確率分布Ｐ（Ｘｈｃ）は、次の式（４）から求まる。

【数4】

この確率分布Ｐ（Ｘｈｃ）を式（１）にあてはめることで、姿勢を評価することができる。

【0038】

次に、手先１３０が平行グリッパである場合の余裕領域の例について説明する。条件設定として、２リンク３自由度シリアルリンクマニピュレータが台車１１０上に搭載されていると想定する。各関節には、垂直方向の周りに回転軸がある。また、マニピュレータの根元部には上下機構があり、ロボットがターゲットオブジェクトを把持するときにエンドエフェクタを下げることができる。マニピュレータの先端には平行な２本のグリッパが取付けられている。指の形状は平面であり、それらは対向している。このグリッパを使用して、ボックスなどの長方形の物体をピックアップするタスクを想定する。

【0039】

図５（ａ）（ｂ）は平行グリッパの場合の把持可能領域を説明する図、図５（ｃ）は平行グリッパの場合の余裕領域を説明する図である。

【0040】

まず、把持可能領域Ｚｇを算出する。把持可能領域Ｚｇは、手先１３０と物体の姿勢のずれ量から決める、グリッパを移動・回転せずとも物体を把持できる領域である。

【0041】

手先１３０と物体１４０の姿勢関係が図５（ａ）のようであったとする。把持をおこなうには手先１３０と物体１４０が干渉しないことが必要条件であるため、幾何学的関係から以下の２式が導出できる。

【数5】

これらの２式から、ハンド開閉方向の把持可能領域が図５（ａ）のように算出できる。

【0042】

一方で、手先開閉方向とは垂直な方向へのズレについても考えると、図５（ｂ）より、以下の２式が導出できる。

【数6】

この２式からハンド開閉方向とは垂直な方向の把持可能領域は図５（ｃ）のように計算できる。把持可能領域Ｚｇは、上記２つの領域が重なり合う領域である。

【0043】

一方で、修正領域Ｚｍは、手先１３０の中心と物体１４０の中心を頂点にもつ矩形とする。図５（ｃ）に示すように、修正領域Ｚｍは把持可能領域Ｚｇと平行な四辺で構成する。最後に、把持可能領域Ｚｇと修正領域Ｚｍの共通領域を求め、残った領域を余裕領域Ｚとする。

【0044】

なお、もしｙ０＝０もしくはｙ０＝－ｘ０／ｔａｎθとなった場合は、余裕領域は線分になる。この場合は、２次元平面としてではなく１次元線分として領域を考える。

【0045】

次に、余裕領域の数学的表現方法（確率密度分布を用いた数理モデル）について説明する。図４（ｂ）に示したように余裕領域Ｚの平行四辺形の各辺に内接する楕円を考え、それが二次元正規分布におけるマハラノビス距離（確率的距離）が１の楕円であるとする。そして、確率密度に従ってこの楕円からパーティクルをサンプルし、それを式（９）の計算に用いる。サンプリングまでの流れは次のようである。まず原点を中心とし、θだけ傾いた楕円の式を考える。

【数7】

【0046】

式（９）中のａ，ｂを利用すると、長軸の傾きはθ＝ｔａｎ－１｛－（λ２－ａ）／ｂ｝と表現できる。λ２は式（９）の左辺中央の行列の固有値であり、もう一つの固有値であるλ１とともに次の式で表せる。

【数8】

ここで、ｘ軸、ｙ軸のそれぞれと平行な長軸と短軸をもつ二次元正規分布の共分散行列は、σｘをｘ方向の標準偏差、σｙをｙ方向の標準偏差としたとき、σ^２ｘを一行一列目、σ^２ｙを二行二列目にとる対角行列になる。これをΣとおくと、余裕領域に内接する楕円型の分布の共分散行列Σ′は、以下の式で表される。

【数9】

【0047】

ここで、σｘ＝１／（ｄ√λ２）、σｙ＝１／（ｄ√λ１）である。ｄはマハラノビスの汎距離であり、例えばｄ＝２．４４８とすると、描かれる楕円は９５％等確率偏差楕円となる。この分布からサンプリングし、式（９）の計算に利用する。

【0048】

［グリッド探索に基づく台車姿勢の決定方法］
上述の提案手法により、手先の目標姿勢と台車の到達姿勢を与えた場合に、移動マニピュレータの姿勢を評価することが可能になった。一方で、台車の到達姿勢をどのように決めるかは別途考えなければならない。本実施形態では、非特許文献１の考え方を一部踏襲して、検索ベースの方式を採る。非特許文献１では、台車の姿勢変化にともなう評価値の変化が滑らかであることに着目して効率的に姿勢探索をおこなう手法を提案した。そこでは、ロボットが存在可能な領域をグリッドに分け、各グリッドの中心に台車が存在しうるものとする。

【0049】

図６はグリッドマップを用いた環境表現を説明する図である。手先目標姿勢を実現できる範囲を等間隔のグリッドに分割し、初期位置からそのグリッドへ至ったのちの台車姿勢誤差を予測した、そののちに姿勢評価をおこなう。ただし、すべてのグリッドについて評価をすることは計算量を要するので、非特許文献１では、隣接するグリッドのうち一番評価値の低いグリッドをたどることにより、極値を見つける手法が提案された。

【0050】

手順は次のとおりである。
１）ロボットが存在可能なグリッドの中から、ランダムに一つのグリッドを選ぶ
２）そのグリッドと、隣り合う四方のグリッドについて姿勢評価をおこなう
３）評価値の最もよいグリッドへ移動し、２）の処理をおこなう
もし現在のグリッドが最もよい評価値を持つのであれば、一回の探索は終了である。この流れを、グリッドを何回かランダムに選んでおこない、探索結果の中でもっとも評価値のよかったグリッドを最終的なロボットの立ち位置とする。評価値マップには極値の個数が多くない為、サンプリングの数が少数でも充分である。

【0051】

［評価マップデータベースを用いる台車姿勢の決定方法］
グリッド表現を用いた手法は、実装が容易である。一方で、提案手法の場合は、グリッドを一つ選ぶごとに、台車の移動経路に応じた手先姿勢誤差の分布を計算する必要があり、処理時間がかかることが課題である。すなわち、この計算を省略できれば、高速化の効果は大きいといえる。そこで、評価値のマップをあらかじめ多数生成しておき、現在の状況に合うものを選び出して利用するアプローチをとる。

【0052】

事前処理として、台車姿勢と手先目標姿勢の関係を様々に変えて、それぞれについて評価値のマップを生成する。一方、探索処理では、入力を台車姿勢と手先目標姿勢のペアとして、もっとも現状に合う評価値のマップを探す。その後、そのマップにおいて障害物領域に重なっていないグリッド群のうち、最も評価値が小さく、かつ障害物領域と重ならないグリッドを見つける。このときに使う手順は、上述した1）～3）のとおりである。ただし、ここでは各グリッドの評価値はすでに算出できているため、極値を見つけるための処理時間を少なくすることができる。

【0053】

上記説明したように、本発明においては、（１）相対姿勢誤差の予測値と誤差の許容範囲との確率的な距離の大きさを評価し、（２）ワーク周辺における上記の評価値の最小値を探索する。これにより、ワークと手先間の誤差を最も吸収し易いロボットの姿勢を一意に決定することができる手法を確立した。本発明を用いることにより、移動マニピュレータに用いる位置計測系の要求精度を緩和してコスト低減を図ることができると共に、ロボットの姿勢をティーチングレスで決定することが可能になる。

【0054】

以上、添付図面を参照しながら本発明の好適な実施例について説明したが、本発明は斯かる例に限定されないことは言うまでもない。当業者であれば、特許請求の範囲に記載された範疇内において、各種の変更例または修正例に想到し得ることは明らかであり、それらについても当然に本発明の技術的範囲に属するものと了解される。

【産業上の利用可能性】

【0055】

本発明は、台車にマニピュレータを搭載した移動マニピュレータの姿勢最適化方法として利用することができる。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】