JP6927727B2 - ロボットの制御装置 - Google Patents

Description

本発明は、ロボットの制御技術に関する。

ロボットが構造物等と干渉する可能性がある領域を警戒領域として、移動容易度を判定する技術が提案されている（特許文献１参照）。ロボットを構造物と接触させながら移動させる技術が提案されている（特許文献２参照）。

特許公報第４２７６６２４号 国際特許公開公報 ＷＯ２０１５／０８７５０４

しかし、ロボットが複数の指機構を有するハンドで対象物を把持する際、ハンドに設けられている６軸力センサの出力が平衡状態であっても、当該対象物の把持状態が不明である。６軸力センサの出力が平衡状態である場合、当該出力を受けた制御装置は、例えば図６Ａに示されているように、ロボットのハンド１３がその一対の把持機構Ｆ１およびＦ２の接触面を対象物Ｗに対して全面的に当接させるように当該対象物Ｗを把持していると認識される。

その一方、実際には図６Ｂに示されているように、ロボットのハンド１３がその一対の把持機構Ｆ１およびＦ２の接触面を対象物Ｗに対して部分的にのみ当接させるように当該対象物Ｗを把持している場合がある。このため、対象物座標系（Ｘ_W，Ｙ_W，Ｚ_W）に対するハンド座標系（Ｘ₁，Ｙ₁，Ｚ₁）の相対的な位置および姿勢の認識結果にしたがって、世界座標系における基体座標系（Ｘ₂，Ｙ₂，Ｚ₂）の位置・姿勢（位置および姿勢のうち少なくとも一方を意味する。以下同じ。）が変化するようにロボットの基体１０が動かされると、基体１０の動作が不安定になる可能性がある（図７Ｂ参照）。特に、対象物が手すりまたは梯子の一部などの構造物であり、ロボットがハンドで当該構造物を把持しながら、複数の脚体等の移動機構の動作により基体を移動させる場合、ロボットの全体的なバランスが崩れる可能性がある。

そこで、本発明は、対象物に対するハンド等の機能体の位置・姿勢の認識精度を向上させることにより、基体の位置・姿勢の変化を伴う動作の安定性向上を図り得るようにロボットを制御する装置を提供することを課題とする。

本発明のロボットの制御装置は、基体と、前記基体に対して可動に連結されている第１肢体と、所定の構造物を把持可能であり前記第１肢体に対して可動に連結されているハンドと、前記基体に対して可動に連結されている第２肢体と、前記第１肢体に固定されている撮像装置と、を備えているロボットの制御装置であって、前記ハンドによる前記構造物の所定の把持状態を検知する状態検知要素と、前記撮像装置により取得された撮像画像から、前記構造物の延在態様を表わす、直線成分対またはその延長線の交点である消失点を画像座標系において抽出し、撮像装置座標系において、前記画像座標系の前記消失点の位置を表わす第１基準ベクトルを計算するとともに、ハンド座標系に対する前記撮像装置座標系の位置および姿勢について記憶保持されている関係式に基づいて前記ハンド座標系において、前記画像座標系の前記消失点の位置を表わす第２基準ベクトルを計算する計算要素と、前記第１基準ベクトルおよび前記第２基準ベクトルのなす角度の基準角度に対する偏差の大きさが閾値を超えているときは、前記状態検知要素が検知した前記所定の把持状態が目標状態から乖離しているとして、前記偏差の大きさが前記閾値以下になるように、前記関係式を補正する補正要素と、前記関係式が補正されたときは補正後の前記関係式による前記ハンド座標系に基づく前記ハンドの姿勢制御により前記構造物に対して前記ハンドを把持させるような前記第１肢体および前記ハンドの動作、ならびに、前記基体の位置および姿勢のうち少なくとも一方を変化させるような前記第２肢体の動作を含む前記ロボットの動作を生成する動作生成要素と、を備えていることを特徴とする。

