知財求人 - 知財ポータルサイト「IP Force」
特開2022-161572多軸ロボット用の制御装置、ロボットシステム、および制御プログラム
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022161572
(43)【公開日】2022-10-21
(54)【発明の名称】多軸ロボット用の制御装置、ロボットシステム、および制御プログラム
(51)【国際特許分類】
B25J 19/06 20060101AFI20221014BHJP
【ＦＩ】
B25J19/06
【審査請求】未請求
【請求項の数】9
【出願形態】ＯＬ
(21)【出願番号】P 2021066488
(22)【出願日】2021-04-09
(71)【出願人】
【識別番号】000148357
【氏名又は名称】株式会社前川製作所
(74)【代理人】
【識別番号】110000785
【氏名又は名称】ＳＳＩＰ弁理士法人
(72)【発明者】
【氏名】木村 憲一郎
(72)【発明者】
【氏名】嶋田 圭太郎
(72)【発明者】
【氏名】並木 明夫
【テーマコード（参考）】
3C707
【Ｆターム（参考）】
3C707BS10
3C707KS36
3C707KT01
3C707KT06
3C707LS15
3C707MS08
3C707MS14
3C707MS15
3C707NS02
3C707NS26
(57)【要約】
【課題】多軸ロボットと障害物との衝突を回避しながら多軸ロボットの位置姿勢を変えることができる多軸ロボット用の制御装置、ロボットシステム、および制御プログラムを提供する。
【解決手段】多軸ロボット用の制御装置２０は、位置姿勢補正部２２と姿勢制御部２３を備える。位置姿勢補正部２２は、多軸ロボット３０の先端３８の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、多軸ロボット３０と障害物との距離が閾値以下となる位置姿勢について、距離が閾値を超える多軸ロボット３０の先端３８の補正中間位置姿勢を求めるように構成される。姿勢制御部２３は、多軸ロボット３０の先端３８の位置姿勢が、位置姿勢補正部２２によって求められた補正中間位置姿勢を経て最終位置姿勢まで変化するよう、多軸ロボット３０を制御するように構成される。
【選択図】図１
【解決手段】多軸ロボット用の制御装置２０は、位置姿勢補正部２２と姿勢制御部２３を備える。位置姿勢補正部２２は、多軸ロボット３０の先端３８の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、多軸ロボット３０と障害物との距離が閾値以下となる位置姿勢について、距離が閾値を超える多軸ロボット３０の先端３８の補正中間位置姿勢を求めるように構成される。姿勢制御部２３は、多軸ロボット３０の先端３８の位置姿勢が、位置姿勢補正部２２によって求められた補正中間位置姿勢を経て最終位置姿勢まで変化するよう、多軸ロボット３０を制御するように構成される。
【選択図】図１
【特許請求の範囲】
【請求項1】
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正部と、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正部によって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御部と
を備える多軸ロボット用の制御装置。
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正部によって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御部と
を備える多軸ロボット用の制御装置。
【請求項2】
前記位置姿勢補正部は、前記多軸ロボットの前記先端の前記位置姿勢を示す位置姿勢パラメータおよび前記距離を示す距離パラメータの値が許容範囲内に収まるような前記多軸ロボットの各軸状態量を探索するように構成された請求項１に記載の多軸ロボット用の制御装置。
【請求項3】
前記位置姿勢補正部は、
前記各軸状態量に対する１以上の前記位置姿勢パラメータの感度を少なくとも示す第１行列を取得するための第１行列取得部と、
前記各軸状態量に対する前記距離パラメータの感度を少なくとも示す第２行列を取得するための第２行列取得部と、
前記各軸状態量の候補値に対応する前記１以上の前記位置姿勢パラメータの目標値に対する第１偏差と、前記各軸状態量の前記候補値に対応する前記距離パラメータの目標値に対する第２偏差と、前記第１行列および前記第２行列とを用いて、前記各軸状態量の前記候補値を更新するための更新部と、
を含む請求項２に記載の多軸ロボット用の制御装置。
前記各軸状態量に対する１以上の前記位置姿勢パラメータの感度を少なくとも示す第１行列を取得するための第１行列取得部と、
前記各軸状態量に対する前記距離パラメータの感度を少なくとも示す第２行列を取得するための第２行列取得部と、
前記各軸状態量の候補値に対応する前記１以上の前記位置姿勢パラメータの目標値に対する第１偏差と、前記各軸状態量の前記候補値に対応する前記距離パラメータの目標値に対する第２偏差と、前記第１行列および前記第２行列とを用いて、前記各軸状態量の前記候補値を更新するための更新部と、
を含む請求項２に記載の多軸ロボット用の制御装置。
【請求項4】
前記更新部は、前記各軸状態量の最新の前記候補値に、式（１）によって示されるΔｑを加えることによって前記候補値を更新し、
前記式（１）において、
Δｘ及びＪの組み合わせが、前記第１偏差を要素として含む第１ベクトルおよび前記第１行列の組み合わせ、または、前記第２偏差を要素として含む第２ベクトルおよび前記第２行列の組み合わせの一方であり、
ΔｘＣおよびＪＣの組み合わせが、前記第１ベクトルおよび前記第１行列の組み合わせ、または、前記第２ベクトルおよび前記第２行列の組み合わせの他方である請求項３に記載の多軸ロボット用の制御装置。
Δｑ＝Ｊ＋Δｘ＋（ＪＣ（Ｉ－Ｊ＋Ｊ））＋（ΔｘＣ－ＪＣＪ＋Δｘ）・・・式（１）
前記式（１）において、
Δｘ及びＪの組み合わせが、前記第１偏差を要素として含む第１ベクトルおよび前記第１行列の組み合わせ、または、前記第２偏差を要素として含む第２ベクトルおよび前記第２行列の組み合わせの一方であり、
ΔｘＣおよびＪＣの組み合わせが、前記第１ベクトルおよび前記第１行列の組み合わせ、または、前記第２ベクトルおよび前記第２行列の組み合わせの他方である請求項３に記載の多軸ロボット用の制御装置。
Δｑ＝Ｊ＋Δｘ＋（ＪＣ（Ｉ－Ｊ＋Ｊ））＋（ΔｘＣ－ＪＣＪ＋Δｘ）・・・式（１）
【請求項5】
ΔｘおよびＪの組み合わせが、前記第１偏差を要素として含む前記第１ベクトルおよび前記第１行列の組み合わせであり、
ΔｘＣおよびＪＣの組み合わせが、複数の前記位置姿勢パラメータのうち前記１以上の前記位置姿勢パラメータを除く他の前記位置姿勢パラメータと目標値との偏差と前記第２偏差とを要素として含む前記第２ベクトルおよび前記第２行列の組み合わせである請求項４に記載の多軸ロボット用の制御装置。
ΔｘＣおよびＪＣの組み合わせが、複数の前記位置姿勢パラメータのうち前記１以上の前記位置姿勢パラメータを除く他の前記位置姿勢パラメータと目標値との偏差と前記第２偏差とを要素として含む前記第２ベクトルおよび前記第２行列の組み合わせである請求項４に記載の多軸ロボット用の制御装置。
【請求項6】
前記位置姿勢補正部は、前記多軸ロボットの少なくとも一部を包含するモデルを仮想的に設定し、前記モデルと前記障害物との距離を前記多軸ロボットと前記障害物との前記距離として扱い、前記各軸操作量を求める請求項１から５のいずれかに記載の多軸ロボット用の制御装置。
【請求項7】
前記モデルは、
前記多軸ロボットのリンクの軸方向に沿った円筒部と、
前記円筒部の両端に接続される２つの半球部と、
を含む請求項６に記載の多軸ロボット用の制御装置。
前記多軸ロボットのリンクの軸方向に沿った円筒部と、
前記円筒部の両端に接続される２つの半球部と、
を含む請求項６に記載の多軸ロボット用の制御装置。
【請求項8】
請求項１から７のいずれかに記載の多軸ロボット用の制御装置と、
前記多軸ロボットと
を備える多軸ロボット用のロボットシステム。
前記多軸ロボットと
を備える多軸ロボット用のロボットシステム。
【請求項9】
コンピュータに、
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正ステップと、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正ステップによって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御ステップと
を実行させる多軸ロボット用の制御プログラム。
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正ステップと、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正ステップによって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御ステップと
を実行させる多軸ロボット用の制御プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、多軸ロボット用の制御装置、ロボットシステム、および制御プログラムに関する。
【背景技術】
【0002】
従来、多軸ロボットの位置姿勢が目標の位置姿勢に変化できるかを判定する多軸ロボット用の制御装置が知られている。例えば、特許文献１で開示される多軸ロボット用の制御装置では、多軸ロボットの位置姿勢が変わるときにおける多軸ロボットと障害物との距離の変化を予め算出する。算出結果に基づき、多軸ロボットと障害物との衝突が発生するか判定される。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２００８－２７９５２４号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
しかし、特許文献１では、多軸ロボットが障害物との衝突を回避するための経路を特定するための構成が開示されていない。
【0005】
本開示の目的は、多軸ロボットと障害物との衝突を回避しながら多軸ロボットの位置姿勢を変えることができる多軸ロボット用の制御装置、ロボットシステム、および制御プログラムを提供することである。
【課題を解決するための手段】
【0006】
本発明の少なくとも一実施形態に係る多軸ロボット用の制御装置は、
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正部と、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正部によって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御部と
を備える。
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正部と、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正部によって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御部と
を備える。
【0007】
本発明の少なくとも一実施形態に係る多軸ロボット用のロボットシステムは、
前記多軸ロボット用の制御装置と、
前記多軸ロボットと
を備える。
前記多軸ロボット用の制御装置と、
前記多軸ロボットと
を備える。
【0008】
本発明の少なくとも一実施形態に係る多軸ロボット用の制御プログラムは、
コンピュータに、
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正ステップと、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正ステップによって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御ステップと
を実行させる。
コンピュータに、
多軸ロボットの先端の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、前記多軸ロボットと障害物との距離が閾値以下となる前記位置姿勢について、前記距離が前記閾値を超える前記多軸ロボットの前記先端の補正中間位置姿勢を求めるための位置姿勢補正ステップと、
前記多軸ロボットの前記先端の前記位置姿勢が、前記位置姿勢補正ステップによって求められた前記補正中間位置姿勢を経て前記最終位置姿勢まで変化するよう、前記多軸ロボットを制御するための位置姿勢制御ステップと
を実行させる。
【発明の効果】
【0009】
本開示によれば、多軸ロボットと障害物との衝突を回避しながら多軸ロボットの位置姿勢を変えることができる多軸ロボット用の制御装置、ロボットシステム、および制御プログラムを提供できる。
【図面の簡単な説明】
【0010】
【図1】一実施形態に係るロボットシステム１を示す説明図である。
【図2】一実施形態に係る、位置姿勢補正部２２によって補正される中間点Ｃを示す図である。
【図3】一実施形態に係る位置姿勢補正部２２が補正中間位置姿勢を探索する過程を示す概念図である。
【図4】一実施形態に係る位置姿勢補正部２２の構成を示す図である。
【図5】一実施形態に係る、式（１）に基づき各軸状態量の更新量Δｑを求める原理を示す概念図である。
【図6】一実施形態に係る多軸ロボット３０Ａ、３０Ｂに設定されるモデル５０を説明する図である。
【図7】一実施形態に係る距離パラメータの算出方法を示す図である。
【図8】一実施形態に係るロボットシステム１の電気的構成を示すブロック図である。
【図9】一実施形態に係るロボット制御処理を示すフローチャートである。
【図10】一実施形態に係る位置姿勢補正処理を示すフローチャートである。
【発明を実施するための形態】
【0011】
以下、添付図面を参照して本発明の幾つかの実施形態について説明する。ただし、実施形態として記載され又は図面に示されている構成部品の寸法、材質、形状、その相対的配置等は、本発明の範囲をこれに限定する趣旨ではなく、単なる説明例にすぎない。
例えば、「ある方向に」、「ある方向に沿って」、「平行」、「直交」、「中心」、「同心」或いは「同軸」等の相対的或いは絶対的な配置を表す表現は、厳密にそのような配置を表すのみならず、公差、若しくは、同じ機能が得られる程度の角度や距離をもって相対的に変位している状態も表すものとする。
例えば、「同一」、「等しい」及び「均質」等の物事が等しい状態であることを表す表現は、厳密に等しい状態を表すのみならず、公差、若しくは、同じ機能が得られる程度の差が存在している状態も表すものとする。
例えば、四角形状や円筒形状等の形状を表す表現は、幾何学的に厳密な意味での四角形状や円筒形状等の形状を表すのみならず、同じ効果が得られる範囲で、凹凸部や面取り部等を含む形状も表すものとする。
一方、一つの構成要素を「備える」、「具える」、「具備する」、「含む」、又は「有する」という表現は、他の構成要素の存在を除外する排他的な表現ではない。
例えば、「ある方向に」、「ある方向に沿って」、「平行」、「直交」、「中心」、「同心」或いは「同軸」等の相対的或いは絶対的な配置を表す表現は、厳密にそのような配置を表すのみならず、公差、若しくは、同じ機能が得られる程度の角度や距離をもって相対的に変位している状態も表すものとする。
例えば、「同一」、「等しい」及び「均質」等の物事が等しい状態であることを表す表現は、厳密に等しい状態を表すのみならず、公差、若しくは、同じ機能が得られる程度の差が存在している状態も表すものとする。
例えば、四角形状や円筒形状等の形状を表す表現は、幾何学的に厳密な意味での四角形状や円筒形状等の形状を表すのみならず、同じ効果が得られる範囲で、凹凸部や面取り部等を含む形状も表すものとする。
一方、一つの構成要素を「備える」、「具える」、「具備する」、「含む」、又は「有する」という表現は、他の構成要素の存在を除外する排他的な表現ではない。
【0012】
図１は、一実施形態に係る多軸ロボット用のロボットシステム１（以下、単に、「ロボットシステム１」ともいう）を示す図である。
【0013】
一実施形態のロボットシステム１は、多軸ロボット３０と、多軸ロボット用の制御装置２０（以下、単に「制御装置２０」ともいう）とを備える。一実施形態では、制御装置２０が多軸ロボット３０の先端３８の位置姿勢を制御することで、多軸ロボット３０に装着されたツール３９は目標の移動経路３３に沿って移動する。これにより、多軸ロボット３０はツール３９を用いてワーク５に作業を実行する。ワーク５に実行される作業は一例として、切断、把持、持ち上げ、押圧、穴あけ、撮影、送風、吸引、または放水などである。
一実施形態のワーク５は、例えばベルトコンベアなどの運搬装置７によって搬送される。運搬されたワーク５は撮影装置８によって撮影され、撮影装置８の撮影画像に基づき、移動経路３３は設定される。他の実施形態では、ワーク５が人によって運搬されてもよい。また、撮影装置８は設けられなくてもよい。
ワーク５は、作業が実行される物体であれば何であってもよい。例えば、ワーク５は、食品、日用品、工業製品の部品、または化学材料である。一実施形態のワーク５は、農産物、畜産物、または水産物などの食品である。図１で例示されるワーク５は生鮮肉である（図２も同様）。
なお、一実施形態の制御装置２０は、ロボットコントローラおよびパーソナルコンピュータを構成要素として含む。制御装置２０は、パーソナルコンピュータを含まなくてもよい。
一実施形態のワーク５は、例えばベルトコンベアなどの運搬装置７によって搬送される。運搬されたワーク５は撮影装置８によって撮影され、撮影装置８の撮影画像に基づき、移動経路３３は設定される。他の実施形態では、ワーク５が人によって運搬されてもよい。また、撮影装置８は設けられなくてもよい。
ワーク５は、作業が実行される物体であれば何であってもよい。例えば、ワーク５は、食品、日用品、工業製品の部品、または化学材料である。一実施形態のワーク５は、農産物、畜産物、または水産物などの食品である。図１で例示されるワーク５は生鮮肉である（図２も同様）。
なお、一実施形態の制御装置２０は、ロボットコントローラおよびパーソナルコンピュータを構成要素として含む。制御装置２０は、パーソナルコンピュータを含まなくてもよい。
【0014】
一実施形態の多軸ロボット３０は複数の関節軸を備える。各々の関節軸は、１自由度を有する回転関節または直動関節である。一実施形態では、各々の関節軸の状態量である各軸状態量（関節変数）は、関節軸の角度または軸方向位置によって表される。これらの各軸状態量が変化することで、多軸ロボット３０の先端３８の位置姿勢は変化する。先端３８は、最も先端側にある多軸ロボット３０のリンク３５の先端であってもよいし、該リンク３５に装着されたツール３９の先端であってもよい。
上記の各軸状態量は、各々の関節軸の状態量を要素として含む軸状態量ベクトルｑによって表される。例えば、多軸ロボット３０が有する関節軸が６本である場合、各軸状態量ベクトルｑは６つの各軸状態量ｑ１～ｑ６を要素として含む列ベクトルである。多軸ロボット３０の関節軸の本数は６本に代えて、例えば、５本、または７本以上であってもよい。
上記の各軸状態量は、各々の関節軸の状態量を要素として含む軸状態量ベクトルｑによって表される。例えば、多軸ロボット３０が有する関節軸が６本である場合、各軸状態量ベクトルｑは６つの各軸状態量ｑ１～ｑ６を要素として含む列ベクトルである。多軸ロボット３０の関節軸の本数は６本に代えて、例えば、５本、または７本以上であってもよい。
【0015】
先端３８の位置姿勢は、位置姿勢パラメータを要素として含む位置姿勢ベクトルｐによって表される。
一実施形態では、位置姿勢パラメータは６つである。より具体的には、６つの位置姿勢パラメータは、先端３８の位置を示す３つの位置パラメータ（Ｘ、Ｙ、およびＺ）と、先端３８の姿勢を示す３つの姿勢パラメータ（ＲＸ、ＲＹ、およびＲＺ）とに分類される。一実施形態では、これらのパラメータが、任意に設定可能な基準座標系Σ０において示される。
一実施形態では、位置姿勢パラメータは６つである。より具体的には、６つの位置姿勢パラメータは、先端３８の位置を示す３つの位置パラメータ（Ｘ、Ｙ、およびＺ）と、先端３８の姿勢を示す３つの姿勢パラメータ（ＲＸ、ＲＹ、およびＲＺ）とに分類される。一実施形態では、これらのパラメータが、任意に設定可能な基準座標系Σ０において示される。
【0016】
多軸ロボット３０の位置姿勢ベクトルｐと各軸状態量ベクトルｑとの関係は、以下の式（２）によって規定される。
ｐ＝ｆｒ（ｑ） ・・・式（２）
ｆｒは関節角度から位置姿勢ベクトルへと変換する関数である。具体的に、多軸ロボット３０の関節軸の個数（即ち関節の個数）をｎとした場合、関数ｆｐと用いて以下の式（２Ａ）ようにあらわされる。
ｐ＝ｆｐ（０Ｔｎ） ・・・式（２Ａ）
０Ｔｎは同次変換行列であり、基準座標系Σ０における座標と、多軸ロボット３０の最も先端側にあるｎ番目の関節の座標系Σｎとの関係を規定する。基準座標系Σ０は、多軸ロボット３０の用途に応じて任意に設定できる。一実施形態の基準座標系Σ０は、運搬装置７による運搬経路上に設定される。他の実施形態では、基準座標系Σ０は、例えば台座９などに設定されてもよい。上述した任意の基準座標系Σ０における座標は、ワールド座標と呼ばれることもある。
０Ｔｎは式（３）によって規定される。
０Ｔｎ＝０Ｔ１ １Ｔ２・・・ｎ―１Ｔｎ ・・・・式（３）
式（３）において、０Ｔ１は、基準座標系Σ０を、最も基端側にある関節の座標系Σ１に変換するための同次変換行列である。１Ｔ２は、座標系Σ１を、基端側から２番目の関節の座標系Σ２に変換するための同次変換行列である。このように、式（３）の右辺で示される各々の同次変換行列は、隣り合う２つの関節の座標系の関係を規定する。
一実施形態では、式（３）の右辺で示される各々の同次変換行列は、４行４列の行列である。各同次変換行列には、３行３列の回転行列と、着目される２つの座標系の位置関係を示す１行３列の並進ベクトルが要素として含まれる。例えば、同次変換行列ｎ―１Ｔｎは以下の式（４）で表される。
式（４）において、Ｒ１１、Ｒ１２、Ｒ１３、Ｒ２１、Ｒ２２、Ｒ２３、Ｒ３１、Ｒ３２、およびＲ３３は、座標系Σｎ―１を座標系Σｎに一致させるために座標系Σｎ―１を回転させる角度を示す３行３列の回転行列である。また、Ｘ、Ｙ、およびＺは、座標系Σｎ―１を座標系Σｎに一致させるために座標系Σｎ―１を並進させる並進ベクトルである。
上記のｎ―１Ｔｎは、ｎ番目の関節とｎ－１番目の関節との位置関係、および両関節の機械的構造により定まる。つまり、式（２Ａ）、式（３）で同次変換行列として示される０Ｔｎは、多軸ロボット３０の機械的構造によって定まる行列であり、予め特定することができる行列である。従って、各軸状態量ベクトルｑが判れば、式（２）に基づき先端３８の位置姿勢ベクトルｐは特定できる。反対に、先端３８の位置姿勢ベクトルｐが判れば、式（２）の変形式である以下の式（５）に基づき、多軸ロボット３０の各軸状態量ベクトルｑは特定できる。
ｑ＝ ｆｒ－１（ｐ） ・・・式（５）
ｐ＝ｆｒ（ｑ） ・・・式（２）
ｆｒは関節角度から位置姿勢ベクトルへと変換する関数である。具体的に、多軸ロボット３０の関節軸の個数（即ち関節の個数）をｎとした場合、関数ｆｐと用いて以下の式（２Ａ）ようにあらわされる。
ｐ＝ｆｐ（０Ｔｎ） ・・・式（２Ａ）
０Ｔｎは同次変換行列であり、基準座標系Σ０における座標と、多軸ロボット３０の最も先端側にあるｎ番目の関節の座標系Σｎとの関係を規定する。基準座標系Σ０は、多軸ロボット３０の用途に応じて任意に設定できる。一実施形態の基準座標系Σ０は、運搬装置７による運搬経路上に設定される。他の実施形態では、基準座標系Σ０は、例えば台座９などに設定されてもよい。上述した任意の基準座標系Σ０における座標は、ワールド座標と呼ばれることもある。
０Ｔｎは式（３）によって規定される。
０Ｔｎ＝０Ｔ１ １Ｔ２・・・ｎ―１Ｔｎ ・・・・式（３）
式（３）において、０Ｔ１は、基準座標系Σ０を、最も基端側にある関節の座標系Σ１に変換するための同次変換行列である。１Ｔ２は、座標系Σ１を、基端側から２番目の関節の座標系Σ２に変換するための同次変換行列である。このように、式（３）の右辺で示される各々の同次変換行列は、隣り合う２つの関節の座標系の関係を規定する。
一実施形態では、式（３）の右辺で示される各々の同次変換行列は、４行４列の行列である。各同次変換行列には、３行３列の回転行列と、着目される２つの座標系の位置関係を示す１行３列の並進ベクトルが要素として含まれる。例えば、同次変換行列ｎ―１Ｔｎは以下の式（４）で表される。
【数1】
上記のｎ―１Ｔｎは、ｎ番目の関節とｎ－１番目の関節との位置関係、および両関節の機械的構造により定まる。つまり、式（２Ａ）、式（３）で同次変換行列として示される０Ｔｎは、多軸ロボット３０の機械的構造によって定まる行列であり、予め特定することができる行列である。従って、各軸状態量ベクトルｑが判れば、式（２）に基づき先端３８の位置姿勢ベクトルｐは特定できる。反対に、先端３８の位置姿勢ベクトルｐが判れば、式（２）の変形式である以下の式（５）に基づき、多軸ロボット３０の各軸状態量ベクトルｑは特定できる。
ｑ＝ ｆｒ－１（ｐ） ・・・式（５）
【0017】
一実施形態の多軸ロボット３０は、多軸ロボット３０Ａ、３０Ｂを含む。多軸ロボット３０Ａに装着されるツール３９Ａはナイフである。多軸ロボット３０Ｂに装着されるツール３９Ｂはクランパである。一実施形態では、制御装置２０の制御によって多軸ロボット３０Ａ、３０Ｂが作動することにより、ワーク５は把持されて切断される。一実施形態では、多軸ロボット３０Ａ、３０Ｂは互いに同期せずに作動する。他の実施形態では、多軸ロボット３０Ａ、３０Ｂは互いに同期して作動してもよい。
【0018】
一実施形態では、多軸ロボット３０Ａ、３０Ｂの可動領域の各々には障害物が含まれる。従って、多軸ロボット３０Ａ、３０Ｂの各々の先端３８の位置姿勢は、多軸ロボット３０Ａ、３０Ｂと障害物との衝突を回避しながら変化する必要がある。
一実施形態において、多軸ロボット３０Ａ、３０Ｂの可動領域は互いに重複する。従って、多軸ロボット３０Ｂは多軸ロボット３０Ａの障害物である。
なお、他の実施形態では、運搬装置７などのロボットシステム１の構成要素が多軸ロボット３０の障害物であってもよいし、その他の物体が障害物であってもよい。
一実施形態において、多軸ロボット３０Ａ、３０Ｂの可動領域は互いに重複する。従って、多軸ロボット３０Ｂは多軸ロボット３０Ａの障害物である。
なお、他の実施形態では、運搬装置７などのロボットシステム１の構成要素が多軸ロボット３０の障害物であってもよいし、その他の物体が障害物であってもよい。
【0019】
一実施形態に係る多軸ロボット３０と障害物との距離は、少なくとも１つの距離パラメータを含む距離ベクトルによって表される。距離ベクトルは一例として列ベクトルである。例えば、距離パラメータの個数が２つのとき、距離ベクトルの要素は、ｄ１およびｄ２として表される。図１では１つの距離パラメータが符号ｄｍによって図示される。
該距離ベクトルに含まれる少なくとも１つの距離パラメータが閾値以下である場合、多軸ロボット３０は障害物と衝突する可能性が高い。一実施形態の閾値は、各々の距離パラメータに対応付けて設定される０以上の値である。閾値は安全率を反映した正の値であってもよい。
該距離ベクトルに含まれる少なくとも１つの距離パラメータが閾値以下である場合、多軸ロボット３０は障害物と衝突する可能性が高い。一実施形態の閾値は、各々の距離パラメータに対応付けて設定される０以上の値である。閾値は安全率を反映した正の値であってもよい。
【0020】
一実施形態では、上記の距離ベクトルは、各軸状態量ベクトルｑに基づき特定できる。具体的には以下の通りである。
多軸ロボット３０の各軸状態量ベクトルｑが判れば、式（１）に基づき先端３８の位置パラメータ（Ｘ、Ｙ、およびＺ）は求まる。また、同次変換行列としての０Ｔｎ－１が式（２）、式（２Ａ）に適用されれば、ｎ－１番目のリンク３５の位置パラメータも求まる。同様の手法に則れば、各軸状態量ベクトルｑと式（１）とに基づき、多軸ロボット３０の任意の部位の位置パラメータが求まる。これにより、多軸ロボット３０と障害物との距離ベクトルは求まる。
なお、多軸ロボット３０Ｂが多軸ロボット３０Ａの障害物となる実施形態では、各タイムステップにおいて、多軸ロボット３０Ａ、３０Ｂが停止していると仮定して、多軸ロボット３０Ａ、３０Ｂの距離パラメータは求められる。
距離パラメータの算出方法の更なる詳細については後述する。
多軸ロボット３０の各軸状態量ベクトルｑが判れば、式（１）に基づき先端３８の位置パラメータ（Ｘ、Ｙ、およびＺ）は求まる。また、同次変換行列としての０Ｔｎ－１が式（２）、式（２Ａ）に適用されれば、ｎ－１番目のリンク３５の位置パラメータも求まる。同様の手法に則れば、各軸状態量ベクトルｑと式（１）とに基づき、多軸ロボット３０の任意の部位の位置パラメータが求まる。これにより、多軸ロボット３０と障害物との距離ベクトルは求まる。
なお、多軸ロボット３０Ｂが多軸ロボット３０Ａの障害物となる実施形態では、各タイムステップにおいて、多軸ロボット３０Ａ、３０Ｂが停止していると仮定して、多軸ロボット３０Ａ、３０Ｂの距離パラメータは求められる。
距離パラメータの算出方法の更なる詳細については後述する。
【0021】
一実施形態では、多軸ロボット３０を制御するための制御装置２０は、経路設定部２１、位置姿勢補正部２２、および位置姿勢制御部２３を備える。
一実施形態の経路設定部２１は、ワーク５に作業が実行される際の先端３８の移動経路３３を設定する。例えば、ワーク５の形状または位置に応じて、経路設定部２１は先端３８の移動経路３３を設定する。
一実施形態の移動経路３３は、始点Ｄ、複数の中間点Ｃ、および終点Ｇを含む。移動経路３３に含まれるこれら各々の点において、先端３８の位置姿勢（つまり、先端３８の位置姿勢パラメータ）が設定されている。以下、各々の中間点Ｃにおける先端３８の位置姿勢を中間位置姿勢ともいい、終点Ｇにおける先端３８の位置姿勢を最終位置姿勢ともいう。
一実施形態では、複数の中間点Ｃの少なくとも１つにおいて、位置姿勢パラメータに基づき定まる距離パラメータが閾値以下になる。このような中間点Ｃは補正対象として扱われる。
一実施形態の経路設定部２１は、ワーク５に作業が実行される際の先端３８の移動経路３３を設定する。例えば、ワーク５の形状または位置に応じて、経路設定部２１は先端３８の移動経路３３を設定する。
一実施形態の移動経路３３は、始点Ｄ、複数の中間点Ｃ、および終点Ｇを含む。移動経路３３に含まれるこれら各々の点において、先端３８の位置姿勢（つまり、先端３８の位置姿勢パラメータ）が設定されている。以下、各々の中間点Ｃにおける先端３８の位置姿勢を中間位置姿勢ともいい、終点Ｇにおける先端３８の位置姿勢を最終位置姿勢ともいう。
一実施形態では、複数の中間点Ｃの少なくとも１つにおいて、位置姿勢パラメータに基づき定まる距離パラメータが閾値以下になる。このような中間点Ｃは補正対象として扱われる。
【0022】
一実施形態の位置姿勢補正部２２は、補正対象の中間点Ｃの中間位置姿勢について、多軸ロボット３０と障害物との距離が閾値を超える先端３８の補正中間位置姿勢を求めるように構成される。言い換えると、位置姿勢補正部２２は、先端３８の位置姿勢が最終位置姿勢に変化するまでに経る複数の中間位置姿勢のうち、補正対象とする中間位置姿勢について、補正中間位置姿勢を求めるように構成される。
一実施形態の位置姿勢補正部２２は、多軸ロボット３０の各軸状態量を探索することで補正中間位置姿勢を求めるように構成される。より具体的には、位置姿勢パラメータおよび距離パラメータの値が許容範囲に収まるような各軸状態量の候補値を位置姿勢補正部２２は探索するように構成される。
一実施形態では、位置姿勢パラメータの値が許容範囲に収まることで、位置姿勢補正部２２によって求められる補正中間位置姿勢が理想的な位置姿勢から乖離し過ぎるのを抑制できる。理想的な位置姿勢は、例えば、補正対象となっている中間点Ｃの初期の位置姿勢である。
一実施形態では、距離パラメータの値が許容範囲に収まることで、距離パラメータが、理想的な値（例えば閾値）から乖離し過ぎるのを抑制できる。これにより、先端３８が補正中間位置姿勢を経る多軸ロボット３０の動作において無駄が低減する。
一実施形態の位置姿勢補正部２２は、多軸ロボット３０の各軸状態量を探索することで補正中間位置姿勢を求めるように構成される。より具体的には、位置姿勢パラメータおよび距離パラメータの値が許容範囲に収まるような各軸状態量の候補値を位置姿勢補正部２２は探索するように構成される。
一実施形態では、位置姿勢パラメータの値が許容範囲に収まることで、位置姿勢補正部２２によって求められる補正中間位置姿勢が理想的な位置姿勢から乖離し過ぎるのを抑制できる。理想的な位置姿勢は、例えば、補正対象となっている中間点Ｃの初期の位置姿勢である。
一実施形態では、距離パラメータの値が許容範囲に収まることで、距離パラメータが、理想的な値（例えば閾値）から乖離し過ぎるのを抑制できる。これにより、先端３８が補正中間位置姿勢を経る多軸ロボット３０の動作において無駄が低減する。
【0023】
一実施形態の位置姿勢制御部２３は、多軸ロボット３０の先端３８の位置姿勢が、位置姿勢補正部２２によって求められた補正中間位置姿勢を経て最終位置姿勢まで変化するよう、多軸ロボット３０を制御するように構成される。
一実施形態では、位置姿勢制御部２３が多軸ロボット３０を制御することで、先端３８は移動経路３３に沿って移動する。
一実施形態では、位置姿勢制御部２３が多軸ロボット３０を制御することで、先端３８は移動経路３３に沿って移動する。
【0024】
図２は、一実施形態に係る、位置姿勢補正部２２によって補正される中間点Ｃを示す図である。図２は、運搬装置７の運搬方向に沿って見た図を例示する。一実施形態では、移動経路３３の上流側から順に、複数の中間点Ｃの各々に番号が割り当てられる。
一実施形態では、１番目の中間点Ｃ１とｉ＋１番目の中間点Ｃｉ＋１とにおいて、距離パラメータが閾値以下であり、補正中間位置姿勢が位置姿勢補正部２２によって探索される。
一実施形態では、中間点Ｃ１と中間点Ｃｉ＋１のいずれにおいても、位置姿勢補正部２２は、６つの位置姿勢パラメータのうちで姿勢パラメータ（ＲＸ、ＲＹ、またはＲＺ）の少なくともいずれかが補正された補正中間位置姿勢を探索する。
なお、他の実施形態の位置姿勢補正部２２は、位置姿勢パラメータのうちで位置パラメータ（Ｘ、Ｙ、またはＺ）の少なくともいずれかが補正された補正中間位置姿勢を探索してもよいし、６つの位置姿勢パラメータが全て補正された補正中間位置姿勢を探索してもよい。
位置姿勢制御部２３が多軸ロボット３０を制御する場合、先端３８の位置姿勢は、中間点Ｃ１と中間点Ｃｉ＋１とにおいて、位置姿勢補正部２２によって探索された補正中間位置姿勢に変化する。
一実施形態では、１番目の中間点Ｃ１とｉ＋１番目の中間点Ｃｉ＋１とにおいて、距離パラメータが閾値以下であり、補正中間位置姿勢が位置姿勢補正部２２によって探索される。
一実施形態では、中間点Ｃ１と中間点Ｃｉ＋１のいずれにおいても、位置姿勢補正部２２は、６つの位置姿勢パラメータのうちで姿勢パラメータ（ＲＸ、ＲＹ、またはＲＺ）の少なくともいずれかが補正された補正中間位置姿勢を探索する。
なお、他の実施形態の位置姿勢補正部２２は、位置姿勢パラメータのうちで位置パラメータ（Ｘ、Ｙ、またはＺ）の少なくともいずれかが補正された補正中間位置姿勢を探索してもよいし、６つの位置姿勢パラメータが全て補正された補正中間位置姿勢を探索してもよい。
位置姿勢制御部２３が多軸ロボット３０を制御する場合、先端３８の位置姿勢は、中間点Ｃ１と中間点Ｃｉ＋１とにおいて、位置姿勢補正部２２によって探索された補正中間位置姿勢に変化する。
【0025】
位置姿勢補正部２２の補正対象となる位置姿勢パラメータは、補正中間位置姿勢の探索中、終始同じであるとは限らない。つまり、補正対象となる位置姿勢パラメータは、中間位置姿勢の探索状況または探索条件などに応じて、動的に切り替えることが出来る。例えば、探索回数が増えるに従い（あるいは距離パラメータが満たすべき閾値が小さくなるに従い）、補正対象となる位置姿勢パラメータが増えてもよいし、補正対象となる位置姿勢パラメータの種別が変化するが個数そのものは一定であってもよい。あるいは、これら２つの組み合わせが実行されてもよい。
より具体的な一例として、補正中間位置姿勢の探索回数が規定回数に達するまでの間はＲＸのみが補正対象となる。探索回数が規定回数に達する前に、ＲＸのみが補正された適正な補正中間位置姿勢（つまり、距離パラメータが閾値を超える中間位置姿勢）が特定されれば、探索は終了する。このとき、補正前の位置姿勢に極めて近い補正中間位置姿勢を特定できる。一方で、探索回数が規定回数に達しても、適正な補正中間位置姿勢（衝突回避等が可能な補正中間位置姿勢）が見つからなければ、ＲＸに加えて（あるいはＲＸに代えて）、例えばＲＹ及びＲＺが補正対象として追加できる。これにより、適正な中間位置姿勢が特定できない不具合を回避できる。
以上のように、可能な限り目標位置姿勢に近い補正中間位置姿勢を探索することが出来る上、衝突回避等が可能な補正中間位置姿勢を見つける確率を上げることが出来る。
より具体的な一例として、補正中間位置姿勢の探索回数が規定回数に達するまでの間はＲＸのみが補正対象となる。探索回数が規定回数に達する前に、ＲＸのみが補正された適正な補正中間位置姿勢（つまり、距離パラメータが閾値を超える中間位置姿勢）が特定されれば、探索は終了する。このとき、補正前の位置姿勢に極めて近い補正中間位置姿勢を特定できる。一方で、探索回数が規定回数に達しても、適正な補正中間位置姿勢（衝突回避等が可能な補正中間位置姿勢）が見つからなければ、ＲＸに加えて（あるいはＲＸに代えて）、例えばＲＹ及びＲＺが補正対象として追加できる。これにより、適正な中間位置姿勢が特定できない不具合を回避できる。
以上のように、可能な限り目標位置姿勢に近い補正中間位置姿勢を探索することが出来る上、衝突回避等が可能な補正中間位置姿勢を見つける確率を上げることが出来る。
【0026】
図３は、一実施形態に係る位置姿勢補正部２２が補正中間位置姿勢を探索する過程を示す概念図である。図３の例では、１番目の中間点Ｃ１の補正中間位置姿勢が求められる過程を示す。また、図３では移動経路３３を一部省略して図示する。
中間点Ｃ１において距離パラメータが閾値以下であれば、中間点Ｃ１の中間位置姿勢は補正対象であると判定される。
一実施形態ではこの場合、位置姿勢補正部２２は、初期の中間点Ｃ１である中間点Ｃ１、１を更新して、中間点Ｃ１の候補としての中間点Ｃ１、２を求める。
中間点Ｃ１、２を候補として求める具体的な手法の一例は以下の通りである。位置姿勢補正部２２は、中間点Ｃ１、１の位置姿勢パラメータに対応する対応する各軸状態量ｑ１、１を式（５）に基づき求める。その後、中間点Ｃ１、１おける位置姿勢パラメータおよび目標値の偏差と、中間点Ｃ１、１おける距離パラメータおよび目標値との偏差とに基づき、各軸状態量の更新量であるΔｑ１、１を位置姿勢補正部２２は求める（更新量の求め方の更なる詳細は後述する）。結果、各軸状態量ｑ１、２（＝ｑ１、１＋Δｑ１、１）が求まる。位置姿勢補正部２２は、式（２）と各軸状態量ｑ１、２とを用いて、中間点Ｃ１、２の位置姿勢ベクトルｐ（位置姿勢パラメータ）を求める。
中間点Ｃ１、２における位置姿勢パラメータまたは距離パラメータの少なくとも一方の値が許容範囲から逸脱する場合、位置姿勢補正部２２は中間点Ｃ１、２をさらに更新して、新たな候補の中間点Ｃ１、３を求める。求め方は、上記と同様であり、最新の各軸状態量の候補値であるｑ１、２にΔｑ１、２が加えられることで、中間点Ｃ１、３の位置姿勢パラメータが求まる。
位置姿勢パラメータと距離パラメータのいずれの値も許容範囲に収まるまで、各軸状態量の探索は繰り返される。そして、ｋ－１回目の更新で特定された中間点Ｃ１、ｋにおいて上記パラメータの値が許容範囲に収まれば、中間点Ｃ１、ｋが補正後の中間点Ｃ１として設定される。つまり、中間点Ｃ１、ｋの中間位置姿勢が、初期の中間点Ｃ１の中間位置姿勢に代わる補正中間位置姿勢として扱われる。
この繰り返し時、補正対象となる位置姿勢パラメータ（例えばＲＸ、ＲＹなど）を、条件によって動的に切り替えたり、追加していくことにより、軌道の補正の自由度を高めることが出来る。
詳細な図示および説明は省略するが、ｉ＋１番目の中間点Ｃｉ＋１が補正対象である場合においても、同様の過程が繰り返される。例えばｋ－１回目の更新で特定された中間点Ｃｉ＋１、ｋにおいて、位置姿勢パラメータと距離パラメータがいずれの値も許容範囲に収まれば、中間点Ｃｉ＋１、ｋが補正後の中間点Ｃｉ＋１として扱われる。
中間点Ｃ１において距離パラメータが閾値以下であれば、中間点Ｃ１の中間位置姿勢は補正対象であると判定される。
一実施形態ではこの場合、位置姿勢補正部２２は、初期の中間点Ｃ１である中間点Ｃ１、１を更新して、中間点Ｃ１の候補としての中間点Ｃ１、２を求める。
中間点Ｃ１、２を候補として求める具体的な手法の一例は以下の通りである。位置姿勢補正部２２は、中間点Ｃ１、１の位置姿勢パラメータに対応する対応する各軸状態量ｑ１、１を式（５）に基づき求める。その後、中間点Ｃ１、１おける位置姿勢パラメータおよび目標値の偏差と、中間点Ｃ１、１おける距離パラメータおよび目標値との偏差とに基づき、各軸状態量の更新量であるΔｑ１、１を位置姿勢補正部２２は求める（更新量の求め方の更なる詳細は後述する）。結果、各軸状態量ｑ１、２（＝ｑ１、１＋Δｑ１、１）が求まる。位置姿勢補正部２２は、式（２）と各軸状態量ｑ１、２とを用いて、中間点Ｃ１、２の位置姿勢ベクトルｐ（位置姿勢パラメータ）を求める。
中間点Ｃ１、２における位置姿勢パラメータまたは距離パラメータの少なくとも一方の値が許容範囲から逸脱する場合、位置姿勢補正部２２は中間点Ｃ１、２をさらに更新して、新たな候補の中間点Ｃ１、３を求める。求め方は、上記と同様であり、最新の各軸状態量の候補値であるｑ１、２にΔｑ１、２が加えられることで、中間点Ｃ１、３の位置姿勢パラメータが求まる。
位置姿勢パラメータと距離パラメータのいずれの値も許容範囲に収まるまで、各軸状態量の探索は繰り返される。そして、ｋ－１回目の更新で特定された中間点Ｃ１、ｋにおいて上記パラメータの値が許容範囲に収まれば、中間点Ｃ１、ｋが補正後の中間点Ｃ１として設定される。つまり、中間点Ｃ１、ｋの中間位置姿勢が、初期の中間点Ｃ１の中間位置姿勢に代わる補正中間位置姿勢として扱われる。
この繰り返し時、補正対象となる位置姿勢パラメータ（例えばＲＸ、ＲＹなど）を、条件によって動的に切り替えたり、追加していくことにより、軌道の補正の自由度を高めることが出来る。
詳細な図示および説明は省略するが、ｉ＋１番目の中間点Ｃｉ＋１が補正対象である場合においても、同様の過程が繰り返される。例えばｋ－１回目の更新で特定された中間点Ｃｉ＋１、ｋにおいて、位置姿勢パラメータと距離パラメータがいずれの値も許容範囲に収まれば、中間点Ｃｉ＋１、ｋが補正後の中間点Ｃｉ＋１として扱われる。
【0027】
図４は、一実施形態に係る位置姿勢補正部２２の構成を示す図である。
位置姿勢補正部２２は、第１行列を取得するための第１行列取得部１１、第２行列を取得するための第２行列取得部１２、および、第１行列と第２行列に基づき各軸状態量の候補値を更新するための更新部１３を含む。
位置姿勢補正部２２は、第１行列を取得するための第１行列取得部１１、第２行列を取得するための第２行列取得部１２、および、第１行列と第２行列に基づき各軸状態量の候補値を更新するための更新部１３を含む。
【0028】
一実施形態において、第１行列取得部１１によって取得される第１行列は、各軸状態量に対する１以上の位置姿勢パラメータの感度を少なくとも示す。第１行列は、一例としてヤコブ行列である。
例えば、各軸状態量ベクトルｑが６つの各軸状態量を含み、且つ第１行列が６つの位置姿勢パラメータ（Ｘ、Ｙ、Ｚ、ＲＸ、ＲＹ、およびＲＺ）の各軸状態量に対する感度を示す場合、第１行列（Ｊ１ｓ）は式（６）によって表される。
例えば、各軸状態量ベクトルｑが６つの各軸状態量を含み、且つ第１行列が６つの位置姿勢パラメータ（Ｘ、Ｙ、Ｚ、ＲＸ、ＲＹ、およびＲＺ）の各軸状態量に対する感度を示す場合、第１行列（Ｊ１ｓ）は式（６）によって表される。
【数2】
【0029】
一実施形態において、第２行列取得部１２によって取得される第２行列は、各軸状態量に対する距離パラメータの感度を少なくとも示す。第２行列は、一例としてヤコブ行列である。
例えば、各軸状態量ベクトルｑが６つの各軸状態量を含み、且つ第２行列が複数の距離パラメータ（ｄ１、・・・、ｄｍ）の各軸状態量に対する感度を示す場合、第２行列（Ｊ２ｓ）は式（７）によって表される。
例えば、各軸状態量ベクトルｑが６つの各軸状態量を含み、且つ第２行列が複数の距離パラメータ（ｄ１、・・・、ｄｍ）の各軸状態量に対する感度を示す場合、第２行列（Ｊ２ｓ）は式（７）によって表される。
【数3】
【0030】
一実施形態の更新部１３は、上記の第１行列および第２行列の他に、第１偏差および第２偏差に基づき、各軸状態量の候補値の更新をするように構成される。
第１偏差は、各軸状態量の候補値に対応する１以上の位置姿勢パラメータの、目標値に対する偏差である。一実施形態の該目標値は、補正対象位置姿勢の位置姿勢パラメータである。
第２偏差は、各軸状態量の候補値に対応する距離パラメータの、目標値に対する偏差である。一実施形態の該目標値は、距離パラメータに対応して定められた閾値である。
一実施形態では、第１偏差で扱われる位置姿勢パラメータは、第１行列で扱われる位置姿勢パラメータと同じである。例えば、第１行列が６つの位置姿勢パラメータの各軸状態量に対する感度を示す場合、第１偏差はこれら６つのパラメータに関する偏差である。同様に、第２偏差で扱われる距離パラメータは、第２行列で扱われる距離パラメータと同じである。例えば、第２行列がｍ個の距離パラメータの各軸状態量に対する感度を示す場合、第２偏差は同じ距離パラメータに関する偏差である。
一実施形態では、第１偏差と第１行列が判れば、更新前の候補値に対する位置姿勢パラメータを目標値に近づけるための各軸状態量の更新量が、例えば勾配法の適用によって求まる。同様に、第２偏差と第２行列が判れば、更新前の候補値に対応する距離パラメータを目標値に近づけるための各軸状態量の更新量が求まる。
一実施形態の更新部１３は、第１偏差および第１行列から求まる各軸状態量の第１更新量と、第２偏差および第２行列から求まる各軸状態量の第２更新量とに基づき、各軸状態量の最新の候補値を更新する更新量（Δｑ）を求める。
第１偏差は、各軸状態量の候補値に対応する１以上の位置姿勢パラメータの、目標値に対する偏差である。一実施形態の該目標値は、補正対象位置姿勢の位置姿勢パラメータである。
第２偏差は、各軸状態量の候補値に対応する距離パラメータの、目標値に対する偏差である。一実施形態の該目標値は、距離パラメータに対応して定められた閾値である。
一実施形態では、第１偏差で扱われる位置姿勢パラメータは、第１行列で扱われる位置姿勢パラメータと同じである。例えば、第１行列が６つの位置姿勢パラメータの各軸状態量に対する感度を示す場合、第１偏差はこれら６つのパラメータに関する偏差である。同様に、第２偏差で扱われる距離パラメータは、第２行列で扱われる距離パラメータと同じである。例えば、第２行列がｍ個の距離パラメータの各軸状態量に対する感度を示す場合、第２偏差は同じ距離パラメータに関する偏差である。
一実施形態では、第１偏差と第１行列が判れば、更新前の候補値に対する位置姿勢パラメータを目標値に近づけるための各軸状態量の更新量が、例えば勾配法の適用によって求まる。同様に、第２偏差と第２行列が判れば、更新前の候補値に対応する距離パラメータを目標値に近づけるための各軸状態量の更新量が求まる。
一実施形態の更新部１３は、第１偏差および第１行列から求まる各軸状態量の第１更新量と、第２偏差および第２行列から求まる各軸状態量の第２更新量とに基づき、各軸状態量の最新の候補値を更新する更新量（Δｑ）を求める。
【0031】
より具体的な一例として、更新部１３は、以下の式（８）、式（９）、および式（１０）に基づき、各軸状態量の更新量（Δｑ）を求める。式（９）で示されるＪ＋はＪの逆行列を意味する。その他の式においても、添え字として”＋”が付加された行列は、添え字が付加されていない行列の疑似逆行列を意味する。
Δｑ＝Δｑ１＋Δｑ２ ・・・式（８）
Δｑ１＝Ｊ＋Δｘ ・・・式（９）
Δｑ２＝（ＪＣ（Ｉ－Ｊ＋Ｊ））＋（ΔｘＣ－ＪＣＪ＋Δｘ） ・・・式（１０）
式（８）～式（１０）において、Δｑ１が、上述した第１更新量または第２更新量のいずれか一方であり、Δｑ２が、第１更新量または第２更新量のいずれか他方である。
一実施形態では、Δｑ１が第１更新量であり、Δｑ２が第２更新量である。この場合、Δｘが第１偏差を要素として含む第１ベクトル（一例として列ベクトル）であり、Ｊが第１行列である。また、ΔｘＣが第２偏差を要素として含む第２ベクトル（一例として列ベクトル）であり、ＪＣが第２行列である。即ち、Δｘ及びＪの組み合わせが、第１偏差を要素として含む第１ベクトルおよび第１行列の組み合わせであり、ΔｘＣおよびＪＣの組み合わせが、第２偏差を要素として含む第２ベクトルおよび第２行列の組み合わせである。
別の実施形態では、Δｑ１が第２更新量であり、Δｑ２が第１更新量である。この場合、上記した組み合わせは逆になる。つまり、Δｘ及びＪの組み合わせが、第２ベクトルおよび第２行列の組み合わせであり、ΔｘＣおよびＪＣの組み合わせが、第１ベクトルおよび第１行列の組み合わせである。
例えば、第１偏差が６つの位置姿勢パラメータの目標値に対する偏差であり、第２偏差がｍ個の距離パラメータの偏差であれば、式（８）～式（１０）に適用される第１行列（ＪまたはＪＣの一方）は、式（６）のＪ１ｓと一致し、式（８）～式（１０）に適用される第２行列（ＪまたはＪＣの他方）は、式（７）のＪ２ｓと一致する。
式（８）～式（１０）では、Δｘを小さくすることが、ΔｘＣを小さくなることよりも優先されるΔｑが求まることを意味する。このようなΔｑを求めることができる原理を詳説することに先立ち、式（８）～式（１０）によって導出される式（１）を以下に示す。
Δｑ＝Ｊ＋Δｘ＋（ＪＣ（Ｉ－Ｊ＋Ｊ））＋（ΔｘＣ－ＪＣＪ＋Δｘ）・・・式（１）
Δｑ＝Δｑ１＋Δｑ２ ・・・式（８）
Δｑ１＝Ｊ＋Δｘ ・・・式（９）
Δｑ２＝（ＪＣ（Ｉ－Ｊ＋Ｊ））＋（ΔｘＣ－ＪＣＪ＋Δｘ） ・・・式（１０）
式（８）～式（１０）において、Δｑ１が、上述した第１更新量または第２更新量のいずれか一方であり、Δｑ２が、第１更新量または第２更新量のいずれか他方である。
一実施形態では、Δｑ１が第１更新量であり、Δｑ２が第２更新量である。この場合、Δｘが第１偏差を要素として含む第１ベクトル（一例として列ベクトル）であり、Ｊが第１行列である。また、ΔｘＣが第２偏差を要素として含む第２ベクトル（一例として列ベクトル）であり、ＪＣが第２行列である。即ち、Δｘ及びＪの組み合わせが、第１偏差を要素として含む第１ベクトルおよび第１行列の組み合わせであり、ΔｘＣおよびＪＣの組み合わせが、第２偏差を要素として含む第２ベクトルおよび第２行列の組み合わせである。
別の実施形態では、Δｑ１が第２更新量であり、Δｑ２が第１更新量である。この場合、上記した組み合わせは逆になる。つまり、Δｘ及びＪの組み合わせが、第２ベクトルおよび第２行列の組み合わせであり、ΔｘＣおよびＪＣの組み合わせが、第１ベクトルおよび第１行列の組み合わせである。
例えば、第１偏差が６つの位置姿勢パラメータの目標値に対する偏差であり、第２偏差がｍ個の距離パラメータの偏差であれば、式（８）～式（１０）に適用される第１行列（ＪまたはＪＣの一方）は、式（６）のＪ１ｓと一致し、式（８）～式（１０）に適用される第２行列（ＪまたはＪＣの他方）は、式（７）のＪ２ｓと一致する。
式（８）～式（１０）では、Δｘを小さくすることが、ΔｘＣを小さくなることよりも優先されるΔｑが求まることを意味する。このようなΔｑを求めることができる原理を詳説することに先立ち、式（８）～式（１０）によって導出される式（１）を以下に示す。
Δｑ＝Ｊ＋Δｘ＋（ＪＣ（Ｉ－Ｊ＋Ｊ））＋（ΔｘＣ－ＪＣＪ＋Δｘ）・・・式（１）
【0032】
図５は、一実施形態に係る、式（１）に基づき各軸状態量の更新量Δｑを求める原理を示す概念図である。図５は、ベクトルであるΔｑ、Δｘ、およびΔｘＣを、２次元座標を用いて概念的に示す。
以下では、Δｑ１が第１更新量であり、Δｑ２が第２更新量であると仮定する。つまり、Δｘ及びＪの組み合わせが、第１ベクトルおよび第１行列の組み合わせであり、ΔｘＣおよびＪＣの組み合わせが、第２ベクトルおよび第２行列の組み合わせであると仮定する。なお、組み合わせが逆になっても、更新量Δｑを求める原理は変わらない。
さらに、式（１１）、式（１２）が成立すると仮定する。また、ｆ（ｑ）は上に凸であり、ｇ（ｑ）は下に凸であるとする。
ｘ＝ｆ（ｑ） ・・・式（１１）
ｘＣ＝ｇ（ｑ） ・・・式（１２）
また、ベクトルｘの目標値と許容範囲を各々、ｘｔｇｔ、ｘｔｇｔ－τとし、ベクトルｘＣの目標値と許容範囲を各々、ｘＣ＿ｔｇｔ、ｘＣ＿ｔｇｔ＋τＣとする。さらに、現在の各軸状態量の候補値をｑｃｕｒとする。
以下では、Δｑ１が第１更新量であり、Δｑ２が第２更新量であると仮定する。つまり、Δｘ及びＪの組み合わせが、第１ベクトルおよび第１行列の組み合わせであり、ΔｘＣおよびＪＣの組み合わせが、第２ベクトルおよび第２行列の組み合わせであると仮定する。なお、組み合わせが逆になっても、更新量Δｑを求める原理は変わらない。
さらに、式（１１）、式（１２）が成立すると仮定する。また、ｆ（ｑ）は上に凸であり、ｇ（ｑ）は下に凸であるとする。
ｘ＝ｆ（ｑ） ・・・式（１１）
ｘＣ＝ｇ（ｑ） ・・・式（１２）
また、ベクトルｘの目標値と許容範囲を各々、ｘｔｇｔ、ｘｔｇｔ－τとし、ベクトルｘＣの目標値と許容範囲を各々、ｘＣ＿ｔｇｔ、ｘＣ＿ｔｇｔ＋τＣとする。さらに、現在の各軸状態量の候補値をｑｃｕｒとする。
【0033】
ベクトルｘを許容範囲に収めるための各軸状態量の更新量であるΔｑ１が求まる原理は以下の通りである。
ｑｃｕｒに対応するベクトルｘであるｘｃｕｒは、点Ｐ１の高さ位置に相当する。既述の通り、第１行列（Ｊ）は各軸状態量に対する位置姿勢パラメータの感度を示す。つまり、点Ｐ１におけるグラフの接線の傾きが第１行列に相当する。従って、第１偏差であるΔｘ（＝ｘｃｕｒ－ｘｔｇｔ）は、以下の式（１３）で規定される。
Δｘ＝ＪΔｑ１ ・・・式（１３）
式（１３）を変形することで、上述の式（９）が導かれる。これにより、Δｑ１は、ベクトルｘで扱われる位置姿勢パラメータを目標値に近づけるための更新量であり、第１偏差が反映された更新量であることが理解される。
ｑｃｕｒに対応するベクトルｘであるｘｃｕｒは、点Ｐ１の高さ位置に相当する。既述の通り、第１行列（Ｊ）は各軸状態量に対する位置姿勢パラメータの感度を示す。つまり、点Ｐ１におけるグラフの接線の傾きが第１行列に相当する。従って、第１偏差であるΔｘ（＝ｘｃｕｒ－ｘｔｇｔ）は、以下の式（１３）で規定される。
Δｘ＝ＪΔｑ１ ・・・式（１３）
式（１３）を変形することで、上述の式（９）が導かれる。これにより、Δｑ１は、ベクトルｘで扱われる位置姿勢パラメータを目標値に近づけるための更新量であり、第１偏差が反映された更新量であることが理解される。
【0034】
ベクトルｘＣを許容範囲に収めるための各軸状態量の更新量であるΔｑ２が求まる原理は以下の通りである。
ｑｃｕｒにおけるベクトルｘＣであるｘＣ＿ｃｕｒは、点Ｑ１の高さ位置に相当する。点Ｑ１におけるグラフの接線が第２行列（ＪＣ）を示し、ｘＣ＿ｃｕｒ－ｘＣ＿ｔｇｔが第２偏差であるΔｘＣを示す。
Δｑ２を特定する際には、第２偏差であるΔｘＣではなく、ΔｘＣ－ＪＣＪ＋Δｘで規定される偏差（第２偏差を利用した偏差）が用いられる。また、点Ｑ１におけるグラフの接線（接線の傾きが第２行列に相当する）ではなく、ＪＣ（Ｉ－Ｊ＋Ｊ）で規定される傾きを有する直線が扱われる。
すなわち、ベクトルｘＣを目標値ｘＣ＿ｔｇｔに近づけるためのΔｑ２は、式（１４）によって規定される。式（１４）を変形することで、上述の式（１０）は求まる。
ΔｘＣ－ＪＣＪ＋Δｘ＝ＪＣ（Ｉ－Ｊ＋Ｊ）Δｑ２ ・・・式（１４）
仮に、第２偏差であるΔｘＣと、且つ点Ｑ１の接線とを用いた場合、式（１３）と同様の式（１５）によって、Δｑ２ｄが求まる。
ΔｘＣ＝ＪＣΔｑ２ｄ ・・・式（１５）
式（１０）に基づき求まるΔｑ２と、式（１５）に基づき求まるΔｑ２ｄは、いずれも、ベクトルｘＣで扱われるパラメータ（距離パラメータが含まれる）を目標値に近づけるための更新量であり、第２偏差が反映された更新量である。
ｑｃｕｒにおけるベクトルｘＣであるｘＣ＿ｃｕｒは、点Ｑ１の高さ位置に相当する。点Ｑ１におけるグラフの接線が第２行列（ＪＣ）を示し、ｘＣ＿ｃｕｒ－ｘＣ＿ｔｇｔが第２偏差であるΔｘＣを示す。
Δｑ２を特定する際には、第２偏差であるΔｘＣではなく、ΔｘＣ－ＪＣＪ＋Δｘで規定される偏差（第２偏差を利用した偏差）が用いられる。また、点Ｑ１におけるグラフの接線（接線の傾きが第２行列に相当する）ではなく、ＪＣ（Ｉ－Ｊ＋Ｊ）で規定される傾きを有する直線が扱われる。
すなわち、ベクトルｘＣを目標値ｘＣ＿ｔｇｔに近づけるためのΔｑ２は、式（１４）によって規定される。式（１４）を変形することで、上述の式（１０）は求まる。
ΔｘＣ－ＪＣＪ＋Δｘ＝ＪＣ（Ｉ－Ｊ＋Ｊ）Δｑ２ ・・・式（１４）
仮に、第２偏差であるΔｘＣと、且つ点Ｑ１の接線とを用いた場合、式（１３）と同様の式（１５）によって、Δｑ２ｄが求まる。
ΔｘＣ＝ＪＣΔｑ２ｄ ・・・式（１５）
式（１０）に基づき求まるΔｑ２と、式（１５）に基づき求まるΔｑ２ｄは、いずれも、ベクトルｘＣで扱われるパラメータ（距離パラメータが含まれる）を目標値に近づけるための更新量であり、第２偏差が反映された更新量である。
【0035】
各軸状態量の更新には以上より求めたΔｑ１とΔｑ２が用いられ、ｑｃｕｒ＋Δｑ１＋Δｑ２が新たなｑｃｕｒとして設定される。概念的に例示する図５では、Δｑ２がΔｑ２ｄよりも小さく、１回目の更新結果として得られるｑｃｕｒ＋Δｑ１＋Δｑ２は、第２偏差よりも第１偏差を優先して小さくするための各軸状態量として理解できる。つまり、式（１）が利用されることによって、ベクトルｘがベクトルｘＣよりも優先して許容範囲に収まるようなΔｑ（＝Δｑ１＋Δｑ２）が求まる。
図５の例では、更新後のベクトルｘを示す点Ｐ２と、更新後のベクトルｘＣを示す点Ｑ２がいずれも許容範囲に収まるので、各軸状態量ベクトルｑの更新は終了する。
仮に、ベクトルｘとベクトルｘＣの少なくとも一方が許容範囲から逸脱すれば、各軸状態量ベクトルｑの更新がさらに実行される。例えば、各軸状態量の更新によって第１偏差が０になった場合であっても、ベクトルｘＣが許容範囲から逸脱すれば、Δｑはさらに更新される。
図５の例では、更新後のベクトルｘを示す点Ｐ２と、更新後のベクトルｘＣを示す点Ｑ２がいずれも許容範囲に収まるので、各軸状態量ベクトルｑの更新は終了する。
仮に、ベクトルｘとベクトルｘＣの少なくとも一方が許容範囲から逸脱すれば、各軸状態量ベクトルｑの更新がさらに実行される。例えば、各軸状態量の更新によって第１偏差が０になった場合であっても、ベクトルｘＣが許容範囲から逸脱すれば、Δｑはさらに更新される。
【0036】
なお、他の実施形態では、第１行列は、６つの位置姿勢パラメータの一部の各軸状態量に対する感度を示してもよい。この場合、第１ベクトルは、６つの位置姿勢パラメータのうちの一部の位置姿勢パラメータの目標値に対する差異を扱う。
例えば第１行列で扱われる位置姿勢パラメータがＸ、Ｙ、Ｚ、ＲＺである場合、第１行列（Ｊ１ｖ）は式（１６）によって表される。この場合、第１偏差を要素として含む第１ベクトルは、位置姿勢パラメータＸ、Ｙ、Ｚ、ＲＺの目標値に対する偏差を示す。
この場合、第１偏差を要素として含む第１ベクトルは、位置姿勢パラメータＸ、Ｙ、Ｚ、ＲＺの目標値に対する偏差を示す。仮に、第１ベクトルがΔｘに割り当てられれば、式（１）におけるＪは、式（１６）のＪ１ｖと一致する。このような実施形態では、位置姿勢パラメータＸ、Ｙ、Ｚ、ＲＺが許容範囲に収まることが、他のパラメータ（距離パラメータ、ＲＸ、およびＲＹ）が許容範囲に収まることよりも優先されるようなΔｑが求められる。
なお、上記実施形態において、Δｘに第２ベクトルが割り当てられ、ΔｘＣに第１ベクトルが割り当てられてもよい。また、第１ベクトルまたは第２ベクトルが示す位置姿勢パラメータは繰り返し計算中に動的に切り替えることも出来る。例えば、位置姿勢パラメータの種類が、繰り返し計算に伴い変更（あるいは増加）してもよい。この場合、第１行列または第２行列の少なくも一方は、位置姿勢パラメータの種類の変更（あるいは増加）に応じて変更されてもよい。
第１ベクトルで扱われる位置姿勢パラメータは、Ｘ、Ｙ、Ｚ、およびＲＺに限定されない。別の実施形態では、第１ベクトルは、６つの位置姿勢パラメータのいずれか２つの目標値に対する偏差のみを要素として含んでもよいし、いずれか４つの目標値に対する偏差を含んでもよい。
例えば第１行列で扱われる位置姿勢パラメータがＸ、Ｙ、Ｚ、ＲＺである場合、第１行列（Ｊ１ｖ）は式（１６）によって表される。この場合、第１偏差を要素として含む第１ベクトルは、位置姿勢パラメータＸ、Ｙ、Ｚ、ＲＺの目標値に対する偏差を示す。
【数4】
なお、上記実施形態において、Δｘに第２ベクトルが割り当てられ、ΔｘＣに第１ベクトルが割り当てられてもよい。また、第１ベクトルまたは第２ベクトルが示す位置姿勢パラメータは繰り返し計算中に動的に切り替えることも出来る。例えば、位置姿勢パラメータの種類が、繰り返し計算に伴い変更（あるいは増加）してもよい。この場合、第１行列または第２行列の少なくも一方は、位置姿勢パラメータの種類の変更（あるいは増加）に応じて変更されてもよい。
第１ベクトルで扱われる位置姿勢パラメータは、Ｘ、Ｙ、Ｚ、およびＲＺに限定されない。別の実施形態では、第１ベクトルは、６つの位置姿勢パラメータのいずれか２つの目標値に対する偏差のみを要素として含んでもよいし、いずれか４つの目標値に対する偏差を含んでもよい。
【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】
上記（９）の構成によれば、上記（１）と同様の理由によって、多軸ロボット３０と障害物との衝突を回避しながら多軸ロボット３０の位置姿勢を変えることができる多軸ロボット用の制御プログラム９５が実現する。
【0076】
上記説明では、図９において、Ｓ１７で許容条件を満たした位置姿勢を示す点と、Ｓ２３において決定される中間点Ｃｉが一致する実施形態を説明した。別の実施形態では、Ｓ１７で許容条件を満たすと判定され、かつ、多軸ロボット３０の修正軌道に含まれる複数の任意の中間点Ｃｉ（例えば５個の中間点）について、重み係数を付与し、その任意の数の中間点Ｃｉにおける位置姿勢を微調整することもできる（つまり、修正軌道を微調整することもできる）。微調整後の中間点はいずれも許容条件を満たすことが好ましい。ただし、Ｓ１７で設定される許容条件を満たさないものの、当該許容条件より緩い衝突回避条件を満たす微調整後の中間点が採用されることがあってもよい。
これにより、ある中間点Ｃｉは、その中間点よりも下流側にある任意の複数の中間点Ｃｉの位置を考慮に入れた位置となり、多軸ロボット３０の軌道を滑らかにできる。従って、多軸ロボット３０の動作がスムーズになる。
これにより、ある中間点Ｃｉは、その中間点よりも下流側にある任意の複数の中間点Ｃｉの位置を考慮に入れた位置となり、多軸ロボット３０の軌道を滑らかにできる。従って、多軸ロボット３０の動作がスムーズになる。
【符号の説明】
【0077】
１ ：ロボットシステム
１１ ：第１行列取得部
１２ ：第２行列取得部
１３ ：更新部
２０ ：制御装置
２２ ：位置姿勢補正部
２３ ：位置姿勢制御部
３０ ：多軸ロボット
３５ ：リンク
３８ ：先端
５０ ：モデル
５５ ：円筒部
５６ ：半球部
ｄ１～ｄ３ ：距離パラメータ
ｑ１～ｑ６ ：軸状態量
１１ ：第１行列取得部
１２ ：第２行列取得部
１３ ：更新部
２０ ：制御装置
２２ ：位置姿勢補正部
２３ ：位置姿勢制御部
３０ ：多軸ロボット
３５ ：リンク
３８ ：先端
５０ ：モデル
５５ ：円筒部
５６ ：半球部
ｄ１～ｄ３ ：距離パラメータ
ｑ１～ｑ６ ：軸状態量
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】