JP2023524897A - Multiload Adaptive Gravity Compensation Method, Apparatus and Control Device for Robotic Arm - Google Patents

Abstract

Translated fromJapanese

本発明は、ロボットアームの多負荷適応重力補償法を提供する。当該ロボットアームの多負荷適応重力補償法は、ロボットアームの運動学モデルを構築するステップＳ１．１と、動力学モデルの重力項を再構築するステップＳ１．２と、無負荷静的位置サンプリングをするステップＳ１．３と、各ツールを取り付けた後にそれぞれ静的位置サンプリングをするステップＳ１．４と、各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算するステップＳ１．５と、各ツールの質量及び質量中心をそれぞれ計算するステップＳ１．６と、現在取り付けられているツールによりフランジに加えられる力を計算するステップＳ２．１と、ツールの重力を補償するステップＳ２．２とを備える。当該方法は、操作ステップを減らし、効率及び重力補償の性能を高めることができる。【選択図】図１The present invention provides a multi-load adaptive gravity compensation method for robotic arms. The multi-load adaptive gravity compensation method for the robot arm includes steps S1.1 of building a kinematic model of the robot arm, steps S1.2 of reconstructing the gravity term of the dynamic model, and no-load static position sampling. step S1.3 for each static position sampling after mounting each tool; step S1.5 for calculating each parameter value awaiting calibration for each tool; It comprises a step S1.6 respectively calculating the center of mass, a step S2.1 calculating the force exerted on the flange by the currently mounted tool, and a step S2.2 compensating for the gravitational force of the tool. The method can reduce operation steps and increase efficiency and performance of gravity compensation. [Selection drawing] Fig. 1

Description

Translated fromJapanese

本願は、ロボット技術分野に関し、特に、ロボットアームの多負荷適応重力補償法、装置及び制御デバイスに関する。 TECHNICAL FIELD The present application relates to the field of robotic technology, and more particularly to multi-load adaptive gravity compensation methods, apparatus and control devices for robotic arms.

＜関連出願の相互引用＞
本願は、２０２０年５月２８日に中国特許庁に提出された、出願番号が２０２０１０４６６０９９．Ｘであり、名称が「ロボットアームの多負荷適応重力補償法」である中国特許出願の優先権を主張し、その全ての内容が引用により本願に組み込まれている。<Cross-citation of related applications>
This application is filed with the Chinese Patent Office on May 28, 2020, with application number 202010466099. X and entitled “Multi-Load Adaptive Gravity Compensation Method for Robotic Arm”, the Chinese patent application, the entire content of which is incorporated herein by reference.

ロボットアーム製造業及びセンサー産業の更なる発展に伴い、ロボットアームは、既に生産ラインに役立つだけでなく、生活の様々な分野にも徐々に参入し始めている。従来の産業用ロボットアームは、人身事故を防ぐために、セキュリティ範囲を設定し、操作中に人員が作業エリアに立ち入ることを厳しく禁止する必要がある。しかしながら、多くの生活の応用シナリオでは、セキュリティ範囲を設定する際に多くの不都合が生じ、人間と機械の協調作業の際においても効率が高くない。人間と機械の作業空間を分断しないことにより、本当の高効率で且つ高精度の人間と機械の協調作業を実現するために、人々は、協調型ロボットアームを設計した。協調型ロボットアームは、接触力を感知する能力を有し、人体とロボットアームの物理的な接触に反応することができるため、オペレーターとロボットアームは、作業空間を共有することができる。協調型ロボットアームの登場により、ホームケア、教育娯楽、健康医療、高度な製造業等の産業におけるロボットアームの応用が大幅に拡大され、ロボットアームの高効率、高精度、高安定性の特徴を利用して生活のあらゆる側面を改善することができた。 With the further development of the robot arm manufacturing industry and the sensor industry, the robot arm has not only served the production line, but also gradually entered various fields of life. Conventional industrial robot arms need to set security perimeters and strictly prohibit personnel from entering the work area during operation in order to prevent personal injury. However, in many life application scenarios, there are many inconveniences in setting the security scope, and the efficiency of human-machine cooperation is not high. In order to realize true high-efficiency and high-precision human-machine collaborative work by not dividing the human-machine working space, people designed collaborative robot arms. A collaborative robotic arm has the ability to sense contact forces and can respond to physical contact between the human body and the robotic arm, allowing the operator and the robotic arm to share a workspace. With the advent of cooperative robot arms, the application of robot arms in home care, education and entertainment, health care, advanced manufacturing and other industries has been greatly expanded, highlighting the characteristics of high efficiency, high precision and high stability of robot arms. I was able to use it to improve every aspect of my life.

ゼロフォース制御技術とは、ドラッグティーチングの過程では、ロボットアーム自身の重力の影響を受けていないかのように、ロボットアームがスムーズに外力に応じて移動できることを意味する。この技術は、ドラッグティーチングの労働強度を軽減し、人間によりロボットアームを制御する際の流暢さを向上させることができる。ロボットアームがエンドツールをクランプした状態でも、依然としてゼロフォース制御を実現することができるために、ロボットアーム本体とツールに対してそれぞれパラメータのキャリブレーションをし、リバースエンジニアリング手法を用いてロボットアームの各セグメントのアーム及びツールの質量及び質量中心を正確に計算する必要がある。ロボットアーム本体のパラメータのキャリブレーションの技術は、文献Ｉｄｅｎｔｉｆｙｉｎｇ ｔｈｅ ｄｙｎａｍｉｃ ｍｏｄｅｌ ｕｓｅｄ ｂｙ ｔｈｅ ＫＵＫＡ ＬＷＲ： Ａ ｒｅｖｅｒｓｅ ｅｎｇｉｎｅｅｒｉｎｇ ａｐｐｒｏａｃｈ．（Ｃ．Ｇａｚ，Ｆ．Ｆｌａｃｃｏ）及びＧｒａｖｉｔｙ ｃｏｍｐｅｎｓａｔｉｏｎ ｏｆ ＫＵＫＡ ＬＢＲ ＩＩＷＡ Ｔｈｒｏｕｇｈ Ｆａｓｔ Ｒｏｂｏｔ Ｉｎｔｅｒｆａｃｅ．（Ｃ．Ｈｏｕ，Ｙ．Ｚｈａｏ）に詳しく紹介されているが、エンドツールのパラメータのキャリブレーションに関する資料が比較的少ない。一部の複雑な応用では、ロボットアームは、作業を完了するために、エンドツールを交換する必要がある。この場合、ロボットアームは、異なるエンドツールの全てがゼロフォース制御を得られるために、如何にツールの重力を適応的に補償できるかが、協調作業がスムーズであるかどうかの鍵になる。 Zero-force control technology means that the robot arm can move smoothly in response to external forces during the drag teaching process, as if it were not affected by the gravity of the robot arm itself. This technology can reduce the labor intensity of drug teaching and improve the fluency in controlling the robot arm by humans. In order to still achieve zero force control even when the robot arm clamps the end tool, we calibrate the parameters for the robot arm body and the tool, and use the reverse engineering method to calibrate the parameters of the robot arm. The segment arm and tool masses and centers of mass must be accurately calculated. A technique for calibrating the parameters of the robot arm body is described in the document Identifying the dynamic model used by the KUKA LWR: A reverse engineering approach. (C. Gaz, F. Flacco) and Gravity compensation of KUKA LBR IIWA Through Fast Robot Interface. (C. Hou, Y. Zhao), there is relatively little documentation on the calibration of end tool parameters. In some complex applications, the robotic arm needs to change end tools to complete the task. In this case, since the robot arm can obtain zero force control for all different end tools, the key to smooth cooperative work is how to adaptively compensate for the gravity of the tools.

現在、ロボットアームの重力補償の方案は、一般的には、まず、計量器を用いてエンドツールの質量を測定し、吊り下げ法又は支持法を用いてツールの質量中心を測定する必要がある。次に、測定して得られたデータをロボットアームの制御システムに導入し、制御システムにツールのパラメータに基づいて重力の補償を行わせ、ロボットアームにゼロフォース制御を実現させる。ただし、ツールの質量と質量中心を測定する際には、ツールがシステムと分離されている状態であり、測定して得られたパラメータは、取り付けプロセスが質量と質量中心に対する影響を省略する傾向がある。また、一度に１つのツールのパラメータのみに対して重力の補償を行うことができ、ツールを切り替える時にプログラムを停止する必要があり、複数のツールを頻繁に切り替える必要がある場合、効率が低下してしまう。 At present, the gravitational compensation scheme of the robot arm generally needs to first use the weighing scale to measure the mass of the end tool, and then use the suspension method or the support method to measure the center of mass of the tool. . Then, the data obtained by the measurement is introduced into the control system of the robot arm, and the control system is made to perform gravity compensation based on the parameters of the tool, so that the robot arm achieves zero force control. However, when measuring the mass and center of mass of the tool, the tool is isolated from the system and the measured parameters tend to omit the effects of the installation process on mass and center of mass. be. Also, gravity compensation can only be done for one tool parameter at a time, requiring the program to be stopped when switching tools, which is less efficient if multiple tools need to be switched frequently. end up