本発明のロボットの制御装置において、前記計算要素が、前記撮像装置により取得された撮像画像から、前記構造体の延在態様を表わす、直線成分対またはその延長線の交点である消失点の撮像装置座標系および機能体座標系のそれぞれにおける位置を表わす第１基準ベクトルおよび第２基準ベクトルを計算することが好ましい。

本発明のロボットの制御装置において、前記計算要素が、前記撮像画像における２つの直線成分の相互の間隔および角度が、前記環境情報により表わされている前記構造物の配置態様によって定まる閾値以下に収まることを要件として、前記２つの直線成分を、前記直線成分対として抽出することが好ましい。

本発明の制御装置によれば、２つの基準ベクトルのなす角度の基準角度に対する偏差の大きさが閾値を超えている場合、機能体と構造物との相互作用状態が目標とする状態から乖離している蓋然性が高い。このため、このような場合、当該偏差の大きさが閾値以下になるように、機能体と構造物との相互作用状態の検知結果から推定される、世界座標系における機能体座標系の姿勢が補正される。これにより、機能体と構造物との相互作用状態が目標とする状態に合致しているという誤った認識結果に基づき、機能体座標系に対して基体座標系の相対的な位置・姿勢が変化するようにロボットの動作が生成される事態が回避され、ロボットの動作の安定性向上が図られる。

本発明の一実施形態としてのロボットの構成に関する説明図。 本発明の一実施形態としての制御装置の構成に関する説明図。 ハンドの位置・姿勢の補正方法に関する説明図。 画像座標系における直線成分の抽出および消失点の算出方法に関する説明図。 基準ベクトルの算出方法に関する説明図。 ハンドによる対象物の第１把持状態に関する説明図。 ハンドによる対象物の第２把持状態に関する説明図。 第１把持状態に基づくロボットの動作態様に関する説明図。 第２把持状態に基づくロボットの動作態様に関する説明図。

（ロボットの構成）
図１に示されている本発明の移動装置の一実施形態としてのロボット１は脚式移動ロボットである。左右の区別のために適宜符号「Ｌ」および「Ｒ」が用いられる。人間と同様に基体１０と、基体１０の上部に設けられた頭部１１と、基体１０の上部左右両側から延設された左右の腕体１２（第１肢体）と、腕体１２の先端部に設けられたハンド１３（手部（機能体））と、基体１０の下部から下方に延設された左右の脚体１４（第２肢体）と、脚体１４の先端部に取り付けられている足平部１５とを備えている。ロボット１は、アクチュエータＭＯＴ（図２参照）から伝達される力によって、人間の肩関節、肘関節、手首関節、股関節、膝関節、足首関節等の複数の関節に相当する複数の関節機構において腕体１２および脚体１４を屈伸運動させることができる。

腕体１２は肩関節機構を介して基体１０に連結された第１腕リンクと、一端が第１腕リンクの端部に肘関節機構を介して連結され、他端が手首関節を介してハンド１３の付根部に連結されている第２腕リンクとを備えている。肩関節機構は、ヨー軸およびピッチ軸のそれぞれの回りの２つの回転自由度を有する。肘関節機構は、ピッチ軸回りの１つの回転自由度を有する。手首関節機構は、ロール軸およびピッチ軸のそれぞれの回りの２つの回転自由度を有する。

腕体１２の先端部には、カメラＣが固定されている。ハンド１３は、手の平部および当該手の平部に対して可動な一または複数の指機構（可動部材）を備え、当該指機構の動作により一もしくは複数の指機構および手の平部の間で、または、複数の指機構の間で対象物を把持することができるように構成されている。

脚体１４は股関節機構を介して基体１０に連結された第１脚リンクと、一端が第１脚リンクの端部に膝関節機構を介して連結され、他端が足首関節を介して足平部１５に連結されている第２脚リンクとを備えている。股関節機構は、ヨー軸、ピッチ軸およびロール軸のそれぞれの回りの３つの回転自由度を有する。膝関節機構は、ピッチ軸回りの１つの回転自由度を有する。足首関節機構は、ピッチ軸およびロール軸のそれぞれ回りの２つの回転自由度を有する。ロボット１は、左右の脚体１４のそれぞれの離床および着床の繰り返しを伴う動きによって自律的に移動することができる。

