特許 7443572 サーボ制御方法、プロセッサ、記憶媒体及び可動ステージ

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

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-02-26

(45)【発行日】2024-03-05

(54)【発明の名称】サーボ制御方法、プロセッサ、記憶媒体及び可動ステージ

(51)【国際特許分類】

   G05D 3/12 20060101AFI20240227BHJP

   G05D 1/244 20240101ALI20240227BHJP

   G05D 1/43 20240101ALI20240227BHJP

【ＦＩ】

G05D3/12 L

G05D1/244

G05D1/43

【請求項の数】 15

(21)【出願番号】P 2022574677

(86)(22)【出願日】2021-06-03

(65)【公表番号】

(43)【公表日】2023-07-06

(86)【国際出願番号】 CN2021098060

(87)【国際公開番号】W WO2021244598

(87)【国際公開日】2021-12-09

【審査請求日】2022-12-02

(31)【優先権主張番号】202010492674.3

(32)【優先日】2020-06-03

(33)【優先権主張国・地域又は機関】CN

(73)【特許権者】

【識別番号】522470921

【氏名又は名称】杭州迦智科技有限公司

(74)【代理人】

【識別番号】100142365

【弁理士】

【氏名又は名称】白井 宏紀

(72)【発明者】

【氏名】孫逸超

(72)【発明者】

【氏名】戴舒▲い▼

(72)【発明者】

【氏名】伍浩賢

(72)【発明者】

【氏名】劉豊

【審査官】田中 友章

(56)【参考文献】

【文献】特開２００９－２４４９６５（ＪＰ，Ａ）

【文献】特開平５－１７３６３６（ＪＰ，Ａ）

【文献】米国特許第９１５７７５７（ＵＳ，Ｂ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００

Ｇ０５Ｄ ３／００

(57)【特許請求の範囲】

【請求項1】

所定の目標位置姿勢を決定するステップと、
少なくとも１つの識別子を取得し、現在の位置姿勢を取得するステップと、
前記所定の目標位置姿勢と前記現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定するステップと、
前記識別子及び前記所定の目標位置姿勢に基づいて目標直線を決定し、前記目標直線と前記現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定するステップと、
前記オフセット変位及び前記オフセット角度に基づいて、制御指令を決定するステップと、を含み、
所定の規則的な間隔に基づいて、少なくとも同一の識別子を取得し、各所定の規則的な間隔内に前記識別子に基づいて現在時刻の現在の位置姿勢を取得し、制御指令を取得し、各前記所定の規則的な間隔内に取得された制御指令を利用して現在の位置姿勢に対してサーボ制御を行う、サーボ制御方法。

【請求項2】

少なくとも１つの識別子を取得し、現在の位置姿勢を取得する前記ステップは、
前記識別子に基づいて識別子座標系Ｏ_ｗ－ｘｙｚを確立し、現在の位置姿勢の前記識別子座標系Ｏ_ｗ－ｘｙｚでの変位と角度の座標を取得するステップを含む、請求項１に記載のサーボ制御方法。

【請求項3】

前記識別子は、地面に垂直に設置された識別子である、請求項１に記載のサーボ制御方法。

【請求項4】

前記所定の目標位置姿勢と前記現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定する前記ステップは、
前記所定の目標位置姿勢の前記識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢、及び前記現在の位置姿勢の前記識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢に基づいて、前記所定の目標位置姿勢と前記現在の位置姿勢との間の相対的な位置姿勢関係を取得し、オフセット角度を決定するステップを含む、請求項２に記載のサーボ制御方法。

【請求項5】

前記識別子及び前記所定の目標位置姿勢に基づいて目標直線を決定し、前記目標直線と前記現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定する前記ステップは、
前記所定の目標位置姿勢の変位座標に基づいて仮想目標点Ｏ′を作成し、前記所定の目標位置姿勢に基づいて所定の目標点座標系Ｏ_ｏ－ｘｙｚを確立するステップと、
前記仮想目標点に基づいて仮想目標位置姿勢を作成するステップと、
前記仮想目標位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標、及び前記現在の位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標に基づいて、前記現在の位置姿勢と前記仮想目標位置姿勢との相対的な位置姿勢関係を取得し、前記識別子及び前記所定の目標位置姿勢に基づいて前記目標直線を決定し、前記目標直線に垂直な方向上の前記現在の位置姿勢と前記仮想目標位置姿勢との偏差変位を決定するステップとを含む、請求項２に記載のサーボ制御方法。

【請求項6】

前記仮想目標位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度に基づくことは、
前記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立し、前記仮想目標点座標系Ｏ_ｏ′－ｘｙｚと前記所定の目標点座標系Ｏ_ｏ－ｘｙｚの座標軸原点の変位及び座標軸の角度偏差に基づいて、前記仮想目標位置姿勢と前記所定の目標位置姿勢との相対的な位置姿勢関係を決定することを含む、請求項５に記載のサーボ制御方法。