本願の目的の１つとしては、ロボットアームの多負荷適応重力補償法、装置、制御デバイス及び読み取り可能な記憶媒体を提供することで、操作ステップを減らし、効率及び重力補償の性能を高めることが含まれている。 One of the objectives of the present application is to provide a multi-load adaptive gravity compensation method, apparatus, control device and readable storage medium for robotic arms to reduce operation steps and increase efficiency and performance of gravity compensation. include.

上述した目的を実現するために、本願による技術案は、以下の通りである。 In order to achieve the above objectives, the technical solution of the present application is as follows.

第一の局面においては、本願の実施例は、以下のステップを備えるロボットアームの多負荷適応重力補償法を提供する。 In a first aspect, embodiments of the present application provide a multi-load adaptive gravity compensation method for a robot arm comprising the following steps.

ステップＳ１．１においては、ロボットアームの運動学モデルを構築する。 In step S1.1, a kinematic model of the robot arm is constructed.

ステップＳ１．２においては、動力学モデルの重力項を再構築する。 In step S1.2, the gravitational term of the dynamics model is reconstructed.

ステップＳ１．３においては、無負荷静的位置サンプリングをする。 In step S1.3, no-load static position sampling is performed.

ステップＳ１．４においては、各ツールを取り付けた後にそれぞれ静的位置サンプリングをする。 In step S1.4, static position sampling is performed after each tool is installed.

ステップＳ１．５においては、各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算する。 In step S1.5, the parameter values awaiting calibration for each tool are calculated respectively.

ステップＳ１．６においては、各ツールの質量及び質量中心をそれぞれ計算する。 In step S1.6, the mass and center of mass of each tool are calculated respectively.

ステップＳ２．１においては、現在、取り付けられているツールによりフランジに加えられる力を計算する。 In step S2.1, the force exerted on the flange by the currently installed tool is calculated.

ステップＳ２．２においては、ツールの重力を補償する。 In step S2.2, the gravity of the tool is compensated.

１つの可能な実現方法では、ステップＳ１．１において、標準Ｄ－Ｈ法を用いてロボットアームの関節座標系を構築する。 In one possible implementation, in step S1.1, the standard DH method is used to construct the joint coordinate system of the robot arm.

１つの可能な実現方法では、ステップＳ１．３において、ロボットアームが無負荷の状況で、作業空間内の任意の非特異位置に移動し、関節の位置及びモーメントの読み取り値をサンプリングする。 In one possible implementation, in step S1.3, the robot arm is moved in an unloaded situation to any non-singular position in the workspace and samples joint position and moment readings.

１つの可能な実現方法では、ステップＳ１．４において、各ツールが何回かに分けてロボットアームの末端に取り付けられ、ステップＳ１．３を繰り返して静的位置サンプリングを行う。 In one possible implementation, in step S1.4 each tool is attached in batches to the end of the robot arm and step S1.3 is repeated to perform static position sampling.

１つの可能な実現方法では、ステップＳ１．４において、各ツールについて、当該ツールをロボットアームの末端に取り付けた後、当該ツールのサイズに基づいて当該ツールに対応するロボットアームの現在の有効な作業空間を決定し、有効な作業空間に基づいてステップＳ１．３を繰り返して当該ツールに対して静的位置サンプリングを行う。 In one possible implementation, in step S1.4, for each tool, after attaching the tool to the end of the robot arm, the current available work of the robot arm corresponding to the tool is calculated based on the size of the tool. Determine the space and repeat step S1.3 based on the available working space to perform static position sampling for the tool.

１つの可能な実現方法では、ステップＳ１．５において、ステップＳ１．３及びステップＳ１．４で得られたサンプリングデータを、ツールに基づいてグループ化し、順にステップＳ１．２の重力項に代入する。 In one possible implementation, in step S1.5, the sampling data obtained in steps S1.3 and S1.4 are grouped based on the tool and substituted in turn into the gravity term of step S1.2.

１つの可能な実現方法では、ステップＳ１．６において、各ツールについて、当該ツール携帯時にキャリブレーションされたパラメータ値と無負荷でキャリブレーションされたパラメータ値を比較し、比較結果および当該ツールの質量、当該ツールの質量中心、ロボットアームのエンドセグメントアームの質量及びエンドセグメントアームの質量中心の関連関係に基づき、当該ツールの質量及び質量中心を計算する。 In one possible implementation, in step S1.6, for each tool, the parameter values calibrated when the tool is carried and the parameter values calibrated without load are compared, the comparison result and the mass of the tool, Based on the relationship between the center of mass of the tool, the mass of the end segment arm of the robot arm, and the center of mass of the end segment arm, the mass and center of mass of the tool are calculated.

第二の局面においては、本願の実施例は、モデル構築モジュール、重力再構築モジュール、位置サンプリングモジュール、位置サンプリングモジュール、パラメータキャリブレーションモジュール、質量パラメータ計算モジュール、外力計算モジュール及び重力補償モジュールを備えるロボットアームの多負荷適応重力補償装置を提供する。 In a second aspect, embodiments of the present application provide a robot comprising a model building module, a gravity reconstruction module, a position sampling module, a position sampling module, a parameter calibration module, a mass parameter calculation module, an external force calculation module, and a gravity compensation module. A multi-load adaptive gravity compensator for the arm is provided.

モデル構築モジュールは、ロボットアームの運動学モデルを構築するように配置されている。 A model building module is arranged to build a kinematic model of the robot arm.

重力再構築モジュールは、動力学モデルの重力項を再構築するように配置されている。 A gravity reconstruction module is arranged to reconstruct the gravity term of the dynamics model.

位置サンプリングモジュールは、無負荷静的位置サンプリングをするように配置されている。 The position sampling module is arranged for no-load static position sampling.

位置サンプリングモジュールは、さらに、各ツールを取り付けた後にそれぞれ静的位置サンプリングをするように配置されている。 The position sampling module is further arranged to perform each static position sampling after mounting each tool.

パラメータキャリブレーションモジュールは、各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算するように配置されている。 A parameter calibration module is arranged to calculate respective parameter values awaiting calibration for each tool.

質量パラメータ計算モジュールは、各ツールの質量及び質量中心をそれぞれ計算するように配置されている。 A mass parameter calculation module is arranged to calculate the mass and center of mass of each tool respectively.

外力計算モジュールは、現在、取り付けられているツールによりフランジに加えられる力を計算するように配置されている。 An external force calculation module is now arranged to calculate the force exerted on the flange by the attached tool.

重力補償モジュールは、ツールの重力を補償するように配置されている。 A gravity compensation module is arranged to compensate for the gravity of the tool.

１つの可能な実現方法では、前記モデル構築モジュールは、ロボットアームの運動学モデルを構築する過程において、標準Ｄ－Ｈ法を用いてロボットアームの関節座標系を構築する。 In one possible implementation, the model building module builds the joint coordinate system of the robot arm using the standard DH method in the process of building the kinematic model of the robot arm.

１つの可能な実現方法では、前記位置サンプリングモジュールは、ロボットアームが無負荷の状況で、作業空間内の任意の非特異位置に移動するように制御し、関節の位置及びモーメントの読み取り値をサンプリングする。 In one possible implementation, the position sampling module controls the robot arm to move to any non-singular position in the workspace under unloaded conditions and samples joint position and moment readings. do.

１つの可能な実現方法では、前記位置サンプリングモジュールは、各ツールをロボットアームの末端に何回かに分けて取り付けた後、各ツールについて、当該ツールのサイズに基づいて当該ツールに対応するロボットアームの現在の有効な作業空間を決定し、有効な作業空間に基づき、ロボットアームが有効な作業空間内の任意の非特異位置に移動するように繰り返して制御し、関節の位置及びモーメントの読み取り値をサンプリングする。 In one possible implementation, the position sampling module attaches each tool to the end of a robot arm in batches, and then, for each tool, selects the robot arm corresponding to that tool based on its size. Determine the current effective workspace of , and based on the effective workspace, iteratively control the robot arm to move to any non-singular position within the effective workspace, and read the joint positions and moments to sample.

１つの可能な実現方法では、前記パラメータキャリブレーションモジュールは、前記位置サンプリングモジュールにより得られたサンプリングデータを、ツールに基づいてグループ化し、順に前記重力再構築モジュールにより得られた重力項に代入して計算することで、各ツールのキャリブレーション待ちのパラメータ値を取得する。 In one possible implementation, the parameter calibration module groups the sampled data obtained by the position sampling module based on tools and in turn substitutes the gravity term obtained by the gravity reconstruction module. By calculating, the parameter value waiting for calibration of each tool is obtained.