（制御装置の構成）
図２に示されている制御装置２はロボット１に搭載されているプログラマブルコンピュータまたは電子制御ユニット（ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏ回路等により構成されている。）により構成されている。制御装置２は内界センサ群Ｓ₁および外界センサ群Ｓ₂のそれぞれの出力信号に基づいて種々の状態変数の値を認識し、当該認識結果に基づいて各アクチュエータＭＯＴの動作を制御するように構成されている。

内界センサ群Ｓ₁にはロボット１の位置（重心位置）を測定するためのＧＰＳ測定装置または加速度センサのほか、基体１０の姿勢を測定するためのジャイロセンサ、各関節機構の各軸回りの間接角度等を測定するロータリーエンコーダ、ハンド１３に作用する外力を測定する６軸力センサ等が含まれている。

外界センサ群Ｓ₂には、カメラＣのほか、ロボット１とは別個独立のモーションキャプチャーシステム（図示略）、ボール等のタスク実行に関連する物体の位置軌道を測定するため、頭部１１に搭載されているステレオイメージセンサ、および、基体１０に搭載されている赤外光を用いたアクティブ型センサ等が含まれる。

制御装置２は、記憶装置２０と、状態検知要素２２と、計算要素２４と、補正要素２６と、動作生成要素２８と、を備えている。記憶装置２０は、世界座標系における構造物の配置態様を表わす「環境情報」など、ロボット１の動作生成に必要な情報を記憶保持する。状態検知要素２２は、内界センサ群Ｓ₁を構成する６軸力センサまたは接触センサ（指機構に配置されている。）の出力信号に基づき、ハンド１３と構造物Ｗとの相互作用状態を検知する。計算要素２４は、カメラＣにより取得された撮像画像に基づき、カメラ座標系およびハンド座標系のそれぞれにおける構造体Ｗの延在態様を表わす第１基準ベクトルＵ₁および第２基準ベクトルＵ₂を計算する。補正要素２６は、第１基準ベクトルＵ₁および第２基準ベクトルＵ₂のなす角度φの基準角度φ₀に対する偏差Δφの大きさが閾値εを超えていることを要件として、世界座標系におけるハンド座標系の姿勢を補正するための補正量を計算する。動作生成要素２８は、構造物Ｗに対してハンド１３を相互作用させるような腕体１２（第１肢体）およびハンド１３（機能体）の動作、ならびに、基体１０の位置・姿勢を変化させるような脚体１４（第２肢体）の動作を含むロボット１の動作または歩容を生成する。

単一のプロセッサ（演算処理装置）が当該複数の要素２２〜２８として機能してもよいし、複数のプロセッサ（マルチコアプロセッサ）が相互通信により連携しながら当該複数の要素２２〜２８として機能してもよい。

（作用）
動作生成要素２８により、ハンド１３により手すり等の構造物Ｗを把持する動作が生成され、ロボット１の動作が当該生成動作にしたがって制御される。これにより、ハンド１３が構造物Ｗを把持すると、内界センサ群Ｓ₁を構成する６軸力センサまたは接触センサ（指機構Ｆ₁，Ｆ₂に配置されている。）の出力信号に基づき、ハンド１３による構造物Ｗの把持状態（両者の相互作用状態）が検知される（図３／ＳＴＥＰ０１）。

当該出力信号が平衡状態にある（出力信号の最大、平均または累積時間変化量が判定値以下である）場合、ハンド１３による構造物Ｗの把持状態が、図６Ａに示されている目標の状態であると推定される。すなわち、ハンド１３による構造物Ｗの把持状態の検知結果により、構造物Ｗに対する相対的なハンド１３の位置および姿勢が図６Ａに示されている関係にあると推察される。

外界センサ群Ｓ₂を構成するカメラＣを通じて撮像画像が取得される（図３／ＳＴＥＰ０２）。

