JP6801802B1 - Controller system and its control method - Google Patents

Controller system and its control method Download PDF

Info

Publication number
JP6801802B1
JP6801802B1 JP2020042303A JP2020042303A JP6801802B1 JP 6801802 B1 JP6801802 B1 JP 6801802B1 JP 2020042303 A JP2020042303 A JP 2020042303A JP 2020042303 A JP2020042303 A JP 2020042303A JP 6801802 B1 JP6801802 B1 JP 6801802B1
Authority
JP
Japan
Prior art keywords
target
control
fluctuation amount
distance
command
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2020042303A
Other languages
Japanese (ja)
Other versions
JP2021144438A (en
Inventor
宏次 森野
宏次 森野
晃一 坂田
晃一 坂田
優大 小林
優大 小林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Omron Corp
Original Assignee
Omron Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Omron Corp filed Critical Omron Corp
Priority to JP2020042303A priority Critical patent/JP6801802B1/en
Application granted granted Critical
Publication of JP6801802B1 publication Critical patent/JP6801802B1/en
Priority to CN202110142907.1A priority patent/CN113467360A/en
Publication of JP2021144438A publication Critical patent/JP2021144438A/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B19/00—Programme-control systems
    • G05B19/02—Programme-control systems electric
    • G05B19/04—Programme control other than numerical control, i.e. in sequence controllers or logic controllers
    • G05B19/05—Programmable logic controllers, e.g. simulating logic interconnections of signals according to ladder diagrams or function charts
    • G05B19/054—Input/output
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/10—Plc systems
    • G05B2219/15—Plc structure of the system
    • G05B2219/15106—High speed limited function sub plc together with slow speed general

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position Or Direction (AREA)
  • Manipulator (AREA)

Abstract

【課題】制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能なコントローラシステムを提供する。【解決手段】コントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部とを備える。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部とを含む。【選択図】図１PROBLEM TO BE SOLVED: To provide a controller system capable of appropriately controlling an operating speed when a controlled object continuously passes through a plurality of passing points. SOLUTION: A controller system has a setting unit for setting the positions of a plurality of passing points in which a passing order is preset, and a command locus between the passing points in which a controlled object continuously passes through the plurality of passing points. It includes a locus setting unit that sets a division polynomial that follows one variable, and a command value calculation unit that calculates a target command value of a control target for each control cycle based on the division polynomial corresponding to the target command trajectory. The command value calculation unit adjusts the distance calculation unit that calculates the target movement distance with respect to the command control speed for each control cycle and the fluctuation amount of the first variable for each control cycle so that the calculated target movement distance becomes constant. Includes variable adjustment section. [Selection diagram] Fig. 1

Description

本開示は、制御対象を制御するコントローラシステムおよびその制御方法に関する。 The present disclosure relates to a controller system for controlling a controlled object and a control method thereof.

各種設備および各設備に配置される各種装置の制御には、ＰＬＣ（プログラマブルロジックコントローラ）などの制御装置が用いられる。 A control device such as a PLC (programmable logic controller) is used to control various facilities and various devices arranged in each facility.

例えば、特開２０００−１３７５０６号公報（特許文献１）には、ロボットの動作を制御する制御装置が指令軌跡に従う指令値を各サーボに出力してロボットの動作を制御する手法が開示されている。 For example, Japanese Patent Application Laid-Open No. 2000-137506 (Patent Document 1) discloses a method in which a control device for controlling the operation of a robot outputs a command value according to a command locus to each servo to control the operation of the robot. ..

特開２０００−１３７５０６号公報Japanese Unexamined Patent Publication No. 2000-137506

複数の通過点を検査するロボットの動作を制御する場合、ロボットの動作を複数の通過点で停止させずに連続的に動作させることは検査効率を向上させる点で重要である。 When controlling the operation of a robot that inspects a plurality of passing points, it is important to continuously operate the robot without stopping at the plurality of passing points in order to improve the inspection efficiency.

一方で、ロボットの動作としては連続的に動作させるだけでなく、動作速度も適切に制御することが重要である。 On the other hand, it is important not only to continuously operate the robot but also to appropriately control the operation speed.

本開示は、上記の課題を解決するためになされたものであって、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能なコントローラシステムおよびその制御方法に関する。 The present disclosure has been made to solve the above-mentioned problems, and is a controller system capable of appropriately controlling the operating speed when a controlled object continuously passes through a plurality of passing points, and its control. Regarding the method.

ある局面に従うコントローラシステムは、通過順序が予め設定された複数の通過点の位置を設定する設定部と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部とを備える。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部とを含む。 A controller system that follows a certain aspect first sets a setting unit that sets the positions of a plurality of passing points in which a passing order is set in advance, and a command locus between each passing point in which a controlled object continuously passes through the plurality of passing points. It includes a locus setting unit that sets a division polynomial according to a variable, and a command value calculation unit that calculates a target command value of a control target for each control cycle based on the division polynomial corresponding to the target command trajectory. The command value calculation unit adjusts the distance calculation unit that calculates the target movement distance with respect to the command control speed for each control cycle and the fluctuation amount of the first variable for each control cycle so that the calculated target movement distance becomes constant. Includes variable adjustment section.

この構成によれば、指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出し、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する。これにより制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。 According to this configuration, the command value calculation unit calculates the target movement distance with respect to the command control speed for each control cycle, and sets the fluctuation amount of the first variable for each control cycle so that the calculated target movement distance becomes constant. adjust. As a result, it is possible to appropriately control the operating speed by keeping the target moving distance for each control cycle constant.

好ましくは、第１変数は、０〜１の間で変化する。 Preferably, the first variable varies between 0 and 1.

この構成によれば、通過点間の距離に応じて軌跡移動速度が変動することを回避して、制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。 According to this configuration, it is possible to prevent the locus movement speed from fluctuating according to the distance between the passing points, and to appropriately control the operation speed by keeping the target movement distance for each control cycle constant.

好ましくは、区分多項式は、３次のエルミート曲線である。 Preferably, the partitioning polynomial is a Hermitian curve of degree 3.

この構成によれば、３次のエルミート曲線を用いることにより滑らかな曲線軌跡を設定することが可能である。 According to this configuration, it is possible to set a smooth curve locus by using a cubic Hermitian curve.

好ましくは、変数調整部は、算出された目標移動距離に基づいて第１変数の基準となる基準変動量を設定し、基準変動量に基づいて区分多項式に基づく制御対象の座標を少なくとも１回算出し、前回の制御周期における制御対象の座標と、基準変動量に基づく算出された制御対象の座標との間の距離を算出し、算出された基準変動量に対する距離と目標移動距離との比較に基づいて、制御周期毎の第１変数の変動量を調整する。 Preferably, the variable adjustment unit sets a reference fluctuation amount as a reference for the first variable based on the calculated target movement distance, and calculates the coordinates of the controlled object based on the division polynomial at least once based on the reference fluctuation amount. Then, the distance between the coordinates of the control target in the previous control cycle and the coordinates of the control target calculated based on the reference fluctuation amount is calculated, and the distance to the calculated reference fluctuation amount is compared with the target movement distance. Based on this, the amount of fluctuation of the first variable for each control cycle is adjusted.

この構成によれば、基準変動量を用いて試算点との距離を算出することにより、制御周期毎の第１変数の変動量を調整するため効率的に第１変数の変動量を調整することが可能である。 According to this configuration, the fluctuation amount of the first variable is efficiently adjusted in order to adjust the fluctuation amount of the first variable for each control cycle by calculating the distance from the trial calculation point using the reference fluctuation amount. Is possible.

好ましくは、変数調整部は、基準変動量毎の区分多項式に基づく制御対象の座標を複数回算出し、基準変動量毎の制御対象の座標間のそれぞれの距離を算出し、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とに基づいて第１変数の基準となる基準変動量を補正する。 Preferably, the variable adjustment unit calculates the coordinates of the controlled object based on the division polynomial for each reference variable amount a plurality of times, calculates the respective distances between the coordinates of the controlled object for each reference variable amount, and performs each reference variable amount. The reference fluctuation amount, which is the reference of the first variable, is corrected based on the total distance between the coordinates of the controlled object and the target movement distance.

この構成によれば、基準変動量を補正して、補正した基準変動量を用いて試算点との距離を算出することにより、精度の高い制御周期毎の第１変数の変動量に調整することが可能である。 According to this configuration, the reference fluctuation amount is corrected, and the distance from the trial calculation point is calculated using the corrected reference fluctuation amount to adjust the fluctuation amount of the first variable for each highly accurate control cycle. Is possible.

好ましくは、変数調整部は、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とを比較し、基準変動量毎の制御対象の座標間の距離の合計が目標移動距離以上の場合に目標移動距離に対する第１変数の変動量を算出する。 Preferably, the variable adjustment unit compares the total distance between the control target coordinates for each reference fluctuation amount with the target movement distance, and the total distance between the control target coordinates for each reference fluctuation amount is the target movement distance. In the above cases, the amount of fluctuation of the first variable with respect to the target movement distance is calculated.

この構成によれば、基準変動量を用いて試算点との距離の合計を算出して、目標移動距離との比較により、制御周期毎の第１変数の変動量を調整するため効率的に第１変数の変動量を調整することが可能である。 According to this configuration, the total distance from the trial calculation point is calculated using the reference fluctuation amount, and the fluctuation amount of the first variable for each control cycle is adjusted efficiently by comparing with the target movement distance. It is possible to adjust the amount of fluctuation of one variable.

好ましくは、変数調整部は、対象とする指令軌跡の通過点間の距離と、目標移動距離と、分割係数とに基づいて基準変動量を設定する。 Preferably, the variable adjustment unit sets the reference fluctuation amount based on the distance between the passing points of the target command locus, the target movement distance, and the division coefficient.

この構成によれば、分割係数により基準変動量を調整することが可能であるため精度の高い制御周期毎の第１変数の変動量に調整することが可能である。 According to this configuration, since the reference fluctuation amount can be adjusted by the division coefficient, it is possible to adjust the fluctuation amount of the first variable for each control cycle with high accuracy.

ある局面に従うコントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を第１変数に従う区分多項式で設定するステップと、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出するステップとを備える。目標指令値を算出するステップは、制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む。 The control method of the controller system according to a certain aspect is a step of setting the positions of a plurality of passing points in which a passing order is preset, and a command locus between the passing points in which the controlled object continuously passes through the plurality of passing points. It includes a step of setting with a division polynomial according to the first variable and a step of calculating a target command value of a control target for each control cycle based on the division polynomial corresponding to the target command locus. The step of calculating the target command value is the step of calculating the target movement distance with respect to the command control speed for each control cycle, and adjusting the fluctuation amount of the first variable for each control cycle so that the calculated target movement distance becomes constant. Including steps to do.

この構成によれば、指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出し、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する。これにより制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。 According to this configuration, the command value calculation unit calculates the target movement distance with respect to the command control speed for each control cycle, and sets the fluctuation amount of the first variable for each control cycle so that the calculated target movement distance becomes constant. adjust. As a result, it is possible to appropriately control the operating speed by keeping the target moving distance for each control cycle constant.

なお、本開示において、「部」及び「装置」とは、単に物理的手段を意味するものではなく、その「部」及び「装置」が有する機能をソフトウェアによって実現する構成も含む。また、１つの「部」及び「装置」が有する機能が２つ以上の物理的手段や装置によって実現されてもよく、或いは、２つ以上の「部」及び「装置」の機能が１つの物理的手段や装置によって実現されてもよい。さらに、「部」及び「装置」とは、例えば「手段」及び「システム」と言い換えることも可能な概念である。 In the present disclosure, the "part" and the "device" do not simply mean a physical means, but also include a configuration in which the functions of the "part" and the "device" are realized by software. Further, the functions of one "part" and "device" may be realized by two or more physical means or devices, or the functions of two or more "parts" and "devices" may be realized by one physical. It may be realized by physical means or equipment. Further, "part" and "device" are concepts that can be paraphrased as, for example, "means" and "system".

本開示のコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能である。 According to the controller system of the present disclosure and the control method thereof, it is possible to appropriately control the operating speed when the controlled object continuously passes through a plurality of passing points.

実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。It is a schematic diagram which shows the application example of the review control system 1 according to embodiment. 実施形態に従う駆動装置２の具体的構成を説明する図である。It is a figure explaining the specific structure of the drive device 2 according to an embodiment. 実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。It is a top view schematically showing an example of the hardware structure of the control device 100 according to an embodiment. 実施形態に従う制御装置１００の機能構成を説明する図である。It is a figure explaining the functional structure of the control device 100 according to an embodiment. 実施形態に従う速度ベクトルの設定について説明する図である。It is a figure explaining the setting of the velocity vector according to the embodiment. 実施形態に従う軌跡設定部３５とサーボ指令制御部４０との関係について説明する図である。It is a figure explaining the relationship between the locus setting unit 35 and the servo command control unit 40 according to embodiment. 実施形態に従うサーボ指令制御部４０が出力する位置座標を説明する図である。It is a figure explaining the position coordinates output by the servo command control unit 40 according to the embodiment. 比較例として指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。As a comparative example, it is a figure explaining the locus movement speed and the parameter t of the cubic Hermitian curve which is a command locus. 実施形態に従う指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。It is a figure explaining the locus movement speed and the parameter t of the 3rd order Hermitian curve which is a command locus according to an embodiment. 実施形態に従う１回目の試算処理を説明する図である。It is a figure explaining the 1st trial calculation process according to embodiment. 実施形態に従うパラメータｔの基準変動量の補正について説明する図である。It is a figure explaining the correction of the reference variation amount of a parameter t according to an embodiment. 実施形態に従う２回目の試算処理を説明する図である。It is a figure explaining the 2nd trial calculation process according to embodiment. 実施形態に従う３回目の試算処理を説明する図である。It is a figure explaining the 3rd trial calculation process according to embodiment. 実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。It is a figure explaining the simulation example at the time of setting 14 passing points according to an embodiment. 実施形態に従う通過座標および３次多項式の係数について説明する図である。It is a figure explaining the passing coordinates and the coefficient of a cubic polynomial according to an embodiment. 実施形態に従うトレース軌跡を説明する図である。It is a figure explaining the trace locus according to embodiment. 実施形態に従う指令速度の波形を説明する図である。It is a figure explaining the waveform of the command speed according to an embodiment.

本発明の実施の形態について、図面を参照しながら詳細に説明する。なお、図中の同一または相当部分については、同一符号を付してその説明は繰り返さない。 Embodiments of the present invention will be described in detail with reference to the drawings. The same or corresponding parts in the drawings are designated by the same reference numerals and the description thereof will not be repeated.

以下では図１のような２軸（Ｘ軸、Ｙ軸）構成の同期グループ（「複数のサーボモータ」及びそれらに対応する「複数のサーボドライバ」を含む複数のサーボ制御デバイスに相当する。）を前提に説明するが、本実施形態の同期グループは、２軸以上のサーボ制御デバイスと１以上のトリガ制御デバイスを含む構成であればよい。以下では対象物が図１のような２軸（Ｘ軸、Ｙ軸）で移動するが、対象物の移動形態は、３軸以上であってもよい。 In the following, a synchronization group having a two-axis (X-axis, Y-axis) configuration as shown in FIG. 1 (corresponding to a plurality of servo control devices including "a plurality of servo motors" and "a plurality of servo drivers" corresponding thereto). However, the synchronization group of the present embodiment may have a configuration including two or more servo control devices and one or more trigger control devices. In the following, the object moves on two axes (X-axis and Y-axis) as shown in FIG. 1, but the moving form of the object may be three or more axes.

＜Ａ．適用例＞
まず、本発明が適用される場面の一例について説明する。 <A. Application example>
First, an example of a situation in which the present invention is applied will be described.

図１は、実施形態に従うレビュー制御システム１のアプリケーション例を示す模式図である。 FIG. 1 is a schematic diagram showing an application example of the review control system 1 according to the embodiment.

図１には、フラットパネルディスプレイＦＰＤを検査するレビュー制御システム１が示されている。 FIG. 1 shows a review control system 1 that inspects a flat panel display FPD.

レビュー制御システム１には、駆動装置２と、撮影装置２０とが設けられている。 The review control system 1 is provided with a drive device 2 and a photographing device 20.

駆動装置２は、各軸方向の任意の位置に移動可能に設けられている。なお、本例においては、Ｘ、Ｙ軸に移動する場合について説明する。 The drive device 2 is provided so as to be movable at an arbitrary position in each axial direction. In this example, the case of moving to the X and Y axes will be described.

駆動装置２の先端には、撮影装置２０が設けられており、駆動装置２を駆動することにより予め検出したフラットパネルディスプレイＦＰＤの欠陥部分を撮影装置２０で再検査する。具体的には、当該位置で撮影装置２０で撮像する。 An imaging device 20 is provided at the tip of the driving device 2, and the imaging device 20 re-inspects a defective portion of the flat panel display FPD detected in advance by driving the driving device 2. Specifically, the imaging device 20 takes an image at the position.

図２は、実施形態に従う駆動装置２の具体的構成を説明する図である。 FIG. 2 is a diagram illustrating a specific configuration of the drive device 2 according to the embodiment.

図２に示されるように、駆動装置２は、３軸を構成する複数のサーボモータと、それぞれのサーボモータに対応するサーボドライバを含むサーボ制御デバイスとにより、撮像検査の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）の位置に移動するように駆動し、当該位置の対象物（フラットパネルディスプレイＦＰＤの欠陥部分）をトリガ制御デバイスとしての撮影装置２０で撮影する。 As shown in FIG. 2, the drive device 2 is an object of imaging inspection (flat panel display) by a plurality of servomotors constituting three axes and a servo control device including a servo driver corresponding to each servomotor. It is driven to move to the position of the defective portion of the FPD), and the object (the defective portion of the flat panel display FPD) at that position is photographed by the photographing device 20 as a trigger control device.