１つの可能な実現方法では、前記質量パラメータ計算モジュールは、各ツールの質量及び質量中心を計算する過程において、各ツールについて、当該ツール携帯時にキャリブレーションされたパラメータ値と無負荷でキャリブレーションされたパラメータ値を比較し、比較結果および当該ツールの質量、当該ツールの質量中心、ロボットアームのエンドセグメントアームの質量及びエンドセグメントアームの質量中心の関連関係に基づき、当該ツールの質量及び質量中心を計算する。 In one possible implementation, the mass parameter calculation module, in the process of calculating the mass and center of mass of each tool, for each tool is calibrated with parameter values calibrated when the tool is carried and without load. Comparing the parameter values and calculating the mass and center of mass of the tool based on the comparison result and the relationship between the mass of the tool, the center of mass of the tool, the mass of the end segment arm of the robot arm and the center of mass of the end segment arm do.

第三の局面においては、本願の実施例は、メモリ及びプロセッサを含むロボットアームの制御デバイスを提供する。前記メモリは、前記プロセッサで実行することが可能なコンピュータプログラムを記憶しており、前記プロセッサが前記コンピュータプログラムを実行すると、前記ロボットアームの多負荷適応重力補償法を実現する。 In a third aspect, embodiments of the present application provide a robotic arm control device including a memory and a processor. The memory stores a computer program executable by the processor to implement a multi-load adaptive gravity compensation method for the robot arm when the processor executes the computer program.

第四の局面においては、本願の実施例は、コンピュータプログラムが記憶された読み取り可能な記憶媒体を提供する。前記コンピュータプログラムがプロセッサに実行されると、前記ロボットアームの多負荷適応重力補償法が実行される。 In a fourth aspect, embodiments of the present application provide a readable storage medium storing a computer program. When the computer program is executed by the processor, the multi-load adaptive gravity compensation method of the robot arm is performed.

本願の実施例の有益な効果の１つとしては、Ｄ－Ｈ法によりロボットアームの関節座標系を構築し、関節座標系に基づいて各セグメントのロボットアームの質量中心位置に対して座標系を構築することが含まれている。元の重力項においては、関節位置に関連する項と質量中心に関連する項を分割し、分割過程においては、キャリブレーション待ちのパラメータを適切に組み合わせ、分割後の項を２つの行列に入れ、それらの乗算が、元の重力項を依然として満たせるようにする。次に、無負荷状態のロボットアームの静的位置をサンプリングし、その後に、各ツールをロボットアームの末端に取り付け、再度それぞれ静的位置をサンプリングする。サンプリングデータを、ツールに基づいてグループ化し、重力項に代入し、ＳＶＤ分解を用いて組み合わせ後のパラメータの値を取得し、最後に、組み合わせオブジェクトパラメータ分離の方法を用いて組み合わせ後のパラメータからツールの質量及び質量中心を抽出する。無負荷でキャリブレーションされたパラメータとリアルタイムの関節位置のフィードバックに基づき、現在取り付けられているエンドツールによりフランジに加えられる力の大きさを計算することができる。フランジでの外力を測定することにより、システムは、現在フランジにどのツールが取り付けられているかを知ることができるため、キャリブレーションされたパラメータ値を直接に用いてツールの重力を補償した後の外力測定を行うことができ、又は、得られた質量及び質量中心をロボットアームの配置に応用することができる。当該方法は、ツールパラメータを事前に計算して取得することにより、実際応用時の操作手順を簡素化し、協調作業の流暢さを大幅に高めることができる。また、関節位置及びモーメントセンサーを用いてツールに対してパラメータをキャリブレーションすることにより、キャリブレーションされたツールパラメータは、ロボットアームの運動学及び動力学の特徴をより適合し、ゼロフォース制御の性能を改善することができる。 As one of the beneficial effects of the embodiments of the present application, a joint coordinate system of the robot arm is constructed by the DH method, and based on the joint coordinate system, a coordinate system is created with respect to the position of the center of mass of the robot arm for each segment. It includes building. In the original gravity term, the term related to the joint position and the term related to the center of mass are split, in the splitting process, the parameters waiting for calibration are appropriately combined, and the split terms are put into two matrices, Their multiplication allows the original gravitational term to still be satisfied. The static position of the unloaded robot arm is then sampled, after which each tool is attached to the end of the robot arm and each static position is sampled again. The sampling data are grouped based on the tool, imputed to the gravity term, using SVD decomposition to obtain the values of the combined parameters, and finally using the method of combined object parameter separation to extract the combined parameters from the tool. Extract the mass and center of mass of . Based on no-load calibrated parameters and real-time joint position feedback, the magnitude of the force exerted by the currently installed end tool on the flange can be calculated. By measuring the external force at the flange, the system can know which tool is currently mounted on the flange, so the external force after compensating for the gravity of the tool using calibrated parameter values directly Measurements can be taken or the resulting mass and center of mass can be applied to the positioning of the robot arm. By pre-calculating and acquiring the tool parameters, the method can simplify the operating procedure in practical application and greatly enhance the fluency of collaborative work. In addition, by calibrating the parameters to the tool using joint position and moment sensors, the calibrated tool parameters better match the kinematics and dynamics characteristics of the robot arm, resulting in zero-force control performance. can be improved.

本願の実施例によるロボットアームの多負荷適応重力補償法のフローチャートである。FIG. 4 is a flowchart of a multi-load adaptive gravity compensation method for a robot arm according to embodiments of the present application; FIG.本願の実施例によるロボットアームの多負荷適応重力補償法のツール及びロボットアームの末端の模式図である。FIG. 3 is a schematic diagram of a tool and a distal end of a robot arm for a multi-load adaptive gravity compensation method for a robot arm according to embodiments of the present application;本願の実施例によるロボットアームの多負荷適応重力補償法のロボットアームの複数のツールの取り付けの模式図である。FIG. 4 is a schematic diagram of mounting multiple tools on a robot arm for a multi-load adaptive gravity compensation method for a robot arm according to an embodiment of the present application;本願の実施例によるロボットアームの多負荷適応重力補償装置の構成模式図である。1 is a structural schematic diagram of a multi-load adaptive gravity compensation device for a robot arm according to an embodiment of the present application; FIG.

以下、具体的な実施例及び図面を参照しながら、本願を更に述べ、説明する。 The present application will now be further described and explained with reference to specific examples and drawings.

図１～図３に示すように、完全なロボットアームの動力学方程式には、慣性項、遠心力及びコリオリ力項、重力項と摩擦力項が含まれている。なお、慣性項は関節加速度に関係し、遠心力及びコリオリ力項は関節速度に関係し、摩擦力項も関節速度に関係するが、関節加速度及び関節速度は、ロボットアームが静的状態である時に何れもゼロであるので、静的位置に対する研究は、重力項だけに対して展開しても良い。重力項を含む動力学方程式を構築するには、ロボットアームの関節位置及びモーメントを入力とする必要があるので、ロボットアームのハードウェアに対してある程度の要求がある。ＫＵＫＡ ＬＢＲ Ｍｅｄ ７ Ｒ８００を例とすれば、当該ロボットアームは、７軸協調型ロボットアームであり、各関節には、高精度の位置センサー及びモーメントセンサーが装備されており、本願におけるロボットアームのハードウェアに対する配置要求を満たしている。ＫＵＫＡ ＬＢＲ Ｍｅｄ ７ Ｒ８００の７軸協調型ロボットアームを例として挙げて実際の操作を説明する。 As shown in FIGS. 1-3, the complete robotic arm dynamics equation includes inertia terms, centrifugal and Coriolis force terms, gravitational terms and frictional force terms. The inertia term relates to joint acceleration, the centrifugal force and Coriolis force terms relate to joint velocity, and the friction force term also relates to joint velocity. Sometimes both are zero, so the study for static positions may be expanded for the gravitational term only. Since the joint positions and moments of the robot arm must be input to construct the dynamics equation including the gravitational term, there are certain demands on the hardware of the robot arm. Taking the KUKA LBR Med 7 R800 as an example, the robot arm is a 7-axis cooperative robot arm, each joint is equipped with a high-precision position sensor and a moment sensor. Satisfies placement requirements for hardware. The actual operation will be explained using the KUKA LBR Med 7 R800 7-axis coordinated robot arm as an example.

ステップＳ１．１においては、ロボットアームの運動学モデルを構築する。 In step S1.1, a kinematic model of the robot arm is constructed.