撮像画像（またはこれがグレースケール化されることで生成されるグレースケール座像）からエッジ点が抽出される。その上で、当該撮像画像から直線状に並んでいるエッジ点群により構成される一または複数の直線成分が抽出される（図３／ＳＴＥＰ０４）。エッジ点は、画像座標系（ｘ，ｙ）においてｘ方向について輝度値等の画素の物理量が閾値を超えて変化する画素の座標値によりその位置が定まる。直線成分は、ハフ変換等の既知の手法にしたがって抽出される。これにより、例えば図４に示されているように、画像座標系（ｘ，ｙ）において、複数の直線成分Ｌ₁、Ｌ₂、‥Ｌ_k-1、Ｌ_k、‥が抽出される。

複数の直線成分が抽出された場合、直線成分対の有無が判定される（図３／ＳＴＥＰ０６）。２つの直線成分またはこれらのそれぞれの上端からの延長線が交わる場合、当該２つの直線成分が直線成分対として抽出される。記憶装置２０に記憶保持されている環境情報により表わされている手すりなどの構造物Ｗの配置態様に鑑みて、当該構造物Ｗに該当しない蓋然性が高い直線成分は直線成分対を構成しないものとして取り扱われてもよい。２つの直線成分ρ₁＝ｘｃｏｓθ₁＋ｙｓｉｎθ₁およびρ₂＝ｘｃｏｓθ₂＋ｙｓｉｎθ₂について、Δρ＝｜ρ₁−ρ₂｜がΔρ_th以下であり、かつ、Δθ＝｜θ₁−θ₂｜がΔθ_th以下であることを要件として、当該２つの直線成分が直線成分対として抽出されてもよい。「Δρ_th」および「Δθ_th」は、環境情報により表わされている構造物Ｗの配置態様（実空間サイズも含む。）に応じて定められる閾値である。

直線成分対が抽出されなかった場合（図３／ＳＴＥＰ０６‥ＮＯ）、次のフレームの撮像画像の取得以降の処理が繰り返される（図３／ＳＴＥＰ０２参照）。

その一方、直線成分対が抽出された場合（図３／ＳＴＥＰ０６‥ＹＥＳ）、カメラＣの光学中心を原点Ｏ_Cを基準とする、カメラ座標系（Ｘ_C，Ｙ_C，Ｚ_C）における消失点Ｑの位置を表わす第１基準ベクトルＱ₁が算出される（図３／ＳＴＥＰ０８）。

具体的には、画像座標系における直線成分対（ρ₁＝ｘｃｏｓθ₁＋ｙｓｉｎθ₁およびρ₂＝ｘｃｏｓθ₂＋ｙｓｉｎθ₂）の交点位置が消失点Ｑの座標値（ｘ_Q，ｙ_Q）として関係式（０１）により表わされる。

ｘ_Q＝（ρ₂ｓｉｎθ₁−ρ₁ｓｉｎθ₂）／（ｓｉｎθ₁ｃｏｓθ₂−ｃｏｓθ₁ｓｉｎθ₂），
ｙ_Q＝｛（ρ₁−ｃｏｓθ₁）／ｓｉｎθ₁｝ｘ_q ‥（０１）。

これにより、例えば図４に示されているように、一対の直線成分Ｌ₁およびＬ₂の交点Ｑが定まる。第１基準ベクトルＵ₁＝^t（ｕ_1X，ｕ_1Y，ｕ_1Z）、カメラＣの焦点距離ｆ、および、画像座標系（ｘ，ｙ）における消失点Ｑ＝（ｘ_Q，ｙ_Q）の関係は、一般的な透視投影モデルにしたがって関係式（０２）により表現される（図５参照）。

ｘ_Q＝ｆ（ｕ_1X／ｕ_1Z）、ｙ_Q＝ｆ（ｕ_1Y／ｕ_1Z） ‥（０２）。

よって、カメラ座標系の原点Ｏ_Cを基準とした、画像座標系における消失点Ｑの位置を表わす第１基準ベクトルＵ₁が関係式（０３）にしたがって算出される。

Ｕ₁＝^t（ｘ_q，ｙ_q，ｆ）‥（０３）。