再び図１を参照して、本例においては、フラットパネルディスプレイＦＰＤに４つの欠陥部分（通過点）が予め示されている。そして、欠陥部分（通過点）の通過順序が予め設定されているものとする。 With reference to FIG. 1 again, in this example, four defective portions (passing points) are previously shown on the flat panel display FPD. Then, it is assumed that the passing order of the defective portion (passing point) is set in advance.

レビュー制御システム１は、複数の欠陥部分（通過点）を通過する滑らかな指令曲線を設定する。撮影装置２０は、欠陥部分（通過点）を通過する際に当該位置を撮像する。 The review control system 1 sets a smooth command curve that passes through a plurality of defective portions (passing points). The photographing device 20 images the position when passing through the defective portion (passing point).

レビュー制御システム１は、設定された指令曲線に基づいて駆動装置２を制御することにより、止まらずに欠陥部分（通過点）を撮像することが可能となる。 By controlling the drive device 2 based on the set command curve, the review control system 1 can image the defective portion (passing point) without stopping.

これにより、フラットパネルディスプレイの検査速度の向上を図ることが可能である。 As a result, it is possible to improve the inspection speed of the flat panel display.

なお、図１に示すレビュー制御システム１は任意のアプリケーションに応用可能である。 The review control system 1 shown in FIG. 1 can be applied to any application.

本実施の形態に従うレビュー制御システム１は、複数のデバイスが組み合わされたアプリケーションに対しても、１つの制御装置で制御することができる。 The review control system 1 according to the present embodiment can control an application in which a plurality of devices are combined with one control device.

なお、フラットパネルディスプレイに限られず、各種の半導体にも同様に適用可能である。 It is not limited to flat panel displays, but can be similarly applied to various semiconductors.

＜Ｂ．構成例＞
［ハードウェア構成］
次に、本実施形態に係る制御装置１００のハードウェア構成の一例について説明する。 <B. Configuration example>
[Hardware configuration]
Next, an example of the hardware configuration of the control device 100 according to the present embodiment will be described.

図３は、実施形態に従う制御装置１００のハードウェアの構成の一例を模式的に示す平面図である。 FIG. 3 is a plan view schematically showing an example of the hardware configuration of the control device 100 according to the embodiment.

図３を参照して、制御装置１００は、図１及び図２に例示した駆動装置２及び撮影装置２０に接続され、制御演算部１０１、通信インタフェース（Ｉ／Ｆ）部１０２、記憶部１０３、入力部１０４、及び出力部１０５を含み、各部はバスライン１０６を介して相互に通信可能に接続され得る。 With reference to FIG. 3, the control device 100 is connected to the drive device 2 and the photographing device 20 illustrated in FIGS. 1 and 2, and the control calculation unit 101, the communication interface (I / F) unit 102, the storage unit 103, Each unit includes an input unit 104 and an output unit 105, and each unit can be communicably connected to each other via a bus line 106.

制御演算部１０１は、ＣＰＵ（Central Processing Unit）、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）等を含み、情報処理に応じて各構成要素の制御及び各種演算を行う。 The control calculation unit 101 includes a CPU (Central Processing Unit), a RAM (Random Access Memory), a ROM (Read Only Memory), and the like, and controls each component and performs various calculations according to information processing.

