特開2022-161544多軸ロボット用の制御装置、ロボットシステム、および制御プログラム
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022161544
(43)【公開日】2022-10-21
(54)【発明の名称】多軸ロボット用の制御装置、ロボットシステム、および制御プログラム
(51)【国際特許分類】
B25J 13/00 20060101AFI20221014BHJP
【ＦＩ】
B25J13/00 Z
【審査請求】未請求
【請求項の数】8
【出願形態】ＯＬ
(21)【出願番号】P 2021066449
(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】
コンピュータに、
多軸ロボットの先端の目標位置姿勢に代替する補正位置姿勢を求めるための位置姿勢補正ステップを実行させ、
前記位置姿勢補正ステップは、
前記多軸ロボットの各軸状態量に対する、前記多軸ロボットの前記先端の位置姿勢を示す複数の位置姿勢パラメータの一部である第１パラメータの感度を少なくとも示す第１行列を取得するための第１行列取得ステップと、
前記各軸状態量に対する、前記位置姿勢パラメータとは異なる第２パラメータの感度を少なくとも示す第２行列を取得するための第２行列取得ステップと、
前記各軸状態量の候補値に対応する前記第１パラメータの前記目標位置姿勢を基準とした偏差を含む第１偏差と、前記各軸状態量の前記候補値に対応する前記第２パラメータの目標値を基準とした偏差を含む第２偏差と、前記第１行列および前記第２行列とを用いて、前記各軸状態量の前記候補値を更新するための更新ステップと
を含む多軸ロボット用の制御プログラム。
多軸ロボットの先端の目標位置姿勢に代替する補正位置姿勢を求めるための位置姿勢補正ステップを実行させ、
前記位置姿勢補正ステップは、
前記多軸ロボットの各軸状態量に対する、前記多軸ロボットの前記先端の位置姿勢を示す複数の位置姿勢パラメータの一部である第１パラメータの感度を少なくとも示す第１行列を取得するための第１行列取得ステップと、
前記各軸状態量に対する、前記位置姿勢パラメータとは異なる第２パラメータの感度を少なくとも示す第２行列を取得するための第２行列取得ステップと、
前記各軸状態量の候補値に対応する前記第１パラメータの前記目標位置姿勢を基準とした偏差を含む第１偏差と、前記各軸状態量の前記候補値に対応する前記第２パラメータの目標値を基準とした偏差を含む第２偏差と、前記第１行列および前記第２行列とを用いて、前記各軸状態量の前記候補値を更新するための更新ステップと
を含む多軸ロボット用の制御プログラム。
【発明の詳細な説明】
【技術分野】
【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】
図２は、一実施形態に係る、位置姿勢補正部２２によって補正される中間点Ｃを示す図である。図２は、運搬装置７の運搬方向に沿って見た図を例示する。一実施形態では、移動経路３３の上流側から順に、複数の中間点Ｃの各々に番号が割り当てられる。
一実施形態では、１番目の中間点Ｃ１とｉ＋１番目の中間点Ｃｉ＋１とにおいて、判定対象パラメータに含まれる距離パラメータが閾値以下である。この場合、中間点Ｃ１と中間点Ｃｉ＋１の各々が示す目標位置姿勢に多軸ロボット３０の先端３８が変化すると、多軸ロボット３０と障害物との衝突が発生し得る。従って、中間点Ｃ１と中間点Ｃｉ＋１の各々が示す目標位置姿勢に先端３８の位置姿勢を厳密な精度で一致させることは困難である。この場合、中間点Ｃ１と中間点Ｃｉ＋１の各々について、目標位置姿勢に代替する補正位置姿勢が位置姿勢補正部２２によって求められる（探索される）。
一実施形態では、中間点Ｃ１の目標位置姿勢に対応する各軸状態量が、各軸状態量の候補値に設定される。位置姿勢補正部２２は、該各軸状態量の候補値を更新することで、中間点Ｃ１における補正位置姿勢を求める（探索する）。中間点Ｃｉ＋１においても同様の手法によって、中間点Ｃｉにおける補正位置姿勢が求められる。一実施形態では、補正位置姿勢における判定対象パラメータの値は許容範囲に収まるので、補正位置姿勢が目標位置姿勢から乖離し過ぎることは抑制される。
図２で例示される補正後の中間点Ｃ１と中間点Ｃｉ＋１とにおいては、６つの位置姿勢パラメータのうちで姿勢パラメータ（ＲＸ、ＲＹ、またはＲＺ）の少なくともいずれかが補正される。なお、他の実施形態では、位置パラメータ（Ｘ、Ｙ、またはＺ）の少なくともいずれかが補正されてもよいし、６つの位置姿勢パラメータが全て補正されてもよい。
このように、先端３８の位置姿勢を目標位置姿勢に厳密な精度で一致させることが困難である場合、目標位置姿勢を示す６つの位置姿勢パラメータのうち少なくとも１つが変更された補正位置姿勢が求められる。この場合、先端３８が目標位置姿勢に変化するための制御が実行されることで、先端３８の位置姿勢は補正位置姿勢に変化する。
一実施形態では、１番目の中間点Ｃ１とｉ＋１番目の中間点Ｃｉ＋１とにおいて、判定対象パラメータに含まれる距離パラメータが閾値以下である。この場合、中間点Ｃ１と中間点Ｃｉ＋１の各々が示す目標位置姿勢に多軸ロボット３０の先端３８が変化すると、多軸ロボット３０と障害物との衝突が発生し得る。従って、中間点Ｃ１と中間点Ｃｉ＋１の各々が示す目標位置姿勢に先端３８の位置姿勢を厳密な精度で一致させることは困難である。この場合、中間点Ｃ１と中間点Ｃｉ＋１の各々について、目標位置姿勢に代替する補正位置姿勢が位置姿勢補正部２２によって求められる（探索される）。
一実施形態では、中間点Ｃ１の目標位置姿勢に対応する各軸状態量が、各軸状態量の候補値に設定される。位置姿勢補正部２２は、該各軸状態量の候補値を更新することで、中間点Ｃ１における補正位置姿勢を求める（探索する）。中間点Ｃｉ＋１においても同様の手法によって、中間点Ｃｉにおける補正位置姿勢が求められる。一実施形態では、補正位置姿勢における判定対象パラメータの値は許容範囲に収まるので、補正位置姿勢が目標位置姿勢から乖離し過ぎることは抑制される。
図２で例示される補正後の中間点Ｃ１と中間点Ｃｉ＋１とにおいては、６つの位置姿勢パラメータのうちで姿勢パラメータ（ＲＸ、ＲＹ、またはＲＺ）の少なくともいずれかが補正される。なお、他の実施形態では、位置パラメータ（Ｘ、Ｙ、またはＺ）の少なくともいずれかが補正されてもよいし、６つの位置姿勢パラメータが全て補正されてもよい。
このように、先端３８の位置姿勢を目標位置姿勢に厳密な精度で一致させることが困難である場合、目標位置姿勢を示す６つの位置姿勢パラメータのうち少なくとも１つが変更された補正位置姿勢が求められる。この場合、先端３８が目標位置姿勢に変化するための制御が実行されることで、先端３８の位置姿勢は補正位置姿勢に変化する。
【0029】
位置姿勢補正部２２の補正対象となる位置姿勢パラメータは、補正位置姿勢の探索中、終始同じであるとは限らない。つまり、補正対象となる位置姿勢パラメータは、中間位置姿勢の探索状況または探索条件などに応じて、動的に切り替えることが出来る。例えば、探索回数が増えるに従い（あるいは距離パラメータが満たすべき閾値が小さくなるに従い）、補正対象となる位置姿勢パラメータが増えてもよいし、補正対象となる位置姿勢パラメータの種別が変化するが個数そのものは一定であってもよい。あるいは、これら２つの組み合わせが実行されてもよい。
より具体的な一例として、補正位置姿勢の探索回数が規定回数に達するまでの間はＲｘのみが補正対象となる。探索回数が規定回数に達する前に、Ｒｘのみが補正された適正な補正位置姿勢（つまり、距離パラメータが閾値を超える中間位置姿勢）が特定されれば、探索は終了する。このとき、補正前の位置姿勢に極めて近い補正位置姿勢を特定できる。一方で、探索回数が規定回数に達しても、適正な補正位置姿勢（衝突回避等が可能な補正位置姿勢）が見つからなければ、Ｒｘに加えて（あるいはＲｘに代えて）、例えばＲＹ及びＲＺが補正対象として追加できる。これにより、適正な中間位置姿勢が特定できない不具合を回避できる。
以上のように、可能な限り目標位置姿勢に近い補正中間位置姿勢を求める（探索する）ことが出来る上、衝突回避等が可能な補正位置姿勢を見つける確率を上げることが出来る。
より具体的な一例として、補正位置姿勢の探索回数が規定回数に達するまでの間はＲｘのみが補正対象となる。探索回数が規定回数に達する前に、Ｒｘのみが補正された適正な補正位置姿勢（つまり、距離パラメータが閾値を超える中間位置姿勢）が特定されれば、探索は終了する。このとき、補正前の位置姿勢に極めて近い補正位置姿勢を特定できる。一方で、探索回数が規定回数に達しても、適正な補正位置姿勢（衝突回避等が可能な補正位置姿勢）が見つからなければ、Ｒｘに加えて（あるいはＲｘに代えて）、例えばＲＹ及びＲＺが補正対象として追加できる。これにより、適正な中間位置姿勢が特定できない不具合を回避できる。
以上のように、可能な限り目標位置姿勢に近い補正中間位置姿勢を求める（探索する）ことが出来る上、衝突回避等が可能な補正位置姿勢を見つける確率を上げることが出来る。
【0030】
図３は、一実施形態に係る位置姿勢補正部２２の構成を示す図である。
位置姿勢補正部２２は、第１行列を取得するための第１行列取得部１１、第２行列を取得するための第２行列取得部１２、および、第１行列と第２行列に基づき各軸状態量の候補値を更新するための更新部１３を含む。各軸状態量の候補値が更新部１３によって更新されることで、位置姿勢補正部２２は補正位置姿勢を求める。
位置姿勢補正部２２は、第１行列を取得するための第１行列取得部１１、第２行列を取得するための第２行列取得部１２、および、第１行列と第２行列に基づき各軸状態量の候補値を更新するための更新部１３を含む。各軸状態量の候補値が更新部１３によって更新されることで、位置姿勢補正部２２は補正位置姿勢を求める。
【0031】
一実施形態では、第１行列取得部１１によって取得される第１行列は、多軸ロボット３０各軸状態量に対する第１パラメータの感度を少なくとも示す。第１パラメータは、位置姿勢パラメータの一部のパラメータである。一例として第１パラメータは、３つの位置パラメータ（Ｘ、Ｙ、およびＺ）と、１つの姿勢パラメータ（ＲＺ）を含む。この場合、各軸状態量ベクトルｑがｑ１～ｑ６の６個の要素を含む実施形態では、第１行列は、以下の式（６）のＪ１ｓによって規定される。
【数2】
【0032】
一実施形態では、第２行列によって取得される第２行列は、多軸ロボット３０の各軸状態量に対する第２パラメータの感度を少なくとも示す。第２パラメータは、各軸状態量に応じて変化するパラメータであり、且つ、位置姿勢パラメータとは異なるパラメータである。第２パラメータは、一例として、距離パラメータである。一実施形態の第２行列は、ｍ個の距離パラメータの各軸状態量に対する感度を示す。この場合、各軸状態量ベクトルｑがｑ１～ｑ６の６個の要素を含む実施形態では、第２行列は、以下の式（７）のＪ２ｓによって規定される。
【数3】
【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】
（６）幾つかの実施形態では、上記（１）から（５）のいずれかの構成において、
前記第１行列取得部１１は、前記各軸状態量の最新の前記候補値における、前記各軸状態量に対する前記第１パラメータの感度を示す前記第１行列を取得するように構成され、
前記第２行列取得部１２は、前記各軸状態量の最新の前記候補値における、前記各軸状態量に対する前記第２パラメータの感度を示す前記第２行列を取得するように構成される。
前記第１行列取得部１１は、前記各軸状態量の最新の前記候補値における、前記各軸状態量に対する前記第１パラメータの感度を示す前記第１行列を取得するように構成され、
前記第２行列取得部１２は、前記各軸状態量の最新の前記候補値における、前記各軸状態量に対する前記第２パラメータの感度を示す前記第２行列を取得するように構成される。
【0076】
上記（６）の構成によれば、更新部１３による候補値の更新の際には、更新対象となっている各軸状態量の候補値における第１偏差と第２偏差が強く反映される。これにより、更新された各軸状態量に対応する第１パラメータと第２パラメータが目標位置姿勢の位置姿勢パラメータから乖離し過ぎることが抑制される。従って、ロボット制御処理における更新回数ｋは低減し、判定対象パラメータの値が許容範囲に収まる補正位置姿勢を求めるための処理の効率が向上する。
【0077】
（７）本発明の少なくとも一実施形態に係る多軸ロボット用のロボットシステム１は、
上記（１）から（６）のいずれかに記載の多軸ロボット用の制御装置２０と、
前記多軸ロボット３０と
を備える。
上記（１）から（６）のいずれかに記載の多軸ロボット用の制御装置２０と、
前記多軸ロボット３０と
を備える。
【0078】
上記（７）の構成によれば、上記（１）と同様の理由により、多軸ロボット３０の先端３８が目標位置姿勢に変化するための制御を柔軟に実行できる多軸ロボット用のロボットシステム１が実現する。
【0079】
（８）本発明の少なくとも一実施形態に係る多軸ロボット用の制御プログラム９５は、
コンピュータに、
多軸ロボット３０の先端３８の目標位置姿勢に代替する補正位置姿勢を求めるための位置姿勢補正ステップを実行させ、
前記位置姿勢補正ステップは、
前記多軸ロボット３０の各軸状態量に対する、前記多軸ロボット３０の前記先端３８の位置姿勢を示す複数の位置姿勢パラメータのうち第１パラメータの感度を少なくとも示す第１行列を取得するための第１行列取得ステップと、
前記各軸状態量に対する、前記位置姿勢パラメータとは異なる第２パラメータの感度を少なくとも示す第２行列を取得するための第２行列取得ステップと、
前記各軸状態量の候補値に対応する前記第１パラメータの前記目標位置姿勢を基準とした偏差を含む第１偏差と、前記各軸状態量の前記候補値に対応する前記第２パラメータの目標値を基準とした偏差を含む第２偏差と、前記第１行列および前記第２行列とを用いて、前記各軸状態量の前記候補値を更新するための更新ステップとを含む。
コンピュータに、
多軸ロボット３０の先端３８の目標位置姿勢に代替する補正位置姿勢を求めるための位置姿勢補正ステップを実行させ、
前記位置姿勢補正ステップは、
前記多軸ロボット３０の各軸状態量に対する、前記多軸ロボット３０の前記先端３８の位置姿勢を示す複数の位置姿勢パラメータのうち第１パラメータの感度を少なくとも示す第１行列を取得するための第１行列取得ステップと、
前記各軸状態量に対する、前記位置姿勢パラメータとは異なる第２パラメータの感度を少なくとも示す第２行列を取得するための第２行列取得ステップと、
前記各軸状態量の候補値に対応する前記第１パラメータの前記目標位置姿勢を基準とした偏差を含む第１偏差と、前記各軸状態量の前記候補値に対応する前記第２パラメータの目標値を基準とした偏差を含む第２偏差と、前記第１行列および前記第２行列とを用いて、前記各軸状態量の前記候補値を更新するための更新ステップとを含む。
【0080】
上記（８）の構成によれば、上記（１）と同様の理由により、多軸ロボット３０の先端３８が目標位置姿勢に変化するための制御を柔軟に実行できる多軸ロボット用の制御プログラム９５が実現する。
【0081】
上記説明では、図９において、Ｓ１７で許容条件を満たした位置姿勢を示す点と、Ｓ２３において決定される中間点Ｃｉが一致する実施形態を説明した。別の実施形態では、Ｓ１７で許容条件を満たすと判定され、かつ、多軸ロボット３０の修正軌道に含まれる複数の任意の中間点Ｃｉ（例えば５個の中間点）について、重み係数を付与し、その任意の数の中間点Ｃｉにおける位置姿勢を微調整することもできる（つまり、修正軌道を微調整することもできる）。微調整後の中間点はいずれも許容条件を満たすことが好ましい。ただし、Ｓ１７で設定される許容条件を満たさないものの、当該許容条件より緩い衝突回避条件を満たす微調整後の中間点が採用されることがあってもよい。
これにより、ある中間点Ｃｉは、その中間点よりも下流側にある任意の複数の中間点Ｃｉの位置を考慮に入れた位置となり、多軸ロボット３０の軌道を滑らかにできる。従って、多軸ロボット３０の動作がスムーズになる。
これにより、ある中間点Ｃｉは、その中間点よりも下流側にある任意の複数の中間点Ｃｉの位置を考慮に入れた位置となり、多軸ロボット３０の軌道を滑らかにできる。従って、多軸ロボット３０の動作がスムーズになる。
【符号の説明】
【0082】
１ ：ロボットシステム
１１ ：第１行列取得部
１２ ：第２行列取得部
１３ ：更新部
２０ ：制御装置
２２ ：位置姿勢補正部
２４ ：判定部
３０ ：多軸ロボット
３８ ：先端
９５ ：制御プログラム
ｑ１～ｑ６ ：軸状態量
１１ ：第１行列取得部
１２ ：第２行列取得部
１３ ：更新部
２０ ：制御装置
２２ ：位置姿勢補正部
２４ ：判定部
３０ ：多軸ロボット
３８ ：先端
９５ ：制御プログラム
ｑ１～ｑ６ ：軸状態量
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】