続いて、ハンド座標系（Ｘ₁，Ｙ₁，Ｚ₁）において、画像座標系における消失点Ｑの位置を表わす第２基準ベクトルＵ₂が算出される（図３／ＳＴＥＰ１０）。ハンド座標系に対するカメラ座標系の位置および姿勢は、世界座標系における並進ベクトルＴおよび回転行列Ｒを用いて関係式（０４）にしたがって表現される。並進ベクトルＴおよび回転行列Ｒは、カメラＣの腕部１２の先端部に対する取り付け態様のキャリブレーションにより既知であって制御装置２を構成する記憶装置２０に記憶保持されている。

Ｐ_C＝Ｔ＋Ｒ・Ｐ₁ ‥（０４）。

第２基準ベクトルＵ₂は関係式（０５）にしたがって算出される。

Ｕ₂＝Ｔ＋Ｒ・Ｐ₁＋Ｕ₁ ‥（０５）。

さらに、第１基準ベクトルＵ₁および第２基準ベクトルＵ₂のなす角度φが計算される（図３／ＳＴＥＰ１２）。当該角度φは、関係式（０６）により表わされる。

φ＝ａｒｃｃｏｓ（Ｕ₁・Ｕ₂／｜Ｕ₁・Ｕ₂｜） ‥（０６）。

角度φの基準角度φ₀に対する偏差Δφ＝φ−φ₀の大きさが閾値εを超えているか否かが判定される（図３／ＳＴＥＰ１４）。閾値εは、当該偏差Δφの大きさがこれを超えると、ハンド座標系を基準として基体座標系（Ｘ₂，Ｙ₂，Ｚ₂）の位置および姿勢が変化した場合に、ロボット１の動作が不安定になる可能性があるという観点から定められている。

偏差Δφの大きさが閾値ε以下であると判定された場合（図３／ＳＴＥＰ１４‥ＮＯ）、次のフレームの撮像画像の取得以降の処理が繰り返される（図３／ＳＴＥＰ０２参照）。

偏差Δφの大きさが閾値εを超えていると判定された場合（図３／ＳＴＥＰ１４‥ＹＥＳ）、世界座標系におけるハンド座標系の姿勢を表わす回転行列Ｒ₁を定めるピッチ回転角度θ_1xおよびヨー回転角度θ_1zのうち少なくとも一方が、偏差Δφの大きさが閾値ε以下になるように決定される（図３／ＳＴＥＰ１６）。

世界座標系におけるハンド座標系の姿勢が当該補正量の分だけ補正される。この補正は、実空間におけるハンド１３の姿勢が変更されずに、世界座標系におけるハンド座標系の姿勢が計算上補正されること、および、世界座標系におけるハンド座標系の姿勢が計算上補正され、世界座標系におけるハンド座標系の当該補正後の姿勢に合致するように実空間におけるハンド１３の姿勢が変更されることの両方を包含する概念である。そして、世界座標系におけるハンド座標系の位置および姿勢を基準として、各関節機構の関節角度に基づき、ロボット１の運動学モデルにしたがって基体座標系の位置および姿勢も目標時間変化態様を含む、ロボット１の動作が生成される。

ロボット１とその環境との関係が未知である状態またはその認識精度が低い状態では、ハンド１３による構造物Ｗの把持状態（両者の相互作用状態）が検知された場合、ハンド座標系の位置・姿勢を基準として、世界座標系における構造物の位置・姿勢を表わす「環境情報」が前記補正量の分だけ補正されてもよい。