本実施例の１つの可能な実現方法においては、ロボットアームの運動学モデルを構築する際、ロボットアームの関節座標系の構築は、古典的なＤ－Ｈ法（Ａ Ｋｉｎｅｍａｔｉｃ Ｎｏｔａｔｉｏｎ ｆｏｒ Ｌｏｗｅｒ－Ｐａｉｒ Ｍｅｃｈａｎｉｓｍｓ Ｂａｓｅｄ ｏｎ Ｍａｔｒｉｃｅｓ， Ｊ．Ｄｅｎａｖｉｔ，Ｒ．Ｓ．Ｈａｒｔｅｎｂｅｒｇ）を採用する。ＫＵＫＡ ＬＢＲ Ｍｅｄ ７ Ｒ８００については、そのＤ－Ｈパラメータ表は、次の通りである。 In one possible implementation of this embodiment, when constructing the kinematic model of the robot arm, the construction of the joint coordinate system of the robot arm is based on the classical DH method (A Kinematic Notation for Lower-Pair Mechanisms). Based on Matrices, J. Denavit, RS Hartenberg). For the KUKA LBR Med 7 R800, the DH parameter table is as follows.

なお、α_ｉは、コネクティングロッドの回転角度を表し、ａ_ｉは、コネクティングロッドの長さを表し、ｄ_ｉは、コネクティングロッドのオフセット距離を表し、θ_ｉは、関節角度を表す。関節座標系が構築された後、一定の規則で各セグメントのロケットアームの質量中心位置に対して質量中心座標系を構築する。質量中心座標系を構築する際には、座標系の原点が各セグメントのアームの質量中心に位置することを保証すれば良く、回転角度は、ｉ＋１番目の関節の回転角度と一致しても良い。

Note that α_i represents the rotation angle of the connecting rod, a_i represents the length of the connecting rod, d_i represents the offset distance of the connecting rod, and θ_i represents the joint angle. After the joint coordinate system is constructed, a center-of-mass coordinate system is constructed for the center-of-mass position of the rocket arm of each segment according to a certain rule. When constructing the center-of-mass coordinate system, it is sufficient to ensure that the origin of the coordinate system is positioned at the center of mass of the arm of each segment, and the rotation angle may coincide with the rotation angle of the (i+1)th joint. .

ステップＳ１．２においては、動力学モデルの重力項を再構築する。 In step S1.2, the gravitational term of the dynamics model is reconstructed.

本実施例においては、静的状態のロケットアームは、重力項がロケットアームの関節モーメントに等しく、式は、次のように表される。

式から分かるように、重力項は、関節角度θ_ｉ、質量ｍ_ｉ及び質量中心ｃ_ｉに関係する。なお、質量ｍ_ｉ及び質量中心ｃ_ｉは、ツールのキャリブレーションに直接関係し、抽出する必要があるため、重力項Ｇは、次のように分割する必要がある。

簡単に分かるように、質量ｍ_ｉ及び質量中心ｃ_ｉは、重力項において結合されており、別々に分離できない。よって、キャリブレーション待ちのパラメータの行列Ａを作成するには、式を満たしながら、パラメータを合理的に組み合わせ、キャリブレーションされたパラメータを組み合わせのｍ_ｉ及びｃ_ｉにする必要がある。パラメータを組み合わせる際には、Ａのパラメータの数をできるだけ減らし、これにより、線形方程式系の解が局所最適に陥らないようにし、より良いキャリブレーション効果を得ることができる。In this example, the static rocket arm has a gravitational term equal to the joint moment of the rocket arm, and the equation is expressed as:

As can be seen from the equation, the gravitational term is related to joint angles θ_i , masses m_i and centers of mass c_i . Note that the mass m_i and the center of mass c_i are directly related to the calibration of the tool and need to be extracted, so the gravitational term G needs to be split as follows:

As can be easily seen, the masses m_i and the centers of mass c_i are coupled in the gravitational term and cannot be separated separately. Thus, to create the matrix A of parameters awaiting calibration, it is necessary to rationally combine the parameters while satisfying the equations and make the calibrated parameters mi and_{c iof} the combination. When combining parameters, the number of parameters in A is reduced as much as possible, so that the solution of the system of linear equations does not fall into local optima, and a better calibration effect can be obtained.

ステップＳ１．３においては、無負荷静的位置サンプリングをする。 In step S1.3, no-load static position sampling is performed.

本実施例においては、ロケットアームが無負荷状態であることを確認し、ロケットアームを作業空間の非特異位置に移動させ、各関節の静止後に関節の位置及びモーメントを収集する。サンプリングポイントが作業空間全体にできるだけ広がるように、サンプリングのステップを繰り返す。サンプリング過程においては、僅かのサンプリングポイントが特異位置に非常に接近する場合があるが、特異位置が原因で、ロケットアームの自由度の不足により、モーメントのフィードバックが不正確であることを招くので、これらのサンプリングポイントがサンプリングセットから除去されるべきである。 In this example, the rocket arm is verified to be unloaded, the rocket arm is moved to a non-singular position in the workspace, and the joint positions and moments are collected after each joint rests. The sampling step is repeated so that the sampling points are spread as far as possible over the working space. In the sampling process, a few sampling points may be very close to the singular position, but due to the singular position, the lack of freedom of the rocket arm will lead to inaccurate feedback of the moment. These sampling points should be removed from the sampling set.

図３に示すように、ステップＳ１．４においては、各ツールを取り付けた後にそれぞれ静的位置サンプリングをする。 As shown in FIG. 3, in step S1.4, static position sampling is performed after each tool is installed.

本実施例においては、順に各ツールをロケットアームの末端に取り付け、ステップＳ１．３を繰り返して各ツールごとに対して１ラウンドのモーションサンプリングを実行し、特異位置を除去した後のデータセットを保存する。ツール自体が一定のサイズを持っているため、ロボットアームの運動範囲に対してある程度の制限を与え、運動過程においては、ロボットアーム本体又は周囲の障害物と衝突する事故が発生する可能性があるので、ツールとロボットアームの静的位置をサンプリングする過程においては、有効的な作業空間を再確認し、合理的な運動軌跡を設計し、サンプリングポイントをできるだけ作業空間に広がりながら、衝突の発生を防止する必要がある。よって、本実施例の１つ可能な実現方法では、各ツールについて、当該ツールがロボットアームの末端に取り付けられた後、当該ツールのサイズに基づいてロボットアームの現在の当該ツールに対応する有効的な作業空間を決定し、有効的な作業空間に基づいてステップＳ１．３を繰り返して当該ツールに対して静的位置サンプリングを行う。 In this example, each tool is attached to the end of the rocket arm in turn, step S1.3 is repeated to perform one round of motion sampling for each tool, and save the data set after singular positions are removed. do. Since the tool itself has a certain size, it limits the range of movement of the robot arm to some extent, and during the movement process, there is a possibility of collision with the robot arm body or surrounding obstacles. Therefore, in the process of sampling the static positions of the tool and robot arm, reconfirm the effective working space, design a rational motion trajectory, spread the sampling points as much as possible in the working space, and prevent the occurrence of collisions. must be prevented. Thus, in one possible implementation of this embodiment, for each tool, after the tool is attached to the distal end of the robot arm, an effective number corresponding to the current tool on the robot arm is assigned based on the size of the tool. Then, based on the valid workspace, step S1.3 is repeated to perform static position sampling for the tool.

ステップＳ１．５においては、各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算する。 In step S1.5, the parameter values awaiting calibration for each tool are calculated respectively.

本実施例においては、ステップＳ１．５を実行するために、収集された全てのデータをツールに基づいてグループ化し、以下に示すように、各グループのデータをステップＳ１．２で得られた方程式系に積み上げる必要がある。

なお、データセット内の関節位置は、行列

に積み上げられるが、関節モーメントは、

に積み上げられる。積み上げ行列

が既に確認されたので、キャリブレーション待ちの行列Ａ（ｍ，ｃ）は、ＳＶＤ分解により線形方程式系を求める方法で得られる。行列

は、次の形式に分解することができる。

なお、左特異行列Ｕ及び右特異行列Ｖは、何れも直交行列であるため、過決定方程式

に対して、

とすると、新しい式ができる。
ΣＸ＝Ｂ
上述した式においては、Σは、対角行列であり、対角要素は、全て行列

の特異値σ_ｉであり、且つσ_１≧σ_２…≧σ_ｎ＞０であるため、Ｘを計算することができる。最後に、Ａ＝ＶＸに基づいて行列Ａの中のキャリブレーション待ちのパラメータを求めることができる。In the present example, to perform step S1.5, all collected data are grouped based on the tool, and each group of data is combined with the equation obtained in step S1.2, as shown below. It must be added to the system.

Note that the joint positions in the dataset are represented by the matrix

, but the joint moment is

stacked on. stacked matrix

has already been confirmed, the matrix A(m,c) waiting for calibration is obtained by the method of finding a system of linear equations by SVD decomposition. queue