通信Ｉ／Ｆ部１０２は、例えば、有線又は無線により他の構成要素である「部」及び「装置」と通信するための通信モジュールである。通信Ｉ／Ｆ部１０２が通信に用いる通信方式は任意であり、例えば、ＬＡＮ（Local Area Network）やＵＳＢ（Universal Serial Bus）等が挙げられ、バスライン１０６と同等の適宜の通信線を適用することもできる。 The communication I / F unit 102 is a communication module for communicating with other components "unit" and "device" by wire or wirelessly, for example. The communication method used by the communication I / F unit 102 for communication is arbitrary, and examples thereof include LAN (Local Area Network) and USB (Universal Serial Bus), and an appropriate communication line equivalent to the bus line 106 is applied. You can also do it.

駆動装置２及び撮影装置２０は、通信Ｉ／Ｆ部１０２を介して、制御演算部１０１等と通信可能に設けることが可能である。 The drive device 2 and the photographing device 20 can be provided so as to be able to communicate with the control calculation unit 101 and the like via the communication I / F unit 102.

記憶部１０３は、例えばハード・ディスク・ドライブ（ＨＤＤ）、ソリッド・ステート・ドライブ（ＳＳＤ）等の補助記憶装置であり、制御演算部１０１で実行される各種プログラム（各種処理を実行するための演算プログラム、並びに、駆動装置２及び撮影装置２０の動作の制御処理を行うための制御プログラム等）、校正条件、測定条件、画像処理条件（対象物の認識パラメータ等）を含むデータベース、撮影装置２０から出力される撮影画像（測定データ）、画像処理結果のデータ、対象物の３次元モデルデータ等を記憶する。このとおり、記憶部１０３に記憶された演算プログラム及び制御プログラムが制御演算部１０１で実行されることにより、後述する機能構成例における各種処理機能が実現される。 The storage unit 103 is an auxiliary storage device such as a hard disk drive (HDD) or a solid state drive (SSD), and various programs (calculations for executing various processes) executed by the control calculation unit 101. From the program, the database including the drive device 2 and the control program for controlling the operation of the photographing device 20), the calibration condition, the measurement condition, the image processing condition (recognition parameter of the object, etc.), and the photographing device 20. The output captured image (measurement data), image processing result data, three-dimensional model data of the object, and the like are stored. As described above, by executing the calculation program and the control program stored in the storage unit 103 in the control calculation unit 101, various processing functions in the function configuration example described later are realized.

入力部１０４は、駆動装置２、撮影装置２０、及び制御装置１００を利用するユーザからの各種入力操作を受け付けるためのインタフェースデバイスであり、例えば、マウス、キーボード、タッチパネル、音声マイク等で実現し得る。出力部１０５は、駆動装置２、撮影装置２０、及び制御装置１００を利用するユーザ等へ、各種情報を、その表示、音声出力、印刷出力等により報知するためのインタフェースデバイスであり、例えば、ディスプレイ、スピーカ、プリンタ等で実現し得る。 The input unit 104 is an interface device for receiving various input operations from a user who uses the drive device 2, the photographing device 20, and the control device 100, and can be realized by, for example, a mouse, a keyboard, a touch panel, a voice microphone, or the like. .. The output unit 105 is an interface device for notifying users and the like who use the drive device 2, the photographing device 20, and the control device 100 of various information by display, audio output, print output, and the like. For example, a display. , Speakers, printers, etc.

［機能構成］
図４は、実施形態に従う制御装置１００の機能構成を説明する図である。 [Functional configuration]
FIG. 4 is a diagram illustrating a functional configuration of the control device 100 according to the embodiment.

図４を参照して、制御装置１００の制御演算部１０１は、記憶部１０３に記憶された各種プログラム（制御プログラム及び演算プログラム等）をＲＡＭに展開し、それらの各種プログラムをＣＰＵにより解釈及び実行して、各構成要素を制御する。これにより、図４に例示の如く、本実施形態に係る制御装置１００は、設定部３０と、軌跡設定部３５と、サーボ指令制御部４０と、トリガ指令制御部５０とを備える構成を実現し得る。 With reference to FIG. 4, the control calculation unit 101 of the control device 100 expands various programs (control program, calculation program, etc.) stored in the storage unit 103 into the RAM, and interprets and executes these various programs by the CPU. And control each component. As a result, as illustrated in FIG. 4, the control device 100 according to the present embodiment has a configuration including a setting unit 30, a locus setting unit 35, a servo command control unit 40, and a trigger command control unit 50. obtain.

設定部３０は、通過順序が予め設定された複数の通過点の位置を設定する。 The setting unit 30 sets the positions of a plurality of passing points whose passing order is preset.

軌跡設定部３５は、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を設定する。 The locus setting unit 35 sets a command locus between each passing point at which the controlled object continuously passes through a plurality of passing points.

サーボ指令制御部４０は、設定された指令軌跡に基づいて駆動装置２の動作を制御する指令信号を出力する。 The servo command control unit 40 outputs a command signal that controls the operation of the drive device 2 based on the set command locus.

トリガ指令制御部５０は、複数の通過点の位置に従って撮影装置２０に対して撮影指令信号を出力する。 The trigger command control unit 50 outputs a shooting command signal to the shooting device 20 according to the positions of the plurality of passing points.

なお、本実施形態では、制御装置１００で実現される各機能が汎用のＣＰＵによって実現される例について説明したが、以上の機能の一部又は全部が、１又は複数の専用のプロセッサ又は専用回路（例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やＦＰＧＡ（Field-Programmable Gate Array）等）によって実現されてもよい。さらに、一部の処理をネットワーク接続された外部装置に担当させてもよい。また、制御装置１００の機能構成は、実施形態や構成例に応じて、適宜、機能の省略、置換、及び追加が行われてももちろんよい。また、「制御装置」とは、一般的な情報処理装置（例えば、コンピュータ、ワークステーション等）と解することができる。 In this embodiment, an example in which each function realized by the control device 100 is realized by a general-purpose CPU has been described, but some or all of the above functions are one or more dedicated processors or dedicated circuits. (For example, it may be realized by ASIC (Application Specific Integrated Circuit), FPGA (Field-Programmable Gate Array), etc.). Further, a part of the processing may be assigned to an external device connected to the network. Further, as for the functional configuration of the control device 100, of course, the functions may be omitted, replaced, or added as appropriate according to the embodiment or the configuration example. Further, the "control device" can be understood as a general information processing device (for example, a computer, a workstation, etc.).

［動作例］
本開示の一実施形態に係る制御装置１００の一例で実現される制御方法について説明する。 [Operation example]
A control method realized by an example of the control device 100 according to the embodiment of the present disclosure will be described.

指令軌跡の算出方式について説明する。 The calculation method of the command locus will be described.

一例として、順序が予め設定された複数の通過点を連続的に通過する指令軌跡の算出について説明する。 As an example, the calculation of a command locus that continuously passes through a plurality of passing points having a preset order will be described.

一例として、指定された通過点（P1、P2…Pn）の（P1−P2、P2−P3、・・・Pn-1−Pn）の点間の軌跡をｘ座標、ｙ座標の３次多項式で定義すると下記式（１）で表される。 As an example, the locus between the points (P 1 − P 2 , P 2 − P 3 , ··· P n-1 −P n ) of the specified passing points (P 1 , P 2 … P n ) is x. When defined by a cubic polynomial of coordinates and y-coordinates, it is expressed by the following equation (1).

Figure 0006801802
Figure 0006801802

そして、次の２つの条件を満たす３次の曲線式としてエルミート曲線で定義する。 Then, the Hermitian curve is defined as a cubic curve formula that satisfies the following two conditions.

具体的には、設定された通過点の座標を通ることと、各通過点の境界および曲線途中で軌跡が連続であることである。 Specifically, it passes through the coordinates of the set passing points, and the locus is continuous at the boundary of each passing point and in the middle of the curve.

上記条件に基づくエルミート曲線は、下記式（２）で表される。 The Hermitian curve based on the above conditions is represented by the following equation (2).

Figure 0006801802
Figure 0006801802

エルミート曲線は、始点位置、始点速度ベクトル、終点位置、終点速度ベクトルの４つのパラメータに基づいて設定される。 The Hermitian curve is set based on four parameters of start point position, start point velocity vector, end point position, and end point velocity vector.

したがって、t=0で始点位置、t=1で終点位置となるため、通過点の座標を始点位置、終点位置のパラメータとして設定する。 Therefore, since the start point position is set at t = 0 and the end point position is set at t = 1, the coordinates of the passing point are set as the parameters of the start point position and the end point position.

Figure 0006801802
Figure 0006801802

エルミート曲線を連続的に繋ぎ合わせる手法として、Catmull-Rom Spline（キャットムル・ロム曲線）を用いる。具体的には、本例においては、各通過点での位置および速度の連続性を保つために、速度ベクトルを通過点の前後で一致するように設定する。 Catmull-Rom Spline is used as a method of continuously connecting Hermitian curves. Specifically, in this example, in order to maintain the continuity of the position and the velocity at each passing point, the velocity vectors are set so as to match before and after the passing point.

図５は、実施形態に従う速度ベクトルの設定について説明する図である。 FIG. 5 is a diagram illustrating the setting of the velocity vector according to the embodiment.

図５に示されるように、具体的には、通過点P2において、指令軌跡Q1の曲線式の終点速度ベクトルと、指令軌跡Q2の曲線式の始点速度ベクトルに同じベクトルを設定する。 As shown in FIG. 5, specifically, in the pass point P 2, and the end point velocity vector of the curve type of command trajectory Q1, sets the same vector at the starting point velocity vector of the curve type of command trajectory Q2.

これにより、順序が予め設定された複数の通過点を連続的に通過する指令軌跡を設定することが可能となる。 This makes it possible to set a command locus that continuously passes through a plurality of passing points whose order is preset.

具体的には、各通過点間毎の区間多項式である３次のエルミート曲線が設定される。 Specifically, a cubic Hermitian curve, which is an interval polynomial for each passing point, is set.

図６は、実施形態に従う軌跡設定部３５とサーボ指令制御部４０との関係について説明する図である。 FIG. 6 is a diagram for explaining the relationship between the locus setting unit 35 and the servo command control unit 40 according to the embodiment.