【請求項7】

前記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立する前記ステップは、
前記識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と前記所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点とのベクトル関係

、前記識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と点Ｐとのベクトル関係、及び前記所定の目標点座標系Ｏ_ｏ－ｘｙｚと前記識別子座標系Ｏ_ｗ－ｘｙｚとの変換関係に基づいて、前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの前記仮想目標位置姿勢の前記目標直線方向上の単位ベクトル

を取得して、前記仮想目標点座標系のＹ座標軸とするステップであって、前記点Ｐは、前記識別子座標系のＺ軸方向に所定の目標点から単位ベクトル離れることにより取得されるものであるステップを含み、
地面に垂直な方向に基づいて単位ベクトルを取得して、前記仮想目標点座標系のＺ座標軸とし、空間直交座標系原則に基づいて、前記目標直線に垂直な仮想目標点座標系のＸ座標軸を取得する、請求項６に記載のサーボ制御方法。

【請求項8】

プログラムコードを呼び出すように構成されるサーボ制御用プロセッサであって、前記プログラムコードが実行される場合、
所定の目標位置姿勢を決定する操作と、少なくとも１つの識別子を取得し、現在の位置姿勢を取得する操作と、前記所定の目標位置姿勢と前記現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定する操作と、前記識別子及び前記所定の目標位置姿勢に基づいて目標直線を決定し、前記目標直線と前記現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定する操作と、前記オフセット変位及び前記オフセット角度に基づいて、サーボ制御を行う操作と、を実行し、
前記プロセッサは、所定の規則的な間隔に基づいて、少なくとも同一の識別子を取得し、各所定の規則的な間隔内に前記識別子に基づいて現在時刻の現在の位置姿勢を取得し、制御指令を取得し、各前記所定の規則的な間隔内に取得された制御指令を利用して現在の位置姿勢に対してサーボ制御を行う、プロセッサ。

【請求項9】

前記プロセッサは、さらに、前記識別子に基づいて識別子座標系Ｏ_ｗ－ｘｙｚを確立し、現在の位置姿勢の前記識別子座標系Ｏ_ｗ－ｘｙｚでの変位と角度の座標を取得するように構成される、請求項８に記載のプロセッサ。

【請求項10】

前記プロセッサは、さらに、前記所定の目標位置姿勢の前記識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢、及び前記現在の位置姿勢の前記識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢に基づいて、前記所定の目標位置姿勢と前記現在の位置姿勢との間の相対的な位置姿勢関係を取得し、オフセット角度を決定するように構成される、請求項９に記載のプロセッサ。

【請求項11】

前記プロセッサは、さらに、前記所定の目標位置姿勢の変位座標に基づいて仮想目標点Ｏ′を作成し、前記所定の目標位置姿勢に基づいて所定の目標点座標系Ｏｏ－ｘｙｚを確立し、前記仮想目標点に基づいて仮想目標位置姿勢を作成し、前記仮想目標位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標、及び前記現在の位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標に基づいて、前記現在の位置姿勢と前記仮想目標位置姿勢との相対的な位置姿勢関係を取得し、前記識別子及び前記所定の目標位置姿勢に基づいて前記目標直線を決定し、前記目標直線に垂直な方向上の前記現在の位置姿勢と前記仮想目標位置姿勢との偏差変位を決定するように構成される、請求項９に記載のプロセッサ。

【請求項12】

前記プロセッサは、さらに、前記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立し、前記仮想目標点座標系Ｏ_ｏ′－ｘｙｚと前記所定の目標点座標系Ｏ_ｏ－ｘｙｚの座標軸原点の変位及び座標軸の角度偏差に基づいて、前記仮想目標位置姿勢と前記所定の目標位置姿勢との相対的な位置姿勢関係を決定するように構成される、請求項１１に記載のプロセッサ。

【請求項13】

前記プロセッサは、さらに、前記識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と前記所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点とのベクトル関係

、前記識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と点Ｐとのベクトル関係、及び前記所定の目標点座標系Ｏ_ｏ－ｘｙｚと前記識別子座標系Ｏ_ｗ－ｘｙｚとの変換関係に基づいて、前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの前記仮想目標位置姿勢の前記目標直線方向上の単位ベクトル

を取得して、前記仮想目標点座標系のＹ座標軸とし、前記点Ｐは、前記識別子座標系のＺ軸方向に所定の目標点から単位ベクトル離れることにより取得され、地面に垂直な方向に基づいて単位ベクトルを取得して、前記仮想目標点座標系のＺ座標軸とし、空間直交座標系原則に基づいて、前記目標直線に垂直な仮想目標点座標系のＸ座標軸を取得するように構成される、請求項１２に記載のプロセッサ。

【請求項14】

コンピュータ指令が記憶されており、前記コンピュータ指令が実行される場合、請求項１～７のいずれか一項に記載のサーボ制御方法を実現する、記憶媒体。