can be decomposed into the form

Since both the left singular matrix U and the right singular matrix V are orthogonal matrices, the overdetermined equation

against

Then we have a new formula.
ΣX=B
In the above equation, Σ is a diagonal matrix, and the diagonal elements are all matrices

, and σ₁ ≧σ₂ . ._. ≧σ_n >0. Finally, we can determine the parameters awaiting calibration in matrix A based on A=VX.

ステップＳ１．６においては、各ツールの質量及び質量中心をそれぞれ計算する。 In step S1.6, the mass and center of mass of each tool are calculated respectively.

本実施例においては、ステップＳ１．５を実行するプロセスでは、剛体としてのツールをロボットアームの末端に設置してからパラメータのキャリブレーションを実行することが必要であるため、キャリブレーションされたパラメータにおける最後のセグメントの剛体の質量及び質量中心は、実際的には、ロボットアームの最後のセグメントとツールの結合後のパラメータである。よって、ツールの質量及び質量中心は、ツール付きのキャリブレーションされたパラメータと無負荷でキャリブレーションされたパラメータを比較し、ツールの質量及び質量中心とロボットアームのエンドセグメントアームの質量及び質量中心の関連関係を特徴付けるために用いられるマルチボディシステムの質量中心公式と組み合わせて決定することができる。故に、本実施例の１つの可能な実現方法では、各ツールについて、当該ツール携帯時にキャリブレーションされたパラメータ値と無負荷でキャリブレーションされたパラメータ値を比較し、比較結果および当該ツールの質量、当該ツールの質量中心、ロボットアームのエンドセグメントアームの質量及びエンドセグメントアームの質量中心の関連関係に基づき、当該ツールの質量及び質量中心を計算する。 In the present embodiment, the process of performing step S1.5 requires that the tool as a rigid body is installed at the end of the robot arm and then calibration of the parameters is performed. The mass and center of mass of the rigid body of the last segment are practically the post-coupling parameters of the last segment of the robot arm and the tool. The mass and center of mass of the tool are then compared with the calibrated parameters with the tool and the calibrated parameters with no load to determine the mass and center of mass of the tool and the end segment arm of the robot arm. It can be determined in combination with the multi-body system center-of-mass formula used to characterize the association relationship. Therefore, in one possible implementation of this embodiment, for each tool, the parameter values calibrated when the tool is carried and the parameter values calibrated without load are compared, and the comparison result and the mass of the tool, Based on the relationship between the center of mass of the tool, the mass of the end segment arm of the robot arm, and the center of mass of the end segment arm, the mass and center of mass of the tool are calculated.

なお、ＫＵＫＡ ＬＢＲ Ｍｅｄ ７ Ｒ８００の末端を例として挙げ、図２に示すように、ツールをクランプした後のロボットアームシステムに対応するマルチボディシステムの質量中心公式は、次の物理的な特性がある。

なお、ｃ_ｃは、ツールとエンドセグメントアームの結合後の質量中心であり、ｍ_ｔ及びｃ_ｔは、それぞれツールの質量及び質量中心であり、ｍ_７及びｃ_７は、それぞれエンドセグメントアームの質量と質量中心である。上述した式とキャリブレーションされたパラメータ式を連立すると、ツールの質量ｍ_ｔ及び質量中心ｃ_ｔを取得することができる。Taking the end of KUKA LBR Med 7 R800 as an example, as shown in Figure 2, the center of mass formula of the multi-body system corresponding to the robot arm system after clamping the tool has the following physical characteristics .

where_cc is the center of mass after coupling of the tool and the end segment arm, m_t and_ct are the mass and center of mass of the tool, respectively, and m₇ and c₇ are the mass of the end segment arm, respectively. and the center of mass. Combining the above equations with the calibrated parametric equations, the tool mass_mt and center of mass_ct can be obtained.

また、本願は、ツールのパラメータを自動的に選択し、重力補償に用いるシステムを設計した。当該システムは、関節モーメント及び位置センサーの出力をシステムの入力とし、システムの内部において現在ツールによりロボットアームの末端に加えられている力を計算することで、クランプしているツールのタイプを判断する。次に、Ｓ１で計算されたパラメータを適用して重力補償を完成する。以下、実施のステップを詳しく説明する。 The present application also designed a system that automatically selects tool parameters and uses it for gravity compensation. The system determines the type of tool being clamped by taking the output of the joint moment and position sensors as input to the system and calculating the force currently being applied by the tool to the end of the robot arm inside the system. . Then, the parameters calculated in S1 are applied to complete gravity compensation. The implementation steps are described in detail below.

ステップＳ２．１においては、現在取り付けられているツールによりフランジ（ロボットアームの末端）に加えられる力を計算する。 In step S2.1, the force exerted on the flange (end of robot arm) by the currently mounted tool is calculated.

本実施例においては、無負荷状態でキャリブレーションされたパラメータを用いることができ、現在位置におけるロボットアーム本体により生じる関節モーメントτ_{ｒｏｂｏｔ}を計算することができる。リアルタイムで測定された関節モーメントτ_{ｍｅａｓｕｒｅ}をτ_{ｒｏｂｏｔ}と減算をさせ、外力による関節モーメントτ_ｅｘｔを取得する。ヤコビ行列を用いて外力を関節空間から作業空間にマッピングし、作業空間におけるロボットアームの末端（フランジ）にかかる外力を計算することができる。In this embodiment, parameters calibrated in the unloaded state can be used to calculate the joint moment τ_robot produced by the robot arm body at the current position. The joint moment τ_measure measured in real time is subtracted from τ_robot to obtain the joint moment τ_ext due to the external force. The Jacobian matrix can be used to map the external forces from the joint space to the work space and calculate the external forces on the distal end (flange) of the robot arm in the work space.

ステップＳ２．２においては、ツールの重力を補償する。 In step S2.2, the gravity of the tool is compensated.

本実施例においては、ツール間の違いが外力の値に反映される。例えば、工具間の質量差が大きい場合、外力のＸＹＺ方向の値を、工具の識別基準として用いることができる。ツール間の質量差が小さく、質量中心の差が比較的大きい場合、外力のＡＢＣ方向のトルクを識別基準とすることが考えられる。質量及び質量中心の差が何れも大きくない場合、それらを１つのツールとして見なし、同じセットのキャリブレーションされたパラメータを適用すれば、同様に比較的に良い補償効果を得ることができる。ツールのパラメータを入力して重力補正を自動的に行うロボットアームの場合、ステップＳ１．６で計算されたツールの質量と質量中心を、ロボットアームの配置に直接的に書き込み、ロボットアームの組み込みプログラムでツールにかかる外力を計算させることができる。重力補償機能のないロボットアームの場合、ステップＳ１．５でキャリブレーションされたパラメータを直接的に用いて現在ツールにかかる外力を計算することができる。よって、ロボットアームが感じる外力は、ツールの重力を補償した後の外力であり、当該外力を入力とする制御戦略もツールの影響を省略し、即ち、ゼロフォース制御を実現することができる。 In this example, differences between tools are reflected in the external force values. For example, when the mass difference between tools is large, the values of the external force in the XYZ directions can be used as the tool identification criteria. When the difference in mass between tools is small and the difference in the center of mass is relatively large, it is conceivable to use the torque of the external force in the ABC directions as the discrimination criterion. If the difference between the mass and the center of mass are both not large, then if we regard them as one tool and apply the same set of calibrated parameters, we can get a relatively good compensation effect as well. For a robot arm that automatically performs gravity correction by inputting tool parameters, the mass and center of mass of the tool calculated in step S1.6 are directly written into the robot arm configuration, and the robot arm built-in program can be used to calculate the external force applied to the tool. For a robot arm without gravity compensation function, the parameters calibrated in step S1.5 can be directly used to calculate the external force currently applied to the tool. Therefore, the external force felt by the robot arm is the external force after compensating for the gravity of the tool, and the control strategy using the external force as input also omits the influence of the tool, that is, can realize zero force control.

また、図４に示すように、本願は、ロボットアームの多負荷適応重力補償装置１００を更に提供し、当該多負荷適応重力補償装置１００に含まれる様々な機能実現モジュールにより、上述したロボットアームの多負荷適応型重力補償方法を実行する。なお、前記多負荷適応重力補償装置１００は、モデル構築モジュール１１０、重力再構築モジュール１２０、位置サンプリングモジュール１３０、パラメータキャリブレーションモジュール１４０、質量パラメータ計算モジュール１５０、外力計算モジュール１６０及び重力補償モジュール１７０を備える。 In addition, as shown in FIG. 4 , the present application further provides a multi-load adaptivegravity compensation device 100 for a robot arm, and various function implementation modules included in the multi-load adaptivegravity compensation device 100 enable the above-mentioned robot arm to: Implement a multi-load adaptive gravity compensation method. The multi-load adaptivegravity compensation device 100 includes amodel building module 110, agravity reconstruction module 120, aposition sampling module 130, aparameter calibration module 140, a massparameter calculation module 150, an externalforce calculation module 160, and agravity compensation module 170. Prepare.