（効果）
本発明の一実施形態としてのロボット１の制御装置２によれば、２つの基準ベクトルＵ₁およびＵ₂のなす角度φ（関係式（０６）参照）の基準角度φ₀に対する偏差Δφの大きさが閾値εを超えている場合、状態検知要素２２により検知されたハンド１３（機能体）と構造物Ｗとの相互作用状態が目標とする状態から乖離している蓋然性が高い（図６Ｂ参照）。このため、このような場合、当該偏差Δφの大きさが閾値ε以下になるように世界座標系におけるハンド座標系の姿勢が補正される（図３／ＳＴＥＰ１４‥ＹＥＳ→ＳＴＥＰ１６参照）。これにより、ハンド１３と構造物Ｗとの相互作用状態が目標とする状態に合致しているという誤った認識結果に基づき、ハンド座標系に対して基体座標系の相対的な位置・姿勢が変化するようにロボット１の動作が生成される事態（図７Ｂ参照）が回避され、ロボット１の動作の安定性向上が図られる（図７Ａ参照）。

（本発明の他の実施形態）
前記実施形態では、直線成分対の交点として消失点Ｑが定められたが（図４参照）、環境情報に鑑みて構造物Ｗの延在態様を表わす蓋然性（環境情報で表わされる世界座標系における直線または曲線との近似度）が基準値以上である直線成分または曲線成分と、地平線に相当する線分との交点が消失点Ｑとして定められてもよく、当該直線成分または曲線成分の画像座標系における上端点が消失点Ｑとして定められてもよい。近接するまたは一方の直線成分が共通する、異なる複数の直線成分対が存在する場合、各直線線分対の交点の重心が消失点Ｑとして定められてもよい。当該重心の算定に際して、各直線成分対が構造物Ｗの延在態様を表わしている尤度の高低に応じた重みが各交点に付けられてもよい。

１‥ロボット、２‥制御装置、１０‥基体、１２‥腕体（第１肢体）、１３‥ハンド、１４‥脚体（第２肢体）、２０‥記憶装置、２２‥状態検知要素、２４‥計算要素、２６‥補正要素、２８‥動作生成要素、Ｃ‥カメラ（撮像装置）、Ｆ１‥第１指機構、Ｆ２‥第２指機構、Ｓ₁‥内界センサ群、Ｓ₂‥外界センサ群、Ｗ‥構造物。

Claims (2)

1. 基体と、
   前記基体に対して可動に連結されている第１肢体と、
   所定の構造物を把持可能であり前記第１肢体に対して可動に連結されているハンドと、
   前記基体に対して可動に連結されている第２肢体と、
   前記第１肢体に固定されている撮像装置と、を備えているロボットの制御装置であって、
   前記ハンドによる前記構造物の所定の把持状態を検知する状態検知要素と、
   前記撮像装置により取得された撮像画像から、前記構造物の延在態様を表わす、直線成分対またはその延長線の交点である消失点を画像座標系において抽出し、撮像装置座標系において、前記画像座標系の前記消失点の位置を表わす第１基準ベクトルを計算するとともに、ハンド座標系に対する前記撮像装置座標系の位置および姿勢について記憶保持されている関係式に基づいて前記ハンド座標系において、前記画像座標系の前記消失点の位置を表わす第２基準ベクトルを計算する計算要素と、
   前記第１基準ベクトルおよび前記第２基準ベクトルのなす角度の基準角度に対する偏差の大きさが閾値を超えているときは、前記状態検知要素が検知した前記所定の把持状態が目標状態から乖離しているとして、前記偏差の大きさが前記閾値以下になるように、前記関係式を補正する補正要素と、
   前記関係式が補正されたときは補正後の前記関係式による前記ハンド座標系に基づく前記ハンドの姿勢制御により前記構造物に対して前記ハンドを把持させるような前記第１肢体および前記ハンドの動作、ならびに、前記基体の位置および姿勢のうち少なくとも一方を変化させるような前記第２肢体の動作を含む前記ロボットの動作を生成する動作生成要素と、を備えていることを特徴とするロボットの制御装置。
2. 請求項１記載のロボットの制御装置において、
   さらに、世界座標系における構造物の配置態様を表わす環境情報を記憶する記憶装置を備え、
   前記計算要素が、前記撮像画像における２つの直線成分の相互の間隔および角度が、前記環境情報により表わされている前記構造物の配置態様によって定まる閾値以下に収まることを要件として、前記２つの直線成分を、前記直線成分対として抽出することを特徴とするロボットの制御装置。

Images