【請求項15】

識別子を取得するように構成される収集器を含み、さらに、請求項８～１３のいずれか一項に記載のプロセッサ、又は、請求項１４に記載の記憶媒体を含む、可動ステージ。

【発明の詳細な説明】

【関連出願の相互参照】

【0001】

本開示は、２０２０年６月３日に提出された、出願番号が２０２０１０４９２６７４．３で、名称が「サーボ制御方法、プロセッサ、記憶媒体及び可動ステージ」である中国特許出願の優先権を主張するものであり、その全ての内容は参照により本開示に組み込まれるものとする。

【技術分野】

【0002】

本開示は、制御の技術分野に属し、具体的には、サーボ制御方法、プロセッサ、記憶媒体及び可動ステージに関する。

【背景技術】

【0003】

現代の物流システム、倉庫保管システム又は工業生産ラインは、精度に対する要求がますます高まっている。工場がスマート化、無人化の工場に向けて転換して発展しており、個々の部品の製造コストが高いため、工場の各製造設備は、非常に高い動作精度が要求されてこそ、工場のシステム全体の高ロバストな閉ループ動作が可能となる。可動ステージは、生産ライン間の搬送装置として、突合せ精度がミリメートル級以上に達しなければ、搬送指標の要件を満たすことができない。

【0004】

従来の可動ステージにおいて、差動シャーシ又はダブルステアリングホイールシャーシのような３自由度で位置姿勢を迅速に調整することが困難なシャーシに基づく移動ステージは、広く用いられているが、３自由度で位置姿勢を迅速に調整することが可能なオムニホイールシャーシに比べて、上記タイプのシャーシには、いくつかの限界がある。例えば、差動シャーシに対して、非ホロノミック拘束の制限のため、狭い空間で横方向の誤差を迅速に収束することができず、ダブルステアリングホイールシャーシに対して、設計自体の制限のため、ミリメートル級の高精度な横方向突合せを行う場合、高頻度の全方向調整は、舵角を頻繁に変化させるため、運動学理論モデルを正確に満たすことができず、同時に耐用年数を低下させる。いくつかの適用シーンに対して、従来の調整方法は、常にホロノミック拘束により、目標点に到達するように正確に調整し、計算過程が煩雑で、効率が低いが、実際に、いくつかの次元の誤差を無視し、かつ同じ目的を達成することができ、これにより、計算及び実行過程を簡略化し、実行効率を向上させる。

【0005】

上記問題に対して、現在では、効果的な解決手段はまだ提案されていない。

【発明の概要】

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

【0006】

本開示は、上記従来製品に存在する問題に対して、非ホロノミック拘束を用いて行われた高精度、高効率のサーボ制御方法、プロセッサ、記憶媒体及び可動ステージを提供する。

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

【0007】

本開示に係るサーボ制御方法は、
所定の目標位置姿勢を決定するステップと、
少なくとも１つの識別子を取得し、現在の位置姿勢を取得するステップと、
所定の目標位置姿勢と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定するステップと、
前記識別子及び所定の目標位置姿勢に基づいて目標直線を取得し、前記目標直線と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定するステップと、
前記オフセット変位及びオフセット角度に基づいて、制御指令を取得するステップと、を含む。

【0008】

本開示のいくつかの実施例では、所定の規則的な間隔で少なくとも同一の識別子を取得し、各所定の規則的な間隔内に前記識別子に基づいて現在時刻の現在の位置姿勢を取得し、制御指令を取得し、各所定の規則的な間隔内に取得された制御指令を利用して現在の位置姿勢に対してサーボ制御を行う。

【0009】

本開示のいくつかの実施例では、少なくとも１つの識別子を取得し、現在の位置姿勢を取得するステップは、前記識別子に基づいて識別子座標系Ｏ_ｗ－ｘｙｚを確立し、現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの変位と角度の座標を取得するステップを含む。

【0010】

本開示のいくつかの実施例では、前記識別子は、地面に垂直に設置された識別子である。

【0011】

本開示のいくつかの実施例では、所定の目標位置姿勢と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定するステップは、前記所定の目標位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢、及び前記現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢に基づいて、所定の目標位置姿勢と現在の位置姿勢との間の相対的な位置姿勢関係を取得し、オフセット角度を抽出するステップを含む。

【0012】

本開示のいくつかの実施例では、識別子及び所定の目標位置姿勢に基づいて目標直線を取得し、前記目標直線と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定するステップは、前記所定の目標位置姿勢の変位座標に基づいて仮想目標点Ｏ′を作成し、所定の目標位置姿勢に基づいて所定の目標点座標系Ｏ_ｏ－ｘｙｚを確立するステップと、前記仮想目標点に基づいて仮想目標位置姿勢を作成するステップと、前記仮想目標位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標、及び現在の位置姿勢の所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標に基づいて、現在の位置姿勢と仮想目標位置姿勢との相対的な位置姿勢関係を取得し、識別子及び所定の目標位置姿勢に基づいて目標直線を取得し、目標直線に垂直な方向上の現在の位置姿勢と仮想目標位置姿勢との偏差変位を抽出するステップとを含む。