モデル構築モジュール１１０は、ロボットアームの運動学モデルを構築するように配置されている。 Amodel building module 110 is arranged to build a kinematic model of the robot arm.

重力再構築モジュール１２０は、動力学モデルの重力項を再構築するように配置されている。 Agravity reconstruction module 120 is arranged to reconstruct the gravity term of the dynamics model.

位置サンプリングモジュール１３０は、無負荷静的位置サンプリングをするように配置されている。 Theposition sampling module 130 is arranged for no-load static position sampling.

位置サンプリングモジュール１３０は、さらに、各ツールを取り付けた後にそれぞれ静的位置サンプリングをするように配置されている。Position sampling module 130 is further arranged to perform static position sampling after each tool is installed.

パラメータキャリブレーションモジュール１４０は、各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算するように配置されている。 Aparameter calibration module 140 is arranged to calculate respective parameter values awaiting calibration for each tool.

質量パラメータ計算モジュール１５０は、各ツールの質量及び質量中心をそれぞれ計算するように配置されている。 A massparameter calculation module 150 is arranged to calculate the mass and center of mass of each tool respectively.

外力計算モジュール１６０は、現在、取り付けられているツールによりフランジに加えられる力を計算するように配置されている。 The externalforce calculation module 160 is arranged to calculate the force exerted on the flange by the currently attached tool.

重力補償モジュール１７０は、ツールの重力を補償するように配置されている。 Agravity compensation module 170 is arranged to compensate for the gravity of the tool.

本実施例の１つの可能な実現方法においては、前記モデル構築モジュール１１０は、ロボットアームの運動学モデルを構築するプロセスにおいて、標準Ｄ－Ｈ法を用いてロボットアームの関節座標系を構築する。 In one possible implementation of this embodiment, themodel building module 110 builds the joint coordinate system of the robot arm using the standard DH method in the process of building the kinematic model of the robot arm.

本実施例の１つの可能な実現方法においては、前記位置サンプリングモジュール１３０は、ロボットアームが無負荷の状況で、ロボットアームを作業空間内の任意の非特異位置に移動するように制御し、関節の位置及びモーメントの読み取り値をサンプリングする。 In one possible implementation of this embodiment, theposition sampling module 130 controls the robot arm to move to any non-singular position in the workspace when the robot arm is unloaded, and the joint sample the position and moment readings of

本実施例の１つの可能な実現方法においては、前記位置サンプリングモジュール１３０は、各ツールを何回かに分けて（ツール毎の順次に）ロボットアームの末端に取り付けた後、各ツール毎につき、当該ツールのサイズに基づいて当該ツールに対応するロボットアームの現在の有効な作業空間を決定し、次に、有効な作業空間に基づき、ロボットアームが有効な作業空間内の任意の非特異位置に移動するように繰り返して制御し、関節の位置及びモーメントの読み取り値をサンプリングする。 In one possible implementation of this embodiment, theposition sampling module 130 attaches each tool in batches (sequentially tool by tool) to the end of a robot arm, then for each tool: Determine the current effective workspace of the robot arm corresponding to the tool based on the size of the tool, and then move the robot arm to any non-singular position within the effective workspace based on the effective workspace. It repeatedly controls the movement and samples the position and moment readings of the joint.

本実施例の１つの可能な実現方法においては、前記パラメータキャリブレーションモジュール１４０は、前記位置サンプリングモジュールにより得られたサンプリングデータを、ツールに基づいてグループ化し、順に前記重力再構築モジュールにより得られた重力項に代入して計算することで、各ツールのキャリブレーション待ちのパラメータ値を取得する。 In one possible implementation of this embodiment, theparameter calibration module 140 groups the sampling data obtained by the position sampling module based on tools, and in turn the data obtained by the gravity reconstruction module. By substituting it into the gravity term and calculating, the parameter value of each tool waiting for calibration is obtained.

なお、本願の実施例による多負荷適応重力補償装置１００は、その基本原理及び技術的な効果は、前記多負荷適応重力補償方法と同じである。説明を簡潔にするために、本実施例で言及されていない部分については、前記多負荷適応重力補償方法の説明内容を参照することができる。 The basic principle and technical effects of the multi-load adaptivegravity compensation device 100 according to the embodiment of the present application are the same as those of the multi-load adaptive gravity compensation method. For the sake of brevity, the description of the multi-load adaptive gravity compensation method can be referred to for the parts not mentioned in this embodiment.

また、本願は、ロボットアームの制御デバイスを更に提供し、当該制御デバイスは、メモリ及びプロセッサを備える。なお、前記メモリは、１つ又は複数のコンピュータプログラム製品を含むことができ、前記コンピュータプログラム製品は、揮発性メモリ及び／又は不揮発性メモリ等のような、様々なタイプの読み取り可能な記憶媒体を含むことができる。前記揮発性メモリは、例えば、ランダムアクセスメモリ及び／又はキャッシュメモリ等を含むことができる。前記不揮発性メモリは、例えば、リードオンリーメモリ、ハードディスク、フラッシュメモリ等を含むことができる。前記読み取り可能な記憶媒体には、１つ又は複数のコンピュータプログラムを記憶することができる。プロセッサは、前記コンピュータプログラムを実行することにより、前記ロボットアームの多負荷適応重力補償方法により表される機能及び／又は他の所望の機能を実現することができる。前記読み取り可能な記憶媒体には、さらに様々なアプリケーションプログラム及び様々なデータを記憶することができ、例えば、前記アプリケーションプログラムが使用及び／又は生成された様々なデータ等である。 Also, the present application further provides a control device for a robot arm, the control device comprising a memory and a processor. It should be noted that the memory may include one or more computer program products, which may include various types of readable storage media such as volatile memory and/or non-volatile memory. can contain. The volatile memory can include, for example, random access memory and/or cache memory. The non-volatile memory can include, for example, read-only memory, hard disk, flash memory, and the like. The readable storage medium can store one or more computer programs. A processor may implement the functions represented by the multi-load adaptive gravity compensation method for the robotic arm and/or other desired functions by executing the computer program. The readable storage medium can further store various application programs and various data, such as various data used and/or generated by the application programs.

前記プロセッサは、デジタル信号プロセッサ、フィールドプログラマブルゲートアレイ及びプログラマブルロジックアレイのうちの少なくとも１つのハードウェアの形で実現することができる。前記プロセッサは、中央処理ユニット、又は、データ処理機能及び／又は命令実行機能を有する他の形の処理ユニットの中の１つ又は複数の組み合わせであってもよく、所望の機能を実行するように、前記制御デバイスの中の他のアセンブリを制御することができる。前記プロセッサは、コンピュータプログラムが表す機能を実現するように、前記メモリに記憶されている当該コンピュータプログラムを相応的に実行することができる。 The processor may be implemented in hardware of at least one of a digital signal processor, a field programmable gate array and a programmable logic array. The processor may be a central processing unit or a combination of one or more among other forms of processing units having data processing capabilities and/or instruction execution capabilities, so as to perform the desired functions. , can control other assemblies in said control device. The processor is capable of correspondingly executing the computer program stored in the memory to perform the functions represented by the computer program.

本実施例の１つの可能な実現方法においては、前記ロボットアームの多負荷適応重力補償装置１００は、ソフトウェア又はファームウェアの形で前記制御デバイスのメモリに記憶してもよく、前記制御デバイスのプロセッサにより、前記メモリの中の前記多負荷適応重力補償装置１００に含まれるソフトウェア機能モジュールおよびコンピュータプログラム等を実行することにより、前記ロボットアームの多負荷適応重力補償方法に対応する機能を実現する。 In one possible implementation of this embodiment, the multi-loadadaptive gravity compensator 100 of the robot arm may be stored in the memory of the control device in the form of software or firmware, and is processed by the processor of the control device. , executing the software function modules and computer programs contained in the multi-load adaptivegravity compensation device 100 in the memory to implement the functions corresponding to the multi-load adaptive gravity compensation method of the robot arm.