図６に示されるように、軌跡設定部３５は、通過点座標と、通過順序および速度ベクトルを入力することにより各通過点毎の３次のエルミート曲線を設定する。 As shown in FIG. 6, the locus setting unit 35 sets a cubic Hermitian curve for each passing point by inputting the passing point coordinates, the passing order, and the velocity vector.

軌跡設定部３５は、算出結果として３次のエルミート曲線の指令軌跡データをサーボ指令制御部４０に出力する。 The locus setting unit 35 outputs the command locus data of the cubic Hermitian curve to the servo command control unit 40 as a calculation result.

サーボ指令制御部４０は、設定された３次のエルミート曲線の指令軌跡データに基づいて駆動装置２の動作を制御する指令信号を出力する。 The servo command control unit 40 outputs a command signal for controlling the operation of the drive device 2 based on the command locus data of the set cubic Hermitian curve.

具体的には、サーボ指令制御部４０は、駆動装置２の動作を制御する位置座標X,Yを出力する。 Specifically, the servo command control unit 40 outputs the position coordinates X and Y that control the operation of the drive device 2.

図７は、実施形態に従うサーボ指令制御部４０が出力する位置座標を説明する図である。 FIG. 7 is a diagram for explaining the position coordinates output by the servo command control unit 40 according to the embodiment.

図７に示されるように、サーボ指令制御部４０は、軌跡設定部３５で設定された指令軌跡データに従って駆動装置２の動作位置を出力する。 As shown in FIG. 7, the servo command control unit 40 outputs the operation position of the drive device 2 according to the command trajectory data set by the trajectory setting unit 35.

具体的には、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の駆動装置２の目標指令値を算出する。 Specifically, the target command value of the drive device 2 for each control cycle is calculated based on the division polynomial corresponding to the target command locus.

これによりサーボ指令制御部４０は、設定された指令軌跡に沿って駆動装置２を動作させることが可能となる。 As a result, the servo command control unit 40 can operate the drive device 2 along the set command locus.

図８は、比較例として指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。 FIG. 8 is a diagram illustrating a locus movement speed and a parameter t of a cubic Hermitian curve which is a command locus as a comparative example.

図８（Ａ）には、各通過点間における軌跡移動速度が示されている。 FIG. 8A shows the locus movement speed between each passing point.

図８（Ｂ）には、各指令軌跡のパラメータｔの変化が示されている。 FIG. 8B shows a change in the parameter t of each command locus.

当該図に示されるように、パラメータｔが一定の割合で０〜１に変化する場合には、一例として通過点間の距離が短い場合には、基準速度V0よりも軌跡移動速度は遅くなる場合が示されている。一方で、通過点間の距離が長い場合には、軌跡移動速度は、基準速度V0よりも軌跡移動速度は速くなる場合が示されている。 As shown in the figure, when the parameter t changes from 0 to 1 at a constant rate, for example, when the distance between the passing points is short, the locus movement speed becomes slower than the reference speed V 0. The case is shown. On the other hand, when the distance between the passing points is long, the locus moving speed may be faster than the reference speed V 0 .

すなわち、パラメータｔを一定の割合で調整した場合には、駆動装置２の速度を一定に調整することはできない。 That is, when the parameter t is adjusted at a constant rate, the speed of the drive device 2 cannot be adjusted to be constant.

図９は、実施形態に従う指令軌跡である３次のエルミート曲線の軌跡移動速度およびパラメータｔを説明する図である。 FIG. 9 is a diagram illustrating a locus movement speed and a parameter t of a cubic Hermitian curve which is a command locus according to an embodiment.

図９（Ａ）には、各通過点間における軌跡移動速度が示されている。 FIG. 9A shows the locus movement speed between each passing point.

図９（Ｂ）には、各指令軌跡のパラメータｔの変化が示されている。 FIG. 9B shows changes in the parameter t of each command locus.

当該図に示されるように、各通過点間における軌跡移動速度を基準速度V0で一定に制御するためには、各通過点間のパラメータｔの変動率を調整する必要がある。 As shown in the figure, in order to control the locus movement speed between the passing points to be constant at the reference speed V 0 , it is necessary to adjust the volatility of the parameter t between the passing points.

実施形態においては、各通過点間における軌跡移動速度を調整する。 In the embodiment, the locus movement speed between each passing point is adjusted.

再び図７を参照してサーボ指令制御部４０は、距離算出部４２と、変数調整部４４とを含む。 With reference to FIG. 7 again, the servo command control unit 40 includes a distance calculation unit 42 and a variable adjustment unit 44.

距離算出部４２は、制御周期毎の指令制御速度に対する目標移動距離を算出する。 The distance calculation unit 42 calculates the target movement distance with respect to the command control speed for each control cycle.

変数調整部４４は、距離算出部４２で算出された目標移動距離が一定となるように制御周期毎のパラメータｔの増分値を調整する。 The variable adjustment unit 44 adjusts the increment value of the parameter t for each control cycle so that the target movement distance calculated by the distance calculation unit 42 becomes constant.

＜Ｃ．設定方式＞
実施形態に従うパラメータｔの設定方式は、パラメータｔに所定の基準変動量を与える試算処理を実行する。 <C. Setting method>
In the method of setting the parameter t according to the embodiment, a trial calculation process of giving a predetermined reference fluctuation amount to the parameter t is executed.

本例においては、一例として２回あるいは３回試算処理した場合に、制御周期毎の目標移動距離の近傍に試算点が到達するような基準変動量を設定する。 In this example, as an example, when the trial calculation process is performed twice or three times, the reference fluctuation amount is set so that the trial calculation point reaches the vicinity of the target movement distance for each control cycle.

図１０は、実施形態に従う１回目の試算処理を説明する図である。 FIG. 10 is a diagram illustrating a first trial calculation process according to the embodiment.

図１０を参照して、本例においては、通過点P0(X0,Y0)と、通過点P1(X1,Y1)との間の指令軌跡Q0の曲線式に対してパラメータｔの変動量を設定する場合の処理について説明する。 With reference to FIG. 10, in this example, the parameters for the curve equation of the command locus Q0 between the passing point P 0 (X 0 , Y 0 ) and the passing point P 1 (X 1 , Y 1 ). The process for setting the fluctuation amount of t will be described.

一例として、現在位置を通過点P0(X0,Y0)とした場合に、次の制御周期で移動する目標位置Pm(Xm,Ym)のパラメータｔの変動量Tmを算出する。 As an example, when the current position is the passing point P 0 (X 0 , Y 0 ), the fluctuation amount T m of the parameter t of the target position P m (X m , Y m ) that moves in the next control cycle is calculated. To do.

［準備処理］
通過点P0(X0,Y0)までの積算移動距離をLsとする。 [Preparation process]
Let L s be the total distance traveled to the passing point P 0 (X 0 , Y 0 ).

制御周期毎の次の目標位置Pm(Xm,Ym)までの積算移動距離はLmとする。 The integrated movement distance to the next target position P m (X m , Y m ) for each control cycle is L m .

目標移動距離ΔLmは、Lm-Lsである。 The target travel distance ΔL m is L m -L s .

本例においては、パラメータｔを２回変化させて試算した場合に積算移動距離がLm近傍になる試算用の基準変動量ΔTをまず設定する。 In this example, the reference fluctuation amount ΔT for the trial calculation in which the integrated movement distance is close to L m when the parameter t is changed twice is first set.

通過点P0(X0,Y0)と、通過点P1(X1,Y1)との間の直線距離ΔLを算出する。 Calculate the linear distance ΔL between the passing point P 0 (X 0 , Y 0 ) and the passing point P 1 (X 1 , Y 1 ).

パラメータｔが０〜１まで変化することにより直線距離ΔL移動すると仮定すると、制御周期毎のパラメータｔの平均変化量は、ΔL/ΔLmである。 Assuming that the parameter t moves by a linear distance ΔL by changing from 0 to 1, the average amount of change of the parameter t for each control cycle is ΔL / ΔL m .

したがって、パラメータｔをｋ回変化させて試算した場合に積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLm/kにより設定する。 Therefore, the reference fluctuation amount ΔT for the trial calculation in which the integrated movement distance is close to L m when the parameter t is changed k times is set by ΔL / ΔL m / k.

本例においては、分割係数k=2に設定する。 In this example, the division coefficient k = 2 is set.

パラメータｔを１回変化させて試算した場合に積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLmを設定する。 ΔL / ΔL m is set as the reference fluctuation amount ΔT for the trial calculation in which the integrated movement distance is close to L m when the parameter t is changed once.

パラメータｔを２回変化させて積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLm/2により設定する。 The reference fluctuation amount ΔT for trial calculation in which the integrated movement distance becomes near L m by changing the parameter t twice is set by ΔL / ΔL m / 2.

パラメータｔを３回変化させて積算移動距離がLm近傍になる試算用の基準変動量ΔTは、ΔL/ΔLm/3により設定する。 The reference fluctuation amount ΔT for trial calculation in which the integrated movement distance becomes near L m by changing the parameter t three times is set by ΔL / ΔL m / 3.

なお、変化させる試算回数を増加させることにより精度の高いパラメータｔの変動量を算出することが可能であるが、演算量が増加する。 It is possible to calculate the fluctuation amount of the parameter t with high accuracy by increasing the number of trial calculations to be changed, but the calculation amount increases.

基準変動量を用いて試算点との距離を算出することにより、パラメータｔの変動量を算出するため効率的にパラメータｔの変動量を調整することが可能である。 By calculating the distance from the trial calculation point using the reference fluctuation amount, it is possible to efficiently adjust the fluctuation amount of the parameter t in order to calculate the fluctuation amount of the parameter t.

［１回目の試算処理］
１回目の試算処理においては第１試算点SPiを算出する。 [First trial calculation process]
In the first trial calculation process, the first trial calculation point SP i is calculated.

iは、試算回数を示す。本例においては、i=1である。 i indicates the number of trial calculations. In this example, i = 1.

具体的には、第１試算点SPiとして、パラメータｔの値をTiに設定する。 Specifically, the value of the parameter t is set to T i as the first trial calculation point SP i .

一例として、通過点P0(X0,Y0)のパラメータｔの値Tsは、０とする。 As an example, the value T s of the parameter t of the passing point P 0 (X 0 , Y 0 ) is set to 0.

Tiは、Ts+ΔTである。 T i is T s + ΔT.

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTiを入力することにより、通過点P0(X0,Y0)から基準変動量ΔT分、移動した第１試算点SPiの位置(Xi,Yi)が算出される。 By inputting T i to the parameter t of the formula shown by the Hermitian curve of the command locus Q, the position of the first estimated point SP i moved from the passing point P 0 (X 0 , Y 0 ) by the reference fluctuation amount ΔT. (X i , Y i ) is calculated.