【0013】

本開示のいくつかの実施例では、前記仮想目標位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度に基づく座標は、前記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立し、前記仮想目標点座標系Ｏ_ｏ′－ｘｙｚと所定の目標点座標系Ｏ_ｏ－ｘｙｚの座標軸原点の変位及び座標軸の角度偏差に基づいて、前記仮想目標位置姿勢と所定の目標位置姿勢との相対的な位置姿勢関係を計算するステップを含む。

【0014】

本開示のいくつかの実施例では、前記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立するステップは、前記識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点とのベクトル関係

、識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と識別子座標系のＺ軸方向に所定の目標点から単位ベクトル離れることにより取得された点Ｐとのベクトル関係、及び前記所定の目標点座標系Ｏ_ｏ－ｘｙｚと前記識別子座標系Ｏ_ｗ－ｘｙｚとの変換関係に基づいて、所定の目標点座標系Ｏ_ｏ－ｘｙｚでの仮想目標位置姿勢の目標直線方向上の単位ベクトル

を取得し、仮想目標点座標系のＹ座標軸とするステップと、地面に垂直な方向に基づいて単位ベクトルを取得して、仮想目標点座標系のＺ座標軸とするステップと、空間直交座標系原則に基づいて、前記目標直線に垂直な仮想目標点座標系のＸ座標軸を取得するステップとを含む。

【0015】

本開示に係るサーボ制御用プロセッサは、プログラムコードを呼び出すように構成され、前記プログラムコードが実行される場合、前記プロセッサは、所定の目標位置姿勢を決定する操作と、少なくとも１つの識別子を取得し、現在の位置姿勢を取得する操作と、所定の目標位置姿勢と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定する操作と、識別子及び所定の目標位置姿勢に基づいて目標直線を取得し、前記目標直線と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定する操作と、前記オフセット変位及びオフセット角度に基づいて、サーボ制御を行う操作と、を実行する。

【0016】

本開示のいくつかの実施例では、前記プロセッサは、さらに前記識別子に基づいて識別子座標系Ｏ_ｗ－ｘｙｚを確立し、現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの変位と角度の座標を取得するように構成される。

【0017】

本開示のいくつかの実施例では、前記プロセッサは、所定の規則的な間隔に基づいて、少なくとも同一の識別子を取得し、各所定の規則的な間隔内に前記識別子に基づいて現在時刻の現在の位置姿勢を取得し、制御指令を取得し、各所定の規則的な間隔内に取得された制御指令を利用して現在の位置姿勢に対してサーボ制御を行う。

【0018】

本開示のいくつかの実施例では、前記プロセッサは、さらに前記所定の目標位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢、及び前記現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢に基づいて、所定の目標位置姿勢と現在の位置姿勢との間の相対的な位置姿勢関係を取得し、オフセット角度を抽出するように構成される。

【0019】

本開示のいくつかの実施例では、前記プロセッサは、さらに、前記所定の目標位置姿勢の変位座標に基づいて仮想目標点Ｏ′を作成し、所定の目標位置姿勢に基づいて所定の目標点座標系Ｏ_ｏ－ｘｙｚを確立し、前記仮想目標点に基づいて仮想目標位置姿勢を作成し、前記仮想目標位置姿勢の前記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標、及び現在の位置姿勢の所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標に基づいて、現在の位置姿勢と仮想目標位置姿勢との相対的な位置姿勢関係を取得し、識別子及び所定の目標位置姿勢に基づいて目標直線を取得し、目標直線に垂直な方向上の現在の位置姿勢と仮想目標位置姿勢との偏差変位を抽出するように構成される。

【0020】

本開示のいくつかの実施例では、前記プロセッサは、さらに、前記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立し、前記仮想目標点座標系Ｏ_ｏ′－ｘｙｚと所定の目標点座標系Ｏ_ｏ－ｘｙｚの座標軸原点の変位及び座標軸の角度偏差に基づいて、前記仮想目標位置姿勢と所定の目標位置姿勢との相対的な位置姿勢関係を計算するように構成される。

【0021】

本開示のいくつかの実施例では、前記プロセッサは、さらに前記識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点とのベクトル関係

、及び識別子座標系Ｏ_ｗ－ｘｙｚでの前記識別子と識別子座標系のＺ軸方向に所定の目標点から単位ベクトル離れることにより取得された点Ｐとのベクトル関係、及び前記所定の目標点座標系Ｏ_ｏ－ｘｙｚと前記識別子座標系Ｏ_ｗ－ｘｙｚとの変換関係に基づいて、所定の目標点座標系Ｏ_ｏ－ｘｙｚでの仮想目標位置姿勢の目標直線方向上の単位ベクトル