上述したように、前記方案は、Ｄ－Ｈ法によりロボットアームの関節座標系を構築し、次に、関節座標系に基づいて各セグメントのロボットアームの質量中心位置に対して座標系構築を行う。元の重力項では、関節位置に関係する項目と質量中心に関係する項目を分割し、分割過程においてキャリブレーション待ちのパラメータを適切に組み合わせる必要があり、分割後の項目を２つの行列に入れ、それらの乗算が、元の重力項を依然として満たさせる。次に、無負荷状態でのロボットアームの静的位置をサンプリングし、その後に、各ツールをロボットアームの末端に取り付け、さらにそれぞれ静的位置サンプリングを行う。サンプリングされたデータをツールに基づいてグループ化した後、重力項に代入し、ＳＶＤ分解を用いて組み合わせ後のパラメータの値を取得し、最後に組み合わせオブジェクトパラメータ分離の方法を用いて組み合わせ後のパラメータからツールの質量及び質量中心を抽出することができる。無負荷でキャリブレーションされたパラメータとリアルタイムの関節位置のフィードバックに基づき、現在取り付けられているエンドツールによりフランジに加えられる力の大きさを計算することができる。フランジでの外力を測定することにより、システムは、現在フランジにどのツールが取り付けられているかを知ることができるため、キャリブレーションされたパラメータ値を直接に用いてツールの重力を補償した後の外力測定を行うことができ、又は、得られた質量及び質量中心をロボットアームの配置に応用することができる。当該方法は、ツールパラメータを事前に計算して取得することにより、実際応用時の操作手順を簡素化し、協調作業の流暢さを大幅に高めることができる。また、関節位置及びモーメントセンサーを用いてツールに対してパラメータをキャリブレーションすることにより、キャリブレーションされたツールパラメータは、ロボットアームの運動学及び動力学の特徴をより適合し、ゼロフォース制御の性能を改善することができる。 As mentioned above, the method constructs the joint coordinate system of the robot arm by the DH method, and then constructs the coordinate system for the center of mass position of the robot arm of each segment according to the joint coordinate system. . In the original gravity term, it is necessary to divide the item related to the joint position and the item related to the center of mass, and properly combine the parameters awaiting calibration in the process of dividing. Their multiplication still causes the original gravitational term to be satisfied. Next, the static position of the robot arm under no load is sampled, after which each tool is attached to the end of the robot arm and each static position is sampled. After grouping the sampled data based on the tool, we impute the gravity term, use SVD decomposition to obtain the values of the combined parameters, and finally use the method of combined object parameter separation to obtain the values of the combined parameters The mass and center of mass of the tool can be extracted from . Based on no-load calibrated parameters and real-time joint position feedback, the magnitude of the force exerted by the currently installed end tool on the flange can be calculated. By measuring the external force at the flange, the system can know which tool is currently mounted on the flange, so the external force after compensating for the gravity of the tool using calibrated parameter values directly Measurements can be taken or the resulting mass and center of mass can be applied to the positioning of the robot arm. By pre-calculating and acquiring the tool parameters, the method can simplify the operating procedure in practical application and greatly enhance the fluency of collaborative work. In addition, by calibrating the parameters to the tool using joint position and moment sensors, the calibrated tool parameters better match the kinematics and dynamics characteristics of the robot arm, resulting in zero-force control performance. can be improved.

最後に、上述した実施例は、本願の技術案を説明するものに過ぎず、本願の保護範囲を限定するものではないことに留意されたい。選択可能な実現方法を参照して本願を詳しく説明したが、当業者は、本願の技術案の精神及び範囲から逸脱しない限り、本願の技術案に対して補正又は等価置換を行うことができることが理解されたい。 Finally, it should be noted that the above-mentioned embodiments are only for describing the technical solutions of the present application, and are not intended to limit the protection scope of the present application. Although the present application has been described in detail with reference to alternative implementations, those skilled in the art may make amendments or equivalent substitutions to the technical solution of the present application without departing from the spirit and scope of the technical solution of the present application. be understood.

本願の実施例は、ロボットアームの多負荷適応重力補償法、装置、制御デバイス及び読み取り可能な記憶媒体を提供し、Ｄ－Ｈ法によりロボットアームの関節座標系を構築し、次に、関節座標系に基づいて各セグメントのロボットアームの質量中心に対して座標系構築を行う。元の重力項では、関節位置に関係する項目と質量中心に関係する項目を分割し、分割過程においてキャリブレーション待ちのパラメータを適切に組み合わせる必要があり、さらに分割後の項目を２つの行列に入れ、それらの乗算が、元の重力項を依然として満たさせる。次に、無負荷状態でのロボットアームの静的位置をサンプリングし、その後に、各ツールをロボットアームの末端に取り付け、さらにそれぞれ静的位置サンプリングを行う。サンプリングされたデータをツールに基づいてグループ化した後、重力項に代入し、ＳＶＤ分解を用いて組み合わせ後のパラメータの値を取得し、最後に組み合わせオブジェクトパラメータ分離の方法を用いて組み合わせ後のパラメータからツールの質量及び質量中心を抽出できる。無負荷でキャリブレーションされたパラメータとリアルタイムの関節位置のフィードバックに基づき、現在取り付けられているエンドツールによりフランジに加えられる力の大きさを計算することができる。算出されたフランジでの外力に基づいて、システムは、現在フランジにどのツールが取り付けられているかを知ることができるため、キャリブレーションされたパラメータ値を直接に用いてツールの重力を補償した後の外力測定を行うことができ、又は、得られた質量及び質量中心をロボットアームの配置に応用することができる。当該方法は、ツールパラメータを事前に計算して取得することにより、実際応用時の操作手順を簡素化し、協調作業の流暢さを大幅に高めることができる。また、関節位置及びモーメントセンサーを用いてツールに対してパラメータをキャリブレーションすることにより、キャリブレーションされたツールパラメータは、ロボットアームの運動学及び動力学の特徴をより適合し、ゼロフォース制御の性能を改善することができる。 Embodiments of the present application provide a multi-load adaptive gravity compensation method, apparatus, control device and readable storage medium for a robot arm, construct the joint coordinate system of the robot arm by the DH method, and then A coordinate system is constructed for the center of mass of the robot arm of each segment based on the system. In the original gravity term, it is necessary to split the terms related to the joint position and the terms related to the center of mass, appropriately combine the parameters awaiting calibration in the splitting process, and put the split terms into two matrices. , their multiplication still causes the original gravitational term to be satisfied. Next, the static position of the robot arm under no load is sampled, after which each tool is attached to the end of the robot arm and each static position is sampled. After grouping the sampled data based on the tool, we impute the gravity term, use SVD decomposition to obtain the values of the combined parameters, and finally use the method of combined object parameter separation to obtain the values of the combined parameters can extract the mass and center of mass of the tool from Based on no-load calibrated parameters and real-time joint position feedback, the magnitude of the force exerted by the currently installed end tool on the flange can be calculated. Based on the calculated external force on the flange, the system can know which tool is currently attached to the flange, so after compensating for tool gravity using the calibrated parameter values directly External force measurements can be taken or the resulting mass and center of mass can be applied to positioning the robot arm. By pre-calculating and acquiring the tool parameters, the method can simplify the operating procedure in practical application and greatly enhance the fluency of collaborative work. In addition, by calibrating the parameters to the tool using joint position and moment sensors, the calibrated tool parameters better match the kinematics and dynamics characteristics of the robot arm, resulting in zero-force control performance. can be improved.

Claims (15)

Translated fromJapanese