通過点P0(X0,Y0)から基準変動量ΔT分、移動した場合の第１試算点SPiの位置(Xi,Yi)の距離は、次式により算出される。 The distance of the position (X i , Y i ) of the first trial calculation point SP i when moving by the reference fluctuation amount ΔT from the passing point P 0 (X 0 , Y 0 ) is calculated by the following equation.

指令軌跡の積算移動距離は、曲線式に制御周期毎にパラメータｔを与えて、線積分を各微小区間の直線補間に近似して積算する。 The integrated movement distance of the command locus is integrated by giving a parameter t to each control cycle in a curved line and approximating the line integral to linear interpolation of each minute section.

Figure 0006801802
Figure 0006801802

ここで、jは積算回数である。 Here, j is the total number of times.

積算移動距離Liは、Ls+ΔLiとして算出される。 The integrated travel distance L i is calculated as L s + ΔL i .

次に、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、１回目の試算点SPiまでの試算距離Liとを比較する。 Next, the integrated movement distance L m , which is the target from the passing point P 0 (X 0 , Y 0 ), is compared with the estimated distance L i to the first estimated point SP i .

比較結果に基づいて、Lm＞Liである場合には、試算処理を継続する。すなわち、２回目の試算処理を実行する。 Based on the comparison result, if L m > L i , the trial calculation process is continued. That is, the second trial calculation process is executed.

［基準変動量の補正］
２回目の試算処理を実行する前にパラメータｔの基準変動量ΔTを補正する。 [Correction of reference fluctuation amount]
The reference fluctuation amount ΔT of the parameter t is corrected before executing the second trial calculation process.

図１１は、実施形態に従うパラメータｔの基準変動量の補正について説明する図である。 FIG. 11 is a diagram illustrating correction of a reference variation amount of the parameter t according to the embodiment.

図１１（Ａ）を参照して、パラメータｔと、積算移動距離Lとの関係が示されている。 With reference to FIG. 11A, the relationship between the parameter t and the integrated movement distance L is shown.

パラメータｔは、０〜１へと変化する。 The parameter t changes from 0 to 1.

パラメータｔが０の位置の場合の通過点P0(X0,Y0)の積算移動距離は、Lsである。 The integrated movement distance of the passing point P 0 (X 0 , Y 0 ) when the parameter t is 0 is L s .

Tiは、TsにΔTを加算した値である。 T i is the value obtained by adding ΔT to T s .

Liは、LsにΔLiを加算した値である。 L i is a value obtained by adding [Delta] L i to L s.

しかしながら、ΔLiは、試算点と通過点P0(X0,Y0)との間の直線距離であるため実際の曲線軌跡を移動した距離とは異なる。 However, ΔL i is a linear distance between the estimated point and the passing point P 0 (X 0 , Y 0 ), so it is different from the distance traveled on the actual curve locus.

理想的には、１回目の試算処理の試算点における積算移動距離の理想値Ltは、目標とする積算移動距離Lmと、Lsとの間の中間値である。 Ideally, the ideal value L t of the integrated travel distance at the trial calculation point of the first trial calculation process is an intermediate value between the target integrated travel distance L m and L s .

したがって、１回目の試算処理の試算点における積算移動距離が理想値Ltとなるように基準変動量ΔTを補正する。 Therefore, the reference fluctuation amount ΔT is corrected so that the integrated movement distance at the trial calculation point of the first trial calculation process becomes the ideal value L t .

具体的には、距離の誤差を算出して、誤差分に応じた量だけ基準変動量ΔTを補正する。 Specifically, the error of the distance is calculated, and the reference fluctuation amount ΔT is corrected by the amount corresponding to the error.

図１１（Ｂ）および（Ｃ）を参照して、補正後の基準変動量ΔTadj1は、次式により算出される。また、調整量Δadj1は、次式により算出される。 With reference to FIGS. 11B and 11C, the corrected reference variation amount ΔT adj1 is calculated by the following equation. The adjustment amount Δ adj1 is calculated by the following equation.

Figure 0006801802
Figure 0006801802

そして、補正後の基準変動量ΔTadj1を用いて２回目の試算処理を実行する。 Then, the second trial calculation process is executed using the corrected reference fluctuation amount ΔT adj1 .

［２回目の試算処理］
図１２は、実施形態に従う２回目の試算処理を説明する図である。 [Second trial calculation process]
FIG. 12 is a diagram illustrating a second trial calculation process according to the embodiment.

図１２に示されるように、２回目の試算処理においては第１試算点SPi-1と、第２試算点SPiとを算出する。 As shown in FIG. 12, in the second trial calculation process, the first trial calculation point SP i-1 and the second trial calculation point SP i are calculated.

iは、試算回数を示す。本例においては、i=2である。 i indicates the number of trial calculations. In this example, i = 2.

具体的には、第１試算点SPi-1として、パラメータｔの値をTi-1に設定する。 Specifically, the value of the parameter t is set to T i-1 as the first trial calculation point SP i-1 .

Ti-1は、Ts+ΔTadj1である。 T i-1 is T s + ΔT adj1 .

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTi-1を入力することにより、通過点P0(X0,Y0)から基準変動量ΔTadj1分、移動した第１試算点SPi-1の位置(Xi-1,Yi-1)が算出される。 By inputting T i-1 to the parameter t of the formula shown by the Hermitian curve of the command locus Q, the reference fluctuation amount ΔT adj 1 minute from the passing point P 0 (X 0 , Y 0 ), and the first estimated point SP moved. The position of i-1 (X i-1 , Y i-1 ) is calculated.

通過点P0(X0,Y0)と、基準変動量ΔTadj1分、移動した場合の試算した第１試算点SPi-1の位置(Xi-1,Yi-1)との距離ΔLi-1を算出する。 Distance between the passing point P 0 (X 0 , Y 0 ) and the position (X i-1 , Y i-1 ) of the first estimated point SP i-1 calculated when moving by the reference fluctuation amount ΔT adj 1 minute. Calculate ΔL i-1 .

積算移動距離Li-1は、LsにΔLi-1を加算した値である。 The integrated travel distance L i-1 is the value obtained by adding ΔL i-1 to L s .

次に、第２試算点SPiを算出する。 Next, the second trial calculation point SP i is calculated.

具体的には、第２試算点SPiとして、パラメータｔの値をTiに設定する。 Specifically, the value of the parameter t is set to T i as the second trial calculation point SP i .

Tiは、Ti-1+ΔTadj1である。 T i is T i-1 + ΔT adj1 .

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTiを入力することにより、第１試算点SPi-1から基準変動量ΔTadj1分、移動した第２試算点SPiの位置(Xi,Yi)が算出される。 By inputting T i to the parameter t of the formula shown by the Hermitian curve of the command locus Q, the position (X) of the second trial calculation point SP i moved by the reference fluctuation amount ΔT adj 1 minute from the first trial calculation point SP i-1. i , Y i ) is calculated.

第１試算点SPi-1の位置(Xi-1,Yi-1)と、基準変動量ΔTadj1分、移動した場合の試算した第２試算点SPiの位置(Xi,Yi)との距離ΔLiを算出する。 The position of the first trial calculation point SP i-1 (X i-1 , Y i-1 ) and the position of the second trial calculation point SP i calculated when moving by the reference fluctuation amount ΔT adj 1 minute (X i , Y i). ) And the distance ΔL i is calculated.

積算移動距離Liは、Li-1にΔLiを加算した値である。 Cumulative travel distance L i is a value obtained by adding [Delta] L i to L i-1.

次に、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、第２試算点SPiまでの試算距離Liとを比較する。 Next, the integrated movement distance L m , which is the target from the passing point P 0 (X 0 , Y 0 ), is compared with the estimated distance L i to the second estimated point SP i .

比較結果に基づいて、Lm≦Liである場合には、積算移動距離Lmに対応するTmを算出する。 Based on the comparison result, in the case of L m ≦ L i calculates the T m corresponding to the integrated travel distance L m.

パラメータｔの変動量Tmは、次式により算出される。 The fluctuation amount T m of the parameter t is calculated by the following equation.

Figure 0006801802
Figure 0006801802

そして、試算処理を終了する。 Then, the trial calculation process is completed.

すなわち、指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTmを入力して算出された目標位置Pm(Xm,Ym)が通過点P0(X0,Y0)から次の制御周期で移動する位置に設定される。 That is, the target position P m (X m , Y m ) calculated by inputting T m to the parameter t of the mathematical formula shown by the Hermitian curve of the command locus Q is next from the passing point P 0 (X 0 , Y 0 ). It is set to the position to move in the control cycle of.

本例の如く、基準変動量を補正して、補正した基準変動量を用いて試算点との距離を算出するため、精度の高いパラメータｔの変動量に調整することが可能である。 As in this example, since the reference fluctuation amount is corrected and the distance from the trial calculation point is calculated using the corrected reference fluctuation amount, it is possible to adjust to the fluctuation amount of the parameter t with high accuracy.

一方、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、第２試算点SPiまでの試算距離Liとを比較した比較結果に基づいて、Lm>Liである場合には、３回目の試算処理を実行する。 On the other hand, based on the comparison result of comparing the integrated movement distance L m , which is the target from the passing point P 0 (X 0 , Y 0 ), and the estimated distance L i to the second estimated point SP i , L m > If it is L i , the third trial calculation process is executed.

［基準変動量の補正］
３回目の試算処理を実行する前にパラメータｔの基準変動量ΔTを補正する。 [Correction of reference fluctuation amount]
The reference fluctuation amount ΔT of the parameter t is corrected before executing the third trial calculation process.

具体的には、上記と同様の方式に従って基準変動量を補正する。 Specifically, the reference fluctuation amount is corrected according to the same method as described above.

理想的には、２回目の試算処理の第２試算点SPiにおける積算移動距離の理想値Ltは、目標とする積算移動距離Lmである。 Ideally, the ideal value L t of the integrated travel distance at the second trial calculation point SP i of the second trial calculation process is the target integrated travel distance L m .

したがって、２回目の試算処理の第２試算点SPiにおける積算移動距離が理想値Ltとなるように基準変動量ΔTを補正する。 Therefore, the reference fluctuation amount ΔT is corrected so that the integrated movement distance at the second trial calculation point SP i in the second trial calculation process becomes the ideal value L t .

具体的には、距離の誤差を算出して、誤差分に応じた量だけ基準変動量ΔTを補正する。 Specifically, the error of the distance is calculated, and the reference fluctuation amount ΔT is corrected by the amount corresponding to the error.