を取得して、仮想目標点座標系のＹ座標軸とし、地面に垂直な方向に基づいて単位ベクトルを取得して、仮想目標点座標系のＺ座標軸とし、空間直交座標系原則に基づいて、前記目標直線に垂直な仮想目標点座標系のＸ座標軸を取得するように構成される。

【0022】

本開示に係る記憶媒体には、コンピュータ指令が記憶されており、前記コンピュータ指令が実行される場合、上記いずれかのサーボ制御方法を実現する。

【0023】

本開示に係る可動ステージは、識別子を取得するように構成される収集器を含み、さらに、上記いずれかのプロセッサ、又は、上記記憶媒体を含む。

【発明の効果】

【0024】

以上の技術を採用した後、本開示は、従来技術に比べて、以下の利点を有する。

【0025】

本開示は、非ホロノミック拘束で、オフセット角度及びオフセット変位のみを計算して決定することでサーボ制御を行うことにより、制御対象を目標直線に到達するように調整する。目標点に正確に到達するではなく、目標直線に到達するように調整するため、本発明の計算過程は、調整角度及び１つの変位成分のみを計算し、調整方法が簡単であり、計算量が小さく、同時に目標直線及び角度拘束により高精度を保証する。本発明は、非全方向移動シャーシの調整過程における不足を克服する。本開示は、ある次元の誤差を無視し、他の自由度を利用して補償して、ホロノミック拘束での３自由度で位置姿勢を迅速に調整することが可能なオムニホイールシャーシのような目標点に迅速かつ正確に到達するという効果を達成する。

【図面の簡単な説明】

【0026】

ここで説明された図面は、本開示をさらに理解するためのものであり、本願の一部を構成し、本開示の例示的な実施例及びその説明は、本開示を解釈することに用いられ、本開示を不当に限定するものではない。

【0027】

【図1】本開示のフローチャートである。

【図2】本開示のサーボ制御方法の概略図である。

【図3】本開示の方法の概略図である。

【図4】本開示の可動ステージの構造図である。

【発明を実施するための形態】

【0028】

以下、図面及び具体的な実施例を参照しながら本開示をさらに説明する。本開示は、可動ステージに対して高精度突合せを行うシーンに適用されてよく、他の非ホロノミック拘束状況での高精度位置決めシーンに適用されてもよい。

【0029】

図４は、本開示の可動ステージの構造図であり、可動ステージに対して高精度突合せを行うシーンの実施例では、図４に示すような、識別子Ｗを取得する収集器を含み、さらに上記プロセッサ又は上記記憶媒体を含む可動ステージに適用することができる。上記可動ステージは、本体に取り付けられた突合せ装置及び移動シャーシを含む。上記移動シャーシは、非全方向移動シャーシを含むが、それに限定されず、図４に示すような可動ステージにおいて、高頻度振動しやすいダブルステアリングホイールシャーシを採用するため、任意の方向に柔軟に前進することができず、その制御難しさがより大きい。本開示に記載の方法により、制御難しさを最大限に減少させ、誤差を縮小することができる。

【0030】

より具体的には、上記突合せ装置は、伸縮式フォークアームであり、上記突合せ装置の突出長さは、現場の需要に応じて調整することができる。上記伸縮式フォークアームには、フォークアームの突出長さを識別するセンサが取り付けられる。突出長さを調整することができるため、上記可動ステージの突合せ装置が目標直線上の１つの点に到達するように調整し、突合せ装置を目標角度で伸縮させれば、高精度突合せを実現することができる。計算方法が簡便である。図４に示すような可動ステージにおいて、正確な突合せを実現するために、本開示に記載の方式により、突合せ装置の突合せ精度を±２ｍｍに制御することができ、高精度突合せを保証する。

【0031】

上記収集器は、上記突合せ装置に取り付けられ、突合せ装置と上記車体との間に機械的な誤差が存在することを回避することができる。

【0032】

具体的な実施過程において、可動ステージを目標位置姿勢の近傍にナビゲーションし、収集器の視野範囲内に位置させ、識別子Ｗに基づいて可動ステージに対してサーボ制御を行うことにより、目標位置姿勢に到達するように高精度に調整する。

【0033】

図１は、本開示のフローチャートであり、図１に示すように、本開示に係るサーボ制御方法は、以下のステップ１０１～ステップ１０５を含む。

【0034】

ステップ１０１では、所定の目標位置姿勢を決定する。

【0035】

本開示において、所定の目標位置姿勢は、教示の方式で、可動ステージを目標位置姿勢に移動させるように制御してから、識別子Ｗを読み取ることにより実現することができる。上記所定の目標位置姿勢は、必ずしも上記可動ステージが実際に到達した目標位置姿勢ではなく、オフセット角度及びオフセット変位の計算を実現するためのものに過ぎない。

【0036】