ロボットアームの運動学モデルを構築するステップＳ１．１と、
動力学モデルの重力項を再構築するステップＳ１．２と、
無負荷静的位置サンプリングをするステップＳ１．３と、
各ツールを取り付けた後にそれぞれ静的位置サンプリングをするステップＳ１．４と、
各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算するステップＳ１．５と、
各ツールの質量及び質量中心をそれぞれ計算するステップＳ１．６と、
現在取り付けられているツールによりフランジに加えられる力を計算するステップＳ２．１と、
ツールの重力を補償するステップＳ２．２と、を備えることを特徴とするロボットアームの多負荷適応重力補償法。step S1.1 of building a kinematic model of the robot arm;
a step S1.2 of reconstructing the gravitational term of the dynamics model;
step S1.3 of no-load static position sampling;
a step S1.4 of static position sampling after mounting each tool;
step S1.5 of calculating respective parameter values awaiting calibration for each tool;
a step S1.6 of calculating the mass and center of mass of each tool respectively;
a step S2.1 of calculating the force exerted on the flange by the currently installed tool;
compensating for tool gravity S2.2. ステップＳ１．１においては、標準Ｄ－Ｈ法を用いてロボットアームの関節座標系を構築することを特徴とする請求項１に記載のロボットアームの多負荷適応重力補償法。 The multi-load adaptive gravity compensation method for a robot arm according to claim 1, wherein in step S1.1, a standard DH method is used to construct the joint coordinate system of the robot arm. ステップＳ１．３においては、ロボットアームが無負荷の状況で、作業空間内の任意の非特異位置に移動し、関節の位置及びモーメントの読み取り値をサンプリングすることを特徴とする請求項１または２に記載のロボットアームの多負荷適応重力補償法。 Claim 1 or 2 characterized in that in step S1.3 the robot arm moves to an arbitrary non-singular position in the working space in an unloaded situation and samples joint position and moment readings. multi-load adaptive gravity compensation method for a robot arm as described in . ステップＳ１．４においては、各ツールをツール毎の順次にロボットアームの末端に取り付け、ステップＳ１．３を繰り返して静的位置サンプリングを行うことを特徴とする請求項３に記載のロボットアームの多負荷適応重力補償法。 4. The robot arm of claim 3, wherein in step S1.4, each tool is attached to the distal end of the robot arm in sequence for each tool, and step S1.3 is repeated to perform static position sampling. Load-adaptive gravity compensation method. ステップＳ１．４においては、各ツールについて、当該ツールをロボットアームの末端に取り付けた後、当該ツールのサイズに基づいて当該ツールに対応するロボットアームの現在の有効な作業空間を決定し、さらに有効な作業空間に基づき、ステップＳ１．３を繰り返して当該ツールに対して静的位置サンプリングを行うことを特徴とする請求項４に記載のロボットアームの多負荷適応重力補償法。 In step S1.4, for each tool, after attaching the tool to the end of the robot arm, determine the current effective workspace of the robot arm corresponding to the tool based on the size of the tool; 5. The multi-load adaptive gravity compensation method for a robot arm according to claim 4, wherein step S1.3 is repeated to perform static position sampling for the tool based on the working space. ステップＳ１．５においては、ステップＳ１．３及びステップＳ１．４で得られたサンプリングデータを、ツールに基づいてグループ化し、順にステップＳ１．２の重力項に代入することを特徴とする請求項１～５の何れか１つに記載のロボットアームの多負荷適応重力補償法。 In step S1.5, the sampling data obtained in steps S1.3 and S1.4 are grouped based on the tool, and sequentially substituted into the gravity term in step S1.2. 6. A multi-load adaptive gravity compensation method for a robot arm according to any one of -5. ステップＳ１．６においては、各ツールについて、当該ツール携帯時にキャリブレーションされたパラメータ値と無負荷でキャリブレーションされたパラメータ値を比較し、比較結果および当該ツールの質量、当該ツールの質量中心、ロボットアームのエンドセグメントアームの質量及びエンドセグメントアームの質量中心の関連関係に基づき、当該ツールの質量及び質量中心を計算することを特徴とする請求項１～６の何れか１つに記載のロボットアームの多負荷適応重力補償法。 In step S1.6, for each tool, the parameter values calibrated when carrying the tool and the parameter values calibrated under no load are compared, and the comparison result and the mass of the tool, the center of mass of the tool, the robot The robot arm according to any one of claims 1 to 6, wherein the mass and center of mass of the tool are calculated based on the relationship between the mass of the end segment arm of the arm and the center of mass of the end segment arm. multi-load adaptive gravity compensation method. ロボットアームの運動学モデルを構築するように配置されているモデル構築モジュールと、
動力学モデルの重力項を再構築するように配置されている重力再構築モジュールと、
無負荷静的位置サンプリングをするように配置されている位置サンプリングモジュールと、
ツールを取り付けた後に静的位置サンプリングをするようにさらに配置されている位置サンプリングモジュールと、
各ツールのキャリブレーション待ちのパラメータ値をそれぞれ計算するように配置されているパラメータキャリブレーションモジュールと、
各ツールの質量及び質量中心をそれぞれ計算するように配置されている質量パラメータ計算モジュールと、
現在取り付けられているツールによりフランジに加えられる力を計算するように配置されている外力計算モジュールと、
ツールの重力を補償するように配置されている重力補償モジュールと、を備えることを特徴とするロボットアームの多負荷適応重力補償装置。a model building module arranged to build a kinematic model of the robotic arm;
a gravity reconstruction module arranged to reconstruct the gravity term of the dynamics model;
a position sampling module arranged for no-load static position sampling;
a position sampling module further arranged to perform static position sampling after mounting the tool;
a parameter calibration module arranged to calculate respective calibration pending parameter values for each tool;
a mass parameter calculation module arranged to calculate the mass and center of mass of each tool respectively;
an external force calculation module arranged to calculate the force exerted on the flange by the currently installed tool;
a gravity compensation module arranged to compensate for tool gravity. 前記モデル構築モジュールは、ロボットアームの運動学モデルを構築する過程において、標準Ｄ－Ｈ法を用いてロボットアームの関節座標系を構築することを特徴とする請求項８に記載のロボットアームの多負荷適応重力補償装置。 10. The robot arm of claim 8, wherein the model construction module constructs a joint coordinate system of the robot arm using a standard DH method in the process of constructing the kinematic model of the robot arm. Load adaptive gravity compensator. 前記位置サンプリングモジュールは、ロボットアームが無負荷の状況で、作業空間内の任意の非特異位置に移動するように制御し、関節の位置及びモーメントの読み取り値をサンプリングすることを特徴とする請求項８又は９に記載のロボットアームの多負荷適応重力補償装置。 4. The position sampling module controls the robot arm to move to any non-singular position in the workspace under no-load conditions and samples joint position and moment readings. 10. A multi-load adaptive gravity compensation device for a robot arm according to 8 or 9. 前記位置サンプリングモジュールは、各ツールをツール毎の順次にロボットアームの末端に取り付けた後、各ツールについて、当該ツールのサイズに基づいて当該ツールに対応するロボットアームの現在の有効な作業空間を決定し、さらに有効な作業空間に基づき、ロボットアームが有効な作業空間内の任意の非特異位置に移動するように繰り返して制御し、関節の位置及びモーメントの読み取り値をサンプリングすることを特徴とする請求項１０に記載のロボットアームの多負荷適応重力補償装置。 The position sampling module attaches each tool to the end of the robot arm in sequence tool by tool, and then determines, for each tool, the current effective workspace of the robot arm corresponding to the tool based on the size of the tool. and further, based on the effective working space, repeatedly controlling the robot arm to move to any non-singular position in the effective working space, and sampling the readings of joint positions and moments. The multi-load adaptive gravity compensation device for a robot arm according to claim 10. 前記パラメータキャリブレーションモジュールは、前記位置サンプリングモジュールにより得られたサンプリングデータを、ツールに基づいてグループ化し、順に前記重力再構築モジュールにより得られた重力項に代入して計算することで、各ツールのキャリブレーション待ちのパラメータ値を取得することを特徴とする請求項８～１１の何れか１つに記載のロボットアームの多負荷適応重力補償装置。 The parameter calibration module groups the sampling data obtained by the position sampling module based on the tool, sequentially substitutes the gravity term obtained by the gravity reconstruction module for calculation, and calculates the The multi-load adaptive gravity compensation device for a robot arm according to any one of claims 8 to 11, wherein parameter values waiting for calibration are acquired. 前記質量パラメータ計算モジュールは、各ツールの質量及び質量中心を計算する過程において、各ツールについて、当該ツール携帯時にキャリブレーションされたパラメータ値と無負荷でキャリブレーションされたパラメータ値を比較し、比較結果および当該ツールの質量、当該ツールの質量中心、ロボットアームのエンドセグメントアームの質量及びエンドセグメントアームの質量中心の関連関係に基づき、当該ツールの質量及び質量中心を計算することを特徴とする請求項８～１２の何れか１つに記載のロボットアームの多負荷適応重力補償装置。 In the process of calculating the mass and center of mass of each tool, the mass parameter calculation module compares the parameter values calibrated when the tool is carried with the parameter values calibrated without load for each tool, and the comparison result is and calculating the mass and center of mass of the tool based on the relationship between the mass of the tool, the center of mass of the tool, the mass of the end segment arm of the robot arm, and the center of mass of the end segment arm 13. A multi-load adaptive gravity compensator for a robot arm according to any one of 8-12. メモリ及びプロセッサを含むロボットアームの制御デバイスであって、
前記メモリには、前記プロセッサで実行することが可能なコンピュータプログラムを記憶しており、
前記プロセッサが前記コンピュータプログラムを実行すると、請求項１～７の何れか１つに記載のロボットアームの多負荷適応重力補償法を実現することを特徴とするロボットアームの制御デバイス。A robotic arm control device comprising a memory and a processor,
The memory stores a computer program executable by the processor,
A control device for a robot arm, wherein the multi-load adaptive gravity compensation method for a robot arm according to any one of claims 1 to 7 is realized when the processor executes the computer program. コンピュータプログラムが記憶されている読み取り可能な記憶媒体であって、
前記コンピュータプログラムがプロセッサに実行されると、請求項１～７の何れか１つに記載のロボットアームの多負荷適応重力補償法を実行することを特徴とする読み取り可能な記憶媒体。A readable storage medium storing a computer program,
A readable storage medium characterized in that, when the computer program is executed by a processor, it performs the multi-load adaptive gravity compensation method for a robot arm according to any one of claims 1-7.

Images