補正後の基準変動量ΔTadj2は、次式により算出される。また、調整量Δadj2は、次式により算出される。また、調整量Δadj2は、次式により算出される。 The corrected reference fluctuation amount ΔT adj2 is calculated by the following equation. The adjustment amount Δ adj2 is calculated by the following equation. The adjustment amount Δ adj2 is calculated by the following equation.

Figure 0006801802
Figure 0006801802

そして、補正後の基準変動量ΔTadj2を用いて３回目の試算処理を実行する。 Then, the third trial calculation process is executed using the corrected reference fluctuation amount ΔT adj2 .

［３回目の試算処理］
図１３は、実施形態に従う３回目の試算処理を説明する図である。 [Third trial calculation process]
FIG. 13 is a diagram illustrating a third trial calculation process according to the embodiment.

図１３に示されるように、３回目の試算処理においては第１試算点SPi-2と、第２試算点SPi-1と、第３試算点SPiとを算出する。 As shown in FIG. 13, in the third trial calculation process, the first trial calculation point SP i-2 , the second trial calculation point SP i-1 , and the third trial calculation point SP i are calculated.

iは、試算回数を示す。本例においては、i=3である。 i indicates the number of trial calculations. In this example, i = 3.

具体的には、第１試算点SPi-2として、パラメータｔの値をTi-2に設定する。 Specifically, the value of the parameter t is set to T i-2 as the first trial calculation point SP i-2 .

Ti-2は、Ts+ΔTadj2である。 T i-2 is T s + ΔT adj 2 .

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTi-2を入力することにより、通過点P0(X0,Y0)から基準変動量ΔTadj2分、移動した第１試算点SPi-2の位置(Xi-2,Yi-2)が算出される。 By inputting T i-2 to the parameter t of the formula shown by the Hermitian curve of the command locus Q, the reference fluctuation amount ΔT adj 2 minutes from the passing point P 0 (X 0 , Y 0 ), the first estimated point SP moved. The position of i-2 (X i-2 , Y i-2 ) is calculated.

通過点P0(X0,Y0)と、基準変動量ΔTadj2分、移動した場合の試算した第１試算点SPi-2の位置(Xi-2,Yi-2)との距離ΔLi-2を算出する。 Distance between the passing point P 0 (X 0 , Y 0 ) and the position (X i-2 , Y i-2 ) of the first estimated point SP i-2 calculated when moving by the reference fluctuation amount ΔT adj 2 minutes Calculate ΔL i-2 .

積算移動距離Li-2は、LsにΔLi-2を加算した値である。 The integrated travel distance L i-2 is the value obtained by adding ΔL i-2 to L s .

次に、第２試算点SPi-1を算出する。 Next, the second trial calculation point SP i-1 is calculated.

具体的には、第２試算点SPi-1として、パラメータｔの値をTi-1に設定する。 Specifically, the value of the parameter t is set to T i-1 as the second trial calculation point SP i-1 .

Ti-1は、Ti-2+ΔTadj2である。 T i-1 is T i-2 + ΔT adj2 .

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTi-1を入力することにより、第１試算点SPi-2から基準変動量ΔTadj2分、移動した第２試算点SPi-1の位置(Xi-1,Yi-1)が算出される。 By inputting T i-1 to the parameter t of the formula shown by the Hermitian curve of the command locus Q, the reference fluctuation amount ΔT adj 2 minutes from the first trial calculation point SP i-2 , and the second trial calculation point SP i-1 moved. The position of (X i-1 , Y i-1 ) is calculated.

第２試算点SPi-1の位置(Xi-1,Yi-1)と、基準変動量ΔTadj2分、移動した場合の試算した第２試算点SPi-1の位置(Xi-1,Yi-1)との距離ΔLi-1を算出する。 The position of the second trial calculation point SP i-1 (X i-1 , Y i-1 ) and the position of the second trial calculation point SP i-1 calculated when moving by the reference fluctuation amount ΔT adj 2 minutes (X i- 1. Calculate the distance ΔL i-1 from Y i-1 ).

積算移動距離Li-1は、Li-2にΔLi-1を加算した値である。 The integrated travel distance L i-1 is the value obtained by adding ΔL i-1 to L i-2 .

次に、第３試算点SPiを算出する。 Next, the third trial calculation point SP i is calculated.

具体的には、第３試算点SPiとして、パラメータｔの値をTiに設定する。 Specifically, the value of the parameter t is set to T i as the third trial calculation point SP i .

Tiは、Ti-1+ΔTadj2である。 T i is T i-1 + ΔT adj2 .

指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTiを入力することにより、第２試算点SPi-1から基準変動量ΔTadj2分、移動した第３試算点SPiの位置(Xi,Yi)が算出される。 By inputting T i to the parameter t of the formula shown by the Hermitian curve of the command locus Q, the position (X) of the third trial calculation point SP i moved by the reference fluctuation amount ΔT adj 2 minutes from the second trial calculation point SP i-1. i , Y i ) is calculated.

第２試算点SPi-1の位置(Xi-1,Yi-1)と、基準変動量ΔTadj2分、移動した場合の試算した第３試算点SPiの位置(Xi,Yi)との距離ΔLiを算出する。 The position of the second trial calculation point SP i-1 (X i-1 , Y i-1 ) and the position of the third trial calculation point SP i calculated when moving by the reference fluctuation amount ΔT adj 2 minutes (X i , Y i) ) And the distance ΔL i is calculated.

積算移動距離Liは、Li-1にΔLiを加算した値である。 Cumulative travel distance L i is a value obtained by adding [Delta] L i to L i-1.

次に、通過点P0(X0,Y0)からの目標である積算移動距離Lmと、第３試算点SPiまでの試算距離Liとを比較する。 Next, the integrated movement distance L m , which is the target from the passing point P 0 (X 0 , Y 0 ), is compared with the estimated distance L i to the third estimated point SP i .

比較結果に基づいて、Lm≦Liである場合には、積算移動距離Lmに対応するTmを算出する。 Based on the comparison result, in the case of L m ≦ L i calculates the T m corresponding to the integrated travel distance L m.

パラメータｔの変動量Tmは、次式により算出される。 The fluctuation amount T m of the parameter t is calculated by the following equation.

Figure 0006801802
Figure 0006801802

そして、試算処理を終了する。 Then, the trial calculation process is completed.

すなわち、指令軌跡Qのエルミート曲線で示される数式のパラメータｔにTmを入力して算出された目標位置Pm(Xm,Ym)が通過点P0(X0,Y0)から次の制御周期で移動する位置に設定される。 That is, the target position P m (X m , Y m ) calculated by inputting T m to the parameter t of the mathematical formula shown by the Hermitian curve of the command locus Q is next from the passing point P 0 (X 0 , Y 0 ). It is set to the position to move in the control cycle of.

そして、設定した位置を基点として、上記で説明した方式を用いて次の制御周期で移動する位置を算出する。 Then, with the set position as the base point, the position to move in the next control cycle is calculated using the method described above.

これにより、変数調整部４４は、距離算出部４２で算出された目標移動距離が一定となるように制御周期毎のパラメータｔの増分値を調整することが可能となる。通過点P0(X0,Y0)から通過点P1(X1,Y1)への指令軌跡Q0に沿って制御周期毎に移動する位置を設定する。 As a result, the variable adjustment unit 44 can adjust the increment value of the parameter t for each control cycle so that the target movement distance calculated by the distance calculation unit 42 becomes constant. Set the position to move in each control cycle along the command locus Q0 from the passing point P 0 (X 0 , Y 0 ) to the passing point P 1 (X 1 , Y 1 ).

一方で、通過点P1(X1,Y1)付近になった場合に、試算処理において、パラメータｔの値であるTiが１を超える場合がある。すなわち、通過点P1(X1,Y1)を跨ぐ場合である。 On the other hand, when it is near the passing point P 1 (X 1 , Y 1 ), T i, which is the value of the parameter t, may exceed 1 in the trial calculation process. That is, it is a case of straddling the passing point P 1 (X 1 , Y 1 ).

パラメータｔの値であるTiが１を超える場合には、対象とする指令軌跡を変更する。通過点P1(X1,Y1)から通過点P2(X2,Y2)への指令軌跡Q1に対応する区分多項式に変更する。すなわち、通過点P1(X1,Y1)と通過点P2(X2,Y2)との間の区分多項式に変更する。 When T i, which is the value of the parameter t, exceeds 1, the target command locus is changed. Change to the partitioning polynomial corresponding to the command locus Q1 from the passing point P 1 (X 1 , Y 1 ) to the passing point P 2 (X 2 , Y 2 ). That is, it is changed to a partitioning polynomial between the passing point P 1 (X 1 , Y 1 ) and the passing point P 2 (X 2 , Y 2 ).

そして、Ti-1をTmとして指令軌跡Q1に対応する区分多項式のパラメータｔに当該Tmを入力して算出された目標位置Pm(Xm,Ym)が次の制御周期で移動する位置に設定される。 Then, the target position P m (X m , Y m ) calculated by inputting the T m into the parameter t of the divisional polynomial corresponding to the command locus Q1 with Ti-1 as T m moves in the next control cycle. Set to position.

当該処理により、各通過点間のパラメータｔの変動率を調整することが可能である。 By this process, it is possible to adjust the volatility of the parameter t between each passing point.

これにより、各通過点間における軌跡移動速度を基準速度V0で一定に制御することが可能である。すなわち、制御周期毎の目標移動距離を一定にして動作速度を適切に制御することが可能である。 As a result, the locus movement speed between each passing point can be controlled to be constant at the reference speed V 0 . That is, it is possible to appropriately control the operating speed by keeping the target moving distance for each control cycle constant.

＜Ｄ．シミュレーション例＞
次に、実施形態に従う制御システム１のシミュレーション例について説明する。 <D. Simulation example>
Next, a simulation example of the control system 1 according to the embodiment will be described.

図１４は、実施形態に従う１４個の通過点を設定した場合のシミュレーション例について説明する図である。 FIG. 14 is a diagram illustrating a simulation example when 14 passing points are set according to the embodiment.

図１４に示されるように、１４個の通過点が示されており、数字は、通過順序を示している。 As shown in FIG. 14, 14 passing points are shown, and the numbers indicate the passing order.

図１５は、実施形態に従う通過座標および３次多項式の係数について説明する図である。 FIG. 15 is a diagram illustrating passing coordinates and coefficients of a cubic polynomial according to an embodiment.