ステップ１０２では、少なくとも１つの識別子Ｗを取得し、現在の位置姿勢を取得し、
収集器から収集された識別子Ｗを含み、上記識別子Ｗは、２次元コードであってよく、位置決め用の他のパターン又は特徴であってもよい。さらに、該収集器は、収集の角度、タイプモデル、収集頻度に限定されない。収集器は、複数回の収集を行うことにより、即時性及び正確さを保証する。収集器の収集方式は、一定の移動距離ごとに上記識別子Ｗを１回収集し、現在の位置姿勢を計算することを含むが、これらに限定されない。図２は、本開示のサーボ制御方法の概略図であり、図２に示すように、現在の位置姿勢は、図２のＥ点で示す。上記識別子Ｗに基づいて識別子座標系を確立し、さらに３次元座標系を確立し、かつ換算する方式を採用して、現在の位置姿勢を計算して取得することができる。上記位置姿勢は、位置及び姿勢を含む。

【0037】

ステップ１０３では、所定の目標位置姿勢と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度Δθを決定し、
上記所定の目標位置姿勢と現在の位置姿勢を同一の３次元座標系に変換し、相違する空間角度、すなわち相対的な位置姿勢関係を決定し、オフセット角度Δθを決定することができる。上記オフセット角度は、可動ステージが目標位置姿勢に移動するために回転する必要がある角度である。

【0038】

ステップ１０４では、識別子Ｗ及び所定の目標位置姿勢に基づいて目標直線を取得し、上記目標直線と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位ΔＸを決定し、
図２に示すように、上記目標直線は、図２のＬで示され、上記目標直線は、可動ステージが最終的に移動した位置の線形拘束である。上記可動ステージの目標位置姿勢は、上記目標直線に位置する任意の可能な位置であり、上記所定の目標位置姿勢は、上記目標直線に位置する１つの具体的な点である。上記目標直線と上記現在の位置姿勢は、同じ識別子Ｗを読み取ることにより取得され、誤差がより小さく、精度がより高い。

【0039】

ステップ１０５では、上記オフセット変位ΔＸ及びオフセット角度Δθに基づいて制御指令を取得する。

【0040】

精度をさらに向上させるために、上記実行されたステップを複数回に分割して実行することができる。より具体的には、所定の規則的な間隔で少なくとも同一の識別子Ｗを取得し、各所定の規則的な間隔内に上記識別子Ｗに基づいて現在時刻の現在の位置姿勢を取得し、制御指令を取得し、各所定の規則的な間隔内に取得された制御指令を利用して現在の位置姿勢に対してサーボ制御を行う。上記所定の規則的な間隔は、一定の時間間隔であってよく、一定の距離間隔であってもよく、又は他の形式であってもよい。所定の規則的な間隔は、周期が短く、頻度が高いほど、制御指令により目標位置姿勢に到達するように制御される精度が高くなる。

【0041】

図２に示すように、経路Ｓは、上記サーボ制御において発生する可能性のある実際の経路であり、目標位置姿勢Ｔは、上記サーボ制御において到達する可能性のある実際の位置姿勢である。その実現過程は、少なくとも１つの識別子Ｗのみを識別することにより、角度及び変位を同時に調整することができ、単独で繰り返して調整する必要がない。

【0042】

ステップ１０２では、少なくとも１つの識別子Ｗを取得し、現在の位置姿勢を取得する前記ステップは、上記識別子Ｗに基づいて識別子座標系Ｏ_ｗ－ｘｙｚを確立し、現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの変位及び姿勢を取得するステップを含む。登録番号がＣＮ１１０２０９１６９Ａで、名称が「可動ステージの制御方法、制御装置及び可動ステージ」である特許に開示された方法を採用し、計算して取得することができる。上記識別子と上記突合せ装置の突合せ対象の設備との間の相対的な位置姿勢関係は、一定である。本実施例では、１つの識別子を採用する方式のみを例示し、同様に複数の識別子が存在する他の拡張方式を証明することができる。

【0043】

本実施例では、上記識別子Ｗは、地面に垂直に設置された識別子Ｗであり、このように、上記識別子Ｗに基づいて確立された識別子座標系のＺ軸Ｚ_ｗ（識別子Ｗが位置する平面に垂直である）は、地面に平行である。上記識別子座標系のＺ軸Ｚ_ｗは、上記目標直線を計算するために用いられる。本実施例では、上記識別子座標系は、上記目標直線と互いに平行である。さらに計算の難しさを簡略化し、演算効率を向上させる。

【0044】

ステップ１０３では、上記所定の目標位置姿勢と現在の位置姿勢の相対的な位置姿勢関係に基づいて、オフセット角度を決定するステップは、上記所定の目標位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢、及び上記現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢に基づいて、所定の目標位置姿勢と現在の位置姿勢との間の相対的な位置姿勢関係を取得し、オフセット角度を抽出するステップを含む。

【0045】

より具体的には、以下の方法を採用することができる。

によりＯＭＥ、すなわち、現在の位置姿勢の所定の目標点座標系Ｏ_ｏ－ｘｙｚでの位置姿勢を取得し、姿勢パラメータ、すなわち、オフセット角度Δθを抽出する。

【0046】

ステップ１０４では、図３に示すように、上記目標直線と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定するステップは、上記所定の目標位置姿勢の変位座標に基づいて仮想目標点Ｏ′を作成し、所定の目標位置姿勢に基づいて所定の目標点座標系Ｏ_ｏ－ｘｙｚを確立するステップと、上記仮想目標点に基づいて仮想目標位置姿勢を作成するステップと、上記仮想目標位置姿勢の上記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標、及び現在の位置姿勢の所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標に基づいて、現在の位置姿勢と仮想目標位置姿勢との相対的な位置姿勢関係を取得し、識別子Ｗ及び所定の目標位置姿勢に基づいて目標直線を取得し、目標直線に垂直な方向上の現在の位置姿勢と仮想目標位置姿勢との偏差変位を抽出するステップとを含む。

【0047】

より具体的には、以下の方法を採用することができる。

によりＯ′ＭＥ、すなわち、現在の位置姿勢の仮想目標点座標系Ｏ_ｏ－ｘｙｚでの位置姿勢を取得し、目標直線に垂直な変位パラメータ、すなわち、オフセット変位ΔＸを抽出する。

【0048】

上記仮想目標点、仮想目標位置姿勢は、計算過程において計算を容易にするために導入された中間パラメータである。計算を容易にするために、本実施例では、上記仮想目標点は、上記所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点と重なり、上記仮想目標位置姿勢と上記所定の目標位置姿勢との間に回転行列で変換する必要がある。理解できるように、仮想目標位置姿勢は、２次元コード座標系のＺ軸Ｚ_ｗに基づいて決定された目標直線上の目標位置姿勢の１つの点であり、所定の目標位置姿勢は、実際の教示結果に基づいて得られた目標位置姿勢の１つの点である。理解できるように、上記仮想目標位置姿勢は、所定の目標位置姿勢から変位パラメータを抽出し、姿勢パラメータを無視し、計算過程を簡略化する。上記仮想目標点と上記偏差変位は、可動ステージが上記目標直線までに移動する最短直線距離である。

【0049】

仮想目標位置姿勢の上記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度に基づく座標は、上記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立し、上記仮想目標点座標系Ｏ_ｏ′－ｘｙｚと所定の目標点座標系Ｏ_ｏ－ｘｙｚの座標軸原点の変位及び座標軸の角度偏差に基づいて、上記仮想目標位置姿勢と所定の目標位置姿勢との相対的な位置姿勢関係を計算するステップを含むが、これらに限定されない。

【0050】

仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立するステップは、上記識別子座標系Ｏ_ｗ－ｘｙｚでの上記識別子Ｗと所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点とのベクトル関係

、識別子座標系Ｏ_ｗ－ｘｙｚでの上記識別子Ｗと識別子座標系のＺ軸Ｚ_ｗ方向に所定の目標点から単位ベクトル離れることにより取得された点Ｐとのベクトル関係

、及び上記所定の目標点座標系Ｏ_ｏ－ｘｙｚと上記識別子座標系Ｏ_ｗ－ｘｙｚとの変換関係に基づいて、所定の目標点座標系Ｏ_ｏ－ｘｙｚでの仮想目標位置姿勢の目標直線方向上の単位ベクトル

を取得し、仮想目標点座標系のＹ座標軸とするステップと、地面に垂直な方向に基づいて単位ベクトルを取得して、仮想目標点座標系のＺ座標軸Ｚ_ｗとするステップと、空間直交座標系原則に基づいて、前記目標直線に垂直な仮想目標点座標系のＸ座標軸を取得するステップとを含む。

【0051】

より具体的には、以下の方法を採用することができる。

により、所定の目標点座標系Ｏ_ｏ－ｘｙｚでの仮想目標位置姿勢の目標直線方向上の単位ベクトルを取得し、

として表記され、上記点Ｐは、識別子座標系のＺ軸Ｚ_ｗ方向に所定の目標点から単位ベクトルから離れることにより取得された点である。

【0052】

図３に示すように、上記識別子座標系のＺ軸Ｚ_ｗ方向は、目標直線方向に平行である。

【0053】

非ホロノミック拘束でのサーボ制御用プロセッサは、プログラムコードを呼び出すために用いられ、上記プログラムコードが実行される場合、上記プロセッサは、所定の目標位置姿勢を決定する操作と、少なくとも１つの識別子Ｗを取得し、現在の位置姿勢を取得する操作と、所定の目標位置姿勢と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット角度を決定する操作と、識別子Ｗ及び所定の目標位置姿勢に基づいて目標直線を取得し、上記目標直線と現在の位置姿勢との相対的な位置姿勢関係に基づいて、オフセット変位を決定する操作と、上記オフセット変位及びオフセット角度に基づいて、サーボ制御を行う操作と、を実行する。