図１５を参照して、本例においては、最初の始点座標および最後の終点座標は同じ（０，０）に設定されている。また、１４個のそれぞれの通過点の座標が設定されている。 With reference to FIG. 15, in this example, the first start point coordinates and the last end point coordinates are set to be the same (0,0). In addition, the coordinates of each of the 14 passing points are set.

また、所定値Rは５０に設定している。 Further, the predetermined value R is set to 50.

上記で説明した方式に基づいて各区間のｘ座標の３次多項式の係数およびｙ座標の３次多項式の係数がそれぞれ算出される。 Based on the method described above, the coefficient of the cubic polynomial of the x-coordinate and the coefficient of the cubic polynomial of the y-coordinate of each section are calculated.

図１６は、実施形態に従うトレース軌跡を説明する図である。 FIG. 16 is a diagram illustrating a trace locus according to an embodiment.

図１６に示されるように、設定された順序に従って１４個の通過点を連続的にかつ滑らかに通過する指令軌跡が描かれている。 As shown in FIG. 16, a command locus is drawn that continuously and smoothly passes through 14 passing points according to a set order.

図１７は、実施形態に従う指令速度の波形を説明する図である。 FIG. 17 is a diagram illustrating a waveform of a command speed according to an embodiment.

図１７に示されるように、本例においては、指令速度として、500mm/sに設定した場合が示されている。 As shown in FIG. 17, in this example, a case where the command speed is set to 500 mm / s is shown.

なお、加速度および減速度は、1000mm/s2とした。また、ジャークは、10000mm/s3に設定した。 The acceleration and deceleration were set to 1000 mm / s 2 . The jerk was set to 10000 mm / s 3 .

当該図に示されるように、各通過点間における軌跡移動速度を基準速度V0で一定に制御することが可能であることを確認することが可能である。 As shown in the figure, it is possible to confirm that the locus movement speed between each passing point can be controlled to be constant at the reference speed V 0 .

なお、本例においては、一例として平面上における通過順序が予め設定された複数の通過点を連続的に通過する各通過点間の指令軌跡として、区分多項式である３次のエルミート曲線で設定する方式について説明した。一方で、２次元の平面上の通過点に限られず３次元の空間上の通過点についても同様に適用可能である。すなわち、ｚ座標についてもエルミート曲線で定義し、３次元の空間上の通過点に対して通過点の速度ベクトルを３次元座標のベクトルとして処理すればよい。 In this example, as an example, a cubic Hermitian curve, which is a divisional polynomial, is set as a command locus between each passing point that continuously passes through a plurality of passing points in which the passing order on the plane is set in advance. The method was explained. On the other hand, the same applies not only to passing points on a two-dimensional plane but also to passing points on a three-dimensional space. That is, the z coordinate may also be defined by the Elmeet curve, and the velocity vector of the passing point may be processed as the vector of the three-dimensional coordinates with respect to the passing point in the three-dimensional space.

また、本例においては、各通過点間の指令軌跡の区分多項式として一例として、３次のエルミート曲線で設定する場合について説明したが、特にこれに限られず他の曲線等を用いることも可能である。すなわち、各通過点間の指令軌跡を１つの共通のパラメータｔで設定するスプライン関数であればどのようなものを採用しても良い。次数も３次に限られず、さらに高次でも良いし、２次あるいは１次の直線としてもよい。 Further, in this example, the case of setting a cubic Hermitian curve as an example as a division polynomial of the command locus between each passing point has been described, but the case is not particularly limited to this, and other curves and the like can be used. is there. That is, any spline function that sets the command locus between each passing point with one common parameter t may be adopted. The order is not limited to the third order, and may be a higher order, or may be a second or first order straight line.

指令軌跡が直線の場合には、１次のスプライン関数として定義することが可能である。 When the command locus is a straight line, it can be defined as a first-order spline function.

この場合であっても距離算出部４２で算出された目標移動距離が一定となるように各通過点間の制御周期毎のパラメータｔの増分値を調整することにより、軌跡移動速度を一定に制御することが可能である。 Even in this case, the locus movement speed is controlled to be constant by adjusting the increment value of the parameter t for each control cycle between the passing points so that the target movement distance calculated by the distance calculation unit 42 is constant. It is possible to do.

＜Ｄ．付記＞
上述したような本実施の形態は、以下のような技術思想を含む。 <D. Addendum>
The present embodiment as described above includes the following technical ideas.

［構成１］
コントローラシステム（１００）は、通過順序が予め設定された複数の通過点の位置を設定する設定部（３０）と、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を区分多項式で設定する軌跡設定部（３５）と、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出する指令値算出部（４０）とを備える。区分多項式は、第１変数に従う３次のエルミート曲線で定義される。指令値算出部は、制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部（４２）と、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整する変数調整部（４４）とを含む。 [Structure 1]
The controller system (100) has a setting unit (30) that sets the positions of a plurality of passing points in which the passing order is preset, and a command locus between the passing points in which the controlled object continuously passes through the plurality of passing points. Is provided with a locus setting unit (35) for setting the above with a division polynomial, and a command value calculation unit (40) for calculating a target command value of a control target for each control cycle based on the division polynomial corresponding to the target command locus. .. The partitioning polynomial is defined by a cubic Hermitian curve that follows the first variable. The command value calculation unit includes a distance calculation unit (42) that calculates the target movement distance with respect to the command control speed for each control cycle, and a fluctuation amount of the first variable for each control cycle so that the calculated target movement distance becomes constant. Includes a variable adjustment unit (44) for adjusting.

［構成２］
第１変数は、０〜１の間で変化する。 [Structure 2]
The first variable varies between 0 and 1.

［構成３］
区分多項式は、３次のエルミート曲線である。 [Structure 3]
The partitioning polynomial is a Hermitian curve of degree 3.

［構成４］
変数調整部は、算出された目標移動距離に基づいて第１変数の基準となる基準変動量ΔTを設定し、基準変動量に基づいて区分多項式に基づく制御対象の座標を少なくとも１回算出し、前回の制御周期における制御対象の座標と、基準変動量に基づく算出された制御対象の座標との間の距離を算出し、算出された基準変動量に対する距離と目標移動距離との比較に基づいて、制御周期毎の第１変数の変動量を調整する。 [Structure 4]
The variable adjustment unit sets the reference fluctuation amount ΔT as the reference of the first variable based on the calculated target movement distance, calculates the coordinates of the control target based on the division polynomial based on the reference fluctuation amount at least once, and then calculates. The distance between the coordinates of the control target in the previous control cycle and the coordinates of the control target calculated based on the reference fluctuation amount is calculated, and the distance to the calculated reference fluctuation amount is compared with the target movement distance. , Adjust the amount of fluctuation of the first variable for each control cycle.

［構成５］
変数調整部は、基準変動量毎の区分多項式に基づく制御対象の座標を複数回算出し、基準変動量毎の制御対象の座標間のそれぞれの距離を算出し、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とに基づいて第１変数の基準となる基準変動量を補正する。 [Structure 5]
The variable adjustment unit calculates the coordinates of the control target based on the division polynomial for each reference fluctuation amount multiple times, calculates the distance between the coordinates of the control target for each reference fluctuation amount, and determines the control target for each reference fluctuation amount. The reference fluctuation amount that is the reference of the first variable is corrected based on the total distance between the coordinates and the target movement distance.

［構成６］
変数調整部は、基準変動量毎の制御対象の座標間の距離の合計と、目標移動距離とを比較し、基準変動量毎の制御対象の座標間の距離の合計が目標移動距離以上の場合に目標移動距離に対する第１変数の変動量を算出する。 [Structure 6]
The variable adjustment unit compares the total distance between the control target coordinates for each reference fluctuation amount with the target movement distance, and when the total distance between the control target coordinates for each reference fluctuation amount is equal to or greater than the target movement distance. The amount of fluctuation of the first variable with respect to the target movement distance is calculated.

［構成７］
変数調整部は、対象とする指令軌跡の通過点間の距離と、目標移動距離と、分割係数ｋとに基づいて基準変動量を設定する。 [Structure 7]
The variable adjustment unit sets the reference fluctuation amount based on the distance between the passing points of the target command locus, the target movement distance, and the division coefficient k.

［構成８］
コントローラシステムの制御方法は、通過順序が予め設定された複数の通過点の位置を設定するステップと、制御対象が複数の通過点を連続的に通過する各通過点間の指令軌跡を区分多項式で設定するステップと、対象とする指令軌跡に対応する区分多項式に基づいて制御周期毎の制御対象の目標指令値を算出するステップとを備える。区分多項式は、第１変数に従う３次のエルミート曲線で定義され、目標指令値を算出するステップは、制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む。 [Structure 8]
The control method of the controller system is a step of setting the positions of a plurality of passing points in which the passing order is preset, and a division polynomial for the command locus between the passing points in which the controlled object continuously passes through the plurality of passing points. It includes a step of setting and a step of calculating a target command value of a control target for each control cycle based on a division polynomial corresponding to a target command locus. The partitioning polynomial is defined by a cubic Elmeat curve according to the first variable, and the steps for calculating the target command value are the step for calculating the target movement distance with respect to the command control speed for each control cycle and the calculated target movement distance. It includes a step of adjusting the fluctuation amount of the first variable for each control cycle so as to be constant.

＜Ｅ．利点＞
本実施の形態に従うコントローラシステムおよびその制御方法によれば、制御対象が複数の通過点を連続的に通過する場合に動作速度を適切に制御することが可能である。 <E. Advantages>
According to the controller system and the control method thereof according to the present embodiment, it is possible to appropriately control the operating speed when the controlled object continuously passes through a plurality of passing points.

今回開示された実施の形態はすべての点で例示であって制限的なものではないと考えられるべきである。本発明の範囲は、上記した説明ではなく、特許請求の範囲によって示され、特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれることが意図される。 It should be considered that the embodiments disclosed this time are exemplary in all respects and not restrictive. The scope of the present invention is shown by the scope of claims, not the above description, and is intended to include all modifications within the meaning and scope equivalent to the scope of claims.

１ レビュー制御システム、２ 駆動装置、２０ 撮影装置、３０ 設定部、３５ 軌跡設定部、４０ サーボ指令制御部、４２ 距離算出部、４４ 変数調整部、５０ トリガ指令制御部、１００ 制御装置、１０１ 制御演算部、１０２ 通信Ｉ／Ｆ部、１０３ 記憶部、１０４ 入力部、１０５ 出力部、１０６ バスライン。 1 Review control system, 2 Drive device, 20 Imaging device, 30 Setting unit, 35 Trajectory setting unit, 40 Servo command control unit, 42 Distance calculation unit, 44 Variable adjustment unit, 50 Trigger command control unit, 100 Control device, 101 control Calculation unit, 102 communication I / F unit, 103 storage unit, 104 input unit, 105 output unit, 106 bus line.