【0054】

一実施形態として、上記プロセッサは、さらに上記識別子に基づいて識別子座標系Ｏ_ｗ－ｘｙｚを確立し、現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの変位と角度の座標を取得する。

【0055】

一実施形態として、上記プロセッサは、所定の規則的な間隔で少なくとも同一の識別子を取得し、各所定の規則的な間隔内に上記識別子に基づいて現在時刻の現在の位置姿勢を取得し、制御指令を取得し、各所定の規則的な間隔内に取得された制御指令を利用して現在の位置姿勢に対してサーボ制御を行う。

【0056】

一実施形態として、上記プロセッサは、さらに上記所定の目標位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢、及び上記現在の位置姿勢の識別子座標系Ｏ_ｗ－ｘｙｚでの姿勢に基づいて、所定の目標位置姿勢と現在の位置姿勢との間の相対的な位置姿勢関係を取得し、オフセット角度を抽出する。

【0057】

一実施形態として、上記プロセッサは、さらに上記所定の目標位置姿勢の変位座標に基づいて仮想目標点Ｏ′を作成し、所定の目標位置姿勢に基づいて所定の目標点座標系Ｏ_ｏ－ｘｙｚを確立し、上記仮想目標点に基づいて仮想目標位置姿勢を作成し、上記仮想目標位置姿勢の上記所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標、及び現在の位置姿勢の所定の目標点座標系Ｏ_ｏ－ｘｙｚでの変位と角度の座標に基づいて、現在の位置姿勢と仮想目標位置姿勢との相対的な位置姿勢関係を取得し、識別子及び所定の目標位置姿勢に基づいて目標直線を取得し、目標直線に垂直な方向上の現在の位置姿勢と仮想目標位置姿勢との偏差変位を抽出する。

【0058】

一実施形態として、上記プロセッサは、さらに上記仮想目標位置姿勢に基づいて仮想目標点座標系Ｏ_ｏ′－ｘｙｚを確立し、上記仮想目標点座標系Ｏ_ｏ′－ｘｙｚと所定の目標点座標系Ｏ_ｏ－ｘｙｚの座標軸原点の変位及び座標軸の角度偏差に基づいて、上記仮想目標位置姿勢と所定の目標位置姿勢との相対的な位置姿勢関係を計算する。

【0059】

一実施形態として、上記プロセッサは、さらに上記識別子座標系Ｏ_ｗ－ｘｙｚでの上記識別子と所定の目標点座標系Ｏ_ｏ－ｘｙｚの原点とのベクトル関係

、識別子座標系Ｏ_ｗ－ｘｙｚでの上記識別子と識別子座標系のＺ軸方向に所定の目標点から単位ベクトルから離れることにより取得された点Ｐとのベクトル関係、及び上記所定の目標点座標系Ｏ_ｏ－ｘｙｚと上記識別子座標系Ｏ_ｗ－ｘｙｚとの変換関係に基づいて、所定の目標点座標系Ｏ_ｏ－ｘｙｚでの仮想目標位置姿勢の目標直線方向上の単位ベクトル

を取得し、仮想目標点座標系のＹ座標軸とし、地面に垂直な方向に基づいて単位ベクトルを取得し、仮想目標点座標系のＺ座標軸とし、空間直交座標系原則に基づいて、上記目標直線に垂直な仮想目標点座標系のＸ座標軸を取得する。

【0060】

記憶媒体であり、上記記憶媒体には、コンピュータ指令が記憶されており、上記コンピュータ指令が実行される場合、上記いずれかのサーボ制御方法を実現する。

【0061】

可動ステージは、識別子を取得する収集器を含み、さらに、上記いずれかのプロセッサ、又は、上記記憶媒体を含む。

【0062】

本明細書の説明において、「一実施形態」、「いくつかの実施例」、「例」、「具体例」、又は「いくつかの例」等の用語を参照した説明は、該実施例又は例に関連して説明された具体的な特徴、構造、材料又は特性が本開示の少なくとも１つの実施形態又は例に含まれることを意味する。本明細書において、上記用語に対する概略的な表現は、必ずしも同じ実施例又は例を指してはいない。さらに、具体的な特徴、構造、材料、又は特性を、１つ以上の実施例又は例において任意の適切なやり方で組み合わせることができる。

【0063】

以上に本開示の実施例を示して説明したが、特許請求の範囲を限定するものと理解してはいけない。本開示は、以上の実施例に限定されるものではなく、その具体的な構造の変化が許容され、本開示の独立請求項の保護範囲内で行われた様々な変化は、いずれも本開示の保護範囲内にある。

【符号の説明】

【0064】

１、本体、２、移動シャーシ、３、突合せ装置、４、収集器。

【図1】

【図2】

【図3】

【図4】