Claims (8)

通過順序が予め設定された複数の通過点の位置を設定する設定部と、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令軌跡を第１変数に従う区分多項式で設定する軌跡設定部と、
対象とする指令軌跡に対応する前記区分多項式に基づいて制御周期毎の前記制御対象の目標指令値を算出する指令値算出部とを備え、
前記指令値算出部は、
前記制御周期毎の指令制御速度に対する目標移動距離を算出する距離算出部と、
前記算出された目標移動距離が一定となるように前記制御周期毎の第１変数の変動量を調整する変数調整部とを含む、コントローラシステム。 A setting unit that sets the positions of multiple passing points whose passing order is preset, and
A locus setting unit that sets a command locus between each of the passing points where the controlled object continuously passes through the plurality of passing points with a division polynomial according to the first variable.
It is provided with a command value calculation unit that calculates a target command value of the control target for each control cycle based on the division polynomial corresponding to the command locus of the target.
The command value calculation unit
A distance calculation unit that calculates the target movement distance with respect to the command control speed for each control cycle,
A controller system including a variable adjusting unit that adjusts a fluctuation amount of a first variable for each control cycle so that the calculated target moving distance becomes constant. 前記第１変数は、０〜１の間で変化する、請求項１記載のコントローラシステム。 The controller system according to claim 1, wherein the first variable varies between 0 and 1. 前記区分多項式は、３次のエルミート曲線である、請求項２記載のコントローラシステム。 The controller system according to claim 2, wherein the partitioning polynomial is a Hermitian curve of degree 3. 前記変数調整部は、
前記算出された目標移動距離に基づいて前記第１変数の基準となる基準変動量を設定し、
前記基準変動量に基づいて前記区分多項式に基づく前記制御対象の座標を少なくとも１回算出し、
前回の制御周期における前記制御対象の座標と、前記基準変動量に基づく算出された前記制御対象の座標との間の距離を算出し、
算出された基準変動量に対する距離と前記目標移動距離との比較に基づいて、前記制御周期毎の第１変数の変動量を調整する、請求項２記載のコントローラシステム。 The variable adjustment unit
Based on the calculated target movement distance, a reference fluctuation amount that serves as a reference for the first variable is set.
Based on the reference variation amount, the coordinates of the control target based on the division polynomial are calculated at least once.
The distance between the coordinates of the control target in the previous control cycle and the coordinates of the control target calculated based on the reference fluctuation amount is calculated.
The controller system according to claim 2, wherein the fluctuation amount of the first variable for each control cycle is adjusted based on the comparison between the calculated distance to the reference fluctuation amount and the target movement distance. 前記変数調整部は、
前記基準変動量毎の前記区分多項式に基づく前記制御対象の座標を複数回算出し、
前記基準変動量毎の前記制御対象の座標間のそれぞれの距離を算出し、
前記基準変動量毎の前記制御対象の座標間の距離の合計と、前記目標移動距離とに基づいて前記第１変数の基準となる前記基準変動量を補正する、請求項４記載のコントローラシステム。 The variable adjustment unit
The coordinates of the control target based on the division polynomial for each reference fluctuation amount are calculated a plurality of times.
The distance between the coordinates of the control target for each reference fluctuation amount is calculated.
The controller system according to claim 4, wherein the reference fluctuation amount, which is the reference of the first variable, is corrected based on the total distance between the coordinates of the control target for each reference fluctuation amount and the target movement distance. 前記変数調整部は、
前記基準変動量毎の前記制御対象の座標間の距離の合計と、前記目標移動距離とを比較し、
前記基準変動量毎の前記制御対象の座標間の距離の合計が前記目標移動距離以上の場合に前記目標移動距離に対する前記第１変数の変動量を算出する、請求項４記載のコントローラシステム。 The variable adjustment unit
Comparing the total distance between the coordinates of the controlled object for each reference fluctuation amount with the target moving distance,
The controller system according to claim 4, wherein when the total distance between the coordinates of the controlled object for each reference fluctuation amount is equal to or greater than the target movement distance, the fluctuation amount of the first variable with respect to the target movement distance is calculated. 前記変数調整部は、前記対象とする指令軌跡の通過点間の距離と、前記目標移動距離と、分割係数とに基づいて前記基準変動量を設定する、請求項４記載のコントローラシステム。 The controller system according to claim 4, wherein the variable adjustment unit sets the reference fluctuation amount based on the distance between the passing points of the target command locus, the target movement distance, and the division coefficient. 通過順序が予め設定された複数の通過点の位置を設定するステップと、
制御対象が前記複数の通過点を連続的に通過する各前記通過点間の指令軌跡を第１変数に従う区分多項式で設定するステップと、
対象とする指令軌跡に対応する前記区分多項式に基づいて制御周期毎の前記制御対象の目標指令値を算出するステップとを備え、
前記目標指令値を算出するステップは、
前記制御周期毎の指令制御速度に対する目標移動距離を算出するステップと、
前記算出された目標移動距離が一定となるように制御周期毎の第１変数の変動量を調整するステップとを含む、コントローラシステムの制御方法。 Steps to set the positions of multiple passing points with preset passing order,
A step of setting a command locus between each of the passing points in which the controlled object continuously passes through the plurality of passing points with a dividing polynomial according to the first variable, and
It includes a step of calculating a target command value of the control target for each control cycle based on the division polynomial corresponding to the command locus of the target.
The step of calculating the target command value is
The step of calculating the target movement distance with respect to the command control speed for each control cycle, and
A control method of a controller system including a step of adjusting a fluctuation amount of a first variable for each control cycle so that the calculated target movement distance becomes constant.
JP2020042303A 2020-03-11 2020-03-11 Controller system and its control method Active JP6801802B1 (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2020042303A JP6801802B1 (en) 2020-03-11 2020-03-11 Controller system and its control method
CN202110142907.1A CN113467360A (en) 2020-03-11 2021-02-02 Controller system and control method thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2020042303A JP6801802B1 (en) 2020-03-11 2020-03-11 Controller system and its control method

Publications (2)

Publication Number Publication Date
JP6801802B1 true JP6801802B1 (en) 2020-12-16
JP2021144438A JP2021144438A (en) 2021-09-24

Family

ID=73740962

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2020042303A Active JP6801802B1 (en) 2020-03-11 2020-03-11 Controller system and its control method

Country Status (2)

Country Link
JP (1) JP6801802B1 (en)
CN (1) CN113467360A (en)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0628024A (en) * 1992-04-23 1994-02-04 Philips Electron Nv Numerically controlled apparatus and tool- route decision method
JPH06230818A (en) * 1992-09-25 1994-08-19 Nec Corp Servo controller
JPH11104982A (en) * 1997-10-01 1999-04-20 Meidensha Corp Teaching method of robot
JP2002018767A (en) * 2000-06-30 2002-01-22 Kawasaki Heavy Ind Ltd Carrying device
JP2007000954A (en) * 2005-06-22 2007-01-11 Nachi Fujikoshi Corp Robot teaching device and method
JP2012083982A (en) * 2010-10-13 2012-04-26 Omron Corp Controller, control system, and control method
JP2014137750A (en) * 2013-01-17 2014-07-28 Hitachi Solutions Ltd Information processing device, information processing system, and information processing program
KR20150104484A (en) * 2014-03-05 2015-09-15 국방과학연구소 Method and apparatus for generating pathe of autonomous vehicle

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0628024A (en) * 1992-04-23 1994-02-04 Philips Electron Nv Numerically controlled apparatus and tool- route decision method
JPH06230818A (en) * 1992-09-25 1994-08-19 Nec Corp Servo controller
JPH11104982A (en) * 1997-10-01 1999-04-20 Meidensha Corp Teaching method of robot
JP2002018767A (en) * 2000-06-30 2002-01-22 Kawasaki Heavy Ind Ltd Carrying device
JP2007000954A (en) * 2005-06-22 2007-01-11 Nachi Fujikoshi Corp Robot teaching device and method
JP2012083982A (en) * 2010-10-13 2012-04-26 Omron Corp Controller, control system, and control method
JP2014137750A (en) * 2013-01-17 2014-07-28 Hitachi Solutions Ltd Information processing device, information processing system, and information processing program
KR20150104484A (en) * 2014-03-05 2015-09-15 국방과학연구소 Method and apparatus for generating pathe of autonomous vehicle

Also Published As

Publication number Publication date
CN113467360A (en) 2021-10-01
JP2021144438A (en) 2021-09-24

Similar Documents

Publication Publication Date Title
JP5762625B2 (en) Trajectory control device
US8831777B2 (en) Robot program changing device
US10248101B2 (en) Tool path generation method and machine tool using rate of change of curvature based smoothing
US8774965B2 (en) Method and device for controlling a manipulator
JP5202735B2 (en) Numerical controller
JP5562430B2 (en) Tool path generation method and generation apparatus
US9651939B2 (en) Numerical controller having display function of physical data of machine tool
US20160341533A1 (en) Method for controlling shape measuring apparatus
JP2009053926A (en) Route-planning device and route-planning method
CN109313429B (en) S-shaped speed planning method, device and system, robot and numerical control machine
JP6801802B1 (en) Controller system and its control method
KR101743795B1 (en) Curve interpolation method in positioning control system
JP2790643B2 (en) Numerical control unit
JP6863497B1 (en) Controller system and its control method
JP5679898B2 (en) Trajectory control device
JP4545701B2 (en) Speed control method and speed control apparatus
JP2737725B2 (en) Robot control device and method
JP2006294053A (en) Numerical control method and speed control device
CN105676898B (en) A kind of robot end&#39;s force control method based on joint position controller
JP7508920B2 (en) Control device and design method
US11654562B2 (en) Apparatus, robot control device, robot system, and method of setting robot coordinate system
KR20220094819A (en) Spline path generating device
JP2023038747A (en) Controller system and control method thereof
JPH09141581A (en) Robot control device
JP2716446B2 (en) Servo control device

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20200409

A871 Explanation of circumstances concerning accelerated examination

Free format text: JAPANESE INTERMEDIATE CODE: A871

Effective date: 20200409

A975 Report on accelerated examination

Free format text: JAPANESE INTERMEDIATE CODE: A971005

Effective date: 20200615

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20200714

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20200915

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20201027

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20201109

R150 Certificate of patent or registration of utility model

Ref document number: 6801802

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150