JP6540810B2 - Hand force sense measurement device, hand force sense measurement method, and hand force sense measurement program - Google Patents

Description

  The present disclosure relates to a hand force sense measurement device, a hand force sense measurement method, and a hand force sense measurement program.

  There is known a method of calculating an external force acting on a robot hand (tip such as an arm tip) without using a force sensor. As such a method, there is a method of calculating a difference between a driving torque and a torque command value as an external torque, calculating a Jacobian matrix based on a joint angle, and calculating an external force from the Jacobian matrix and an external torque.

JP, 2010-105138, A

  However, in the conventional method, the external force is calculated without correcting an error (individual error) which may differ for each individual robot, so the calculation accuracy of the external force may deteriorate.

  Therefore, the present disclosure aims to provide a hand force sense measurement device, a hand force sense measurement method, and a hand force sense measurement program, in which calculation accuracy of an external force is good.

According to one aspect of the present disclosure, based on link parameters that geometrically represent the position and orientation relationship between links in an articulated robot including a link mechanism, and individual errors of the articulated robot that affect the link parameters. the saw including a processor for calculating the external force acting on the articulated robot hand, the individual error is derived based on the observation data of the position of the hand when the actually moved articulated robot, the The observation data includes the position of the hand of the articulated robot, the command value of the driving torque to the motor for applying torque to each joint of the articulated robot, and the joint angle of each joint of the articulated robot, The individual error is calculated by calculating a parameter Δ that minimizes a parameter of Equation 4 described later, and P is the value based on the observation data. A position, sigma, the indicates that accumulates multiple samples of the observed _{data, φ (θ 0, θ 1} , ..., θ n) is each joint angle θ _0, θ _1, .. , Θ _n represents the theoretical position of the hand, and each joint angle θ ₀ , θ ₁ ,..., Θ _n is the theoretical position φ (θ ₀ , A hand force measurement apparatus is provided in which θ ₁ ,..., θ _n ) is expressed based on the parameter Δ and the link parameter .

  According to the present disclosure, it is possible to obtain a hand force sense measurement device and the like in which the calculation accuracy of the external force is improved.

It is a figure which shows an example of an articulated robot provided with a link mechanism. It is a figure which shows the mechanical model of an articulated robot. It is a figure which shows the geometrical relationship of an articulated robot. It is explanatory drawing of the external force concerning an articulated robot. It is explanatory drawing of the drive mechanism in a joint. FIG. 2 is a view showing an example of a control block of the articulated robot 1; It is explanatory drawing of the calculation method of an individual difference | error. It is explanatory drawing of the acquisition method of observation data. It is explanatory drawing of the acquisition method of observation data. It is explanatory drawing of the acquisition method of observation data. It is a figure which shows an example of the hardware constitutions of the processing apparatus of a hand force sense measuring apparatus. It is a functional block diagram showing an example of a function of a processing device. It is a flowchart which shows an example of the process performed by a processing apparatus.

  Hereinafter, each example will be described in detail with reference to the attached drawings.

  FIG. 1 is a view showing an example of an articulated robot provided with a link mechanism. In the example illustrated in FIG. 1, the articulated robot 1 includes two joints 11 and 12 and three links 21 to 23 as link mechanisms. The articulated robot 1 includes the base 2 at one end of the link mechanism and the hand 30 at the other end (tip). The hand 32 is attached to the hand 30. The hand 32 has a function of gripping a work (not shown) or the like.

  FIG. 2 is a view showing a mechanical model of the articulated robot, FIG. 3 is a view showing a geometrical relationship of the articulated robot, and FIG. 4 is an explanatory view of an external force applied to the articulated robot.

FIG. 2 shows a mechanical model of the articulated robot 1 shown in FIG. Hereinafter, the numbers i = 0, 1, 2,..., N (hereinafter referred to as “joint numbers”) are given to the joints in the order from the base 2 to the hand 30. The base 2 has joint number i = 0. In the example shown in FIG. 3, the joint angle is indicated by θ ₁ and the link length is indicated by L ₁ with respect to the joint 11 (i = 1). Also, for the joint 12 (i = 2), the joint angle is indicated by θ ₂ and the link length is indicated by L ₂ . The joint angle represents the joint angle around the z-axis unless otherwise stated. In the coordinate axes, the link direction is the x-axis, the direction perpendicular to the paper is the z-axis, and the y-axis is an axis perpendicular to the x-axis and the z-axis. “X” in FIG. 3 is a parameter representing the position of the hand 32, and is represented by a coordinate system with the base 2 as the origin. As shown in FIG. 4, torques T ₁ and T ₂ are respectively applied to the joints via a drive mechanism described later, and an external force F is applied to the hand 32 at the time of operation. In FIG. 4, the dotted line shows the state of the articulated robot 1 when driven according to the command value when the external force F is not applied. By applying the external force F, it can be seen that the position or the like of the hand 32 is changed.

  FIG. 5 is an explanatory view of a drive mechanism in a joint, schematically showing the drive mechanism. A drive mechanism is provided for each joint. The drive mechanism 40 includes a motor 41 and a speed reducer 42. The rotational torque of the motor 41 is increased via the reduction gear 42 to cause relative rotation around the joint 12 between the link 22 and the link 23. The motor 41 is provided with an encoder 43 that measures the rotation angle of the motor 41.

  FIG. 6 is a view showing an example of a control block of the articulated robot 1. In FIG. 6, a portion surrounded by a dotted line represents a control target.

As shown in FIG. 6, first, the target position calculation unit 600 calculates the target position X _ref of the hand 32. Next, in the compliance calculation unit 601, the position correction amount Δx is calculated based on the target position X _ref and the external force F. The compliance calculation can be expressed by the following characteristic equation.

ここで、Δxは位置補正量、Ｍ（ハット付き）は仮想慣性係数、Ｄ（ハット付き）は仮想粘性係数、Ｋ（ハット付き）は仮想剛性係数を表す。Δxの上に付されたドットは、微分を表し、２つのドットは、２回微分を表す。コンプライアンス計算部６０１では、外力Ｆから特性方程式を満足するようにΔxが算出される。次いで、変換部６０２において、位置補正量Δxに基づいて、逆キネマティクス計算により関節角補正量Δθが算出される。次いで、位置・速度制御部６０３では、関節角補正量Δθと現在の関節角θ（エンコーダ４３の計測値）とに基づいて、各モータ４１に印加する電流値ｕを算出し、該電流値ｕに応じた電流を各モータ４１に印加する。

Here, Δx represents a position correction amount, M (with a hat) represents a virtual inertia coefficient, D (with a hat) represents a virtual viscosity coefficient, and K (with a hat) represents a virtual stiffness coefficient. Dots placed above Δx represent derivatives, and two dots represent derivatives twice. The compliance calculating unit 601 calculates Δx from the external force F so as to satisfy the characteristic equation. Next, in the conversion unit 602, the joint angle correction amount Δθ is calculated by inverse kinematics calculation based on the position correction amount Δx. Next, the position / speed controller 603 calculates the current value u applied to each motor 41 based on the joint angle correction amount Δθ and the current joint angle θ (measurement value of the encoder 43), and the current value u The current corresponding to the current is applied to each motor 41.

  Next, a hand force sense measurement method (apparatus) for calculating (measuring) the external force F applied to the hand 30 (or the hand 32, hereinafter the same) of the articulated robot 1 will be described. In the control shown in FIG. 6, the external force F can use a value calculated (measured) by the hand force sense measuring device described below.

  In the present embodiment, the hand force sense measurement device 70 is based on parameters that geometrically represent the position and posture relationship between links in the articulated robot 1 and individual errors of the articulated robot 1 that affect the parameters. The external force F applied to the hand 30 of the articulated robot 1 is calculated.

The parameters that geometrically represent the position and posture relationship between links in the articulated robot 1 are so-called link parameters. The link parameter represents the position-attitude relationship between two adjacent links by three translation components and three rotation components. The link parameters can be expressed by a matrix (see matrix M _i described later). However, the link parameter may be a DH (Denabit-Hartenberg) parameter.

  The individual errors include static errors and attitude dependent errors. The static error is an error that does not depend on the attitude of the articulated robot 1 and includes, for example, machining errors of parts, assembly errors, deformation due to temperature, and zero point errors of the encoder 43. The attitude dependent error includes backlash, link deflection error due to weight and load including rattling, and control model error.

  The individual error is an individual error that affects the link parameter, and is represented by, for example, three translation components and three rotation components as in the link parameter.

  The individual error can be derived based on data obtained by performing various tests for each individual of the articulated robot 1. The data is, for example, data relating to control information (command value of motor torque etc.) of the articulated robot 1 and the position (hand position) of the hand 30 at that time and the posture (joint angle etc.) of the articulated robot 1 . The position of the hand 30 can be measured by, for example, a three-dimensional position measurement device 50 (see FIG. 10) such as a camera (motion capture). As this type of sensor, a sensor for positioning the hand 30 may be used.

  FIG. 7 is an explanatory diagram of a method of calculating an individual error, and is a schematic flowchart showing a flow of a method of calculating an individual error. Here, a method of calculating an individual error for one certain articulated robot 1 will be described.

In step S700, the designer (or his / her assistant, the same applies hereinafter) acquires (collects) observation data by causing the articulated robot 1 to work. For example, in the unmounted state of the hand 32 as shown in FIG. 8A, the mounting state of the hand 32 as shown in FIG. 8B, and the gripping state of the work W as shown in FIG. Let For example, the hand 30 is approached and fixed from a plurality of directions such as up, down, left, right, front, and back to a grid point placed within the robot movement range. At this time, the designer (or his assistant) collects the following information as time-series data.
(1) Control information such as command value of motor torque (2) Measurement information including hand position and attitude (measured by motion capture and encoder 43)
In step S702, the designer uses a computer to calculate static errors based on observation data (eg, observation data obtained in the state of FIG. 8A) obtained in a stationary state in which no external force is applied to the articulated robot 1. Calculate each element of. Specifically, first, a link structure including an individual error is represented by a matrix M _i ′ that geometrically represents the individual error.

ここで、ｉは、上述した関節番号であり、行列Ｍ_ｉ'は関節番号毎に算出される。行列Ｍ_ｉは、上述したリンクパラメータを表す行列（隣接する2つのリンク間の変換行列）である。数２における行列Ｃ_ｉは、リンクパラメータの行列Ｍ_ｉを補正するための行列であり、以下、「補正行列Ｃ_ｉ」と称する。補正行列Ｃ_ｉは、数２に示すように、3個の並進成分のそれぞれに係る個体誤差Δｘ_ｉ,Δｙ_ｉ,Δｚ_ｉ（第１個体誤差の一例）、及び3個の回転成分のそれぞれに係る個体誤差δ_ｉｘ,δ_ｉｙ,δ_ｉｚ（第２個体誤差の一例）を含む。個体誤差Δｘ_ｉ,Δｙ_ｉ,Δｚ_ｉは、静的誤差のみを含む。他方、個体誤差δ_ｉｘは、静的誤差と、姿勢依存誤差とを含み、例えば以下のとおりである。

Here, i is the joint number described above, and the matrix M _i ′ is calculated for each joint number. The matrix M _i is a matrix (a transformation matrix between two adjacent links) representing the link parameters described above. The matrix C _i in Equation 2 is a matrix for correcting the matrix M _{i of} link parameters, and is hereinafter referred to as “correction matrix C _i ”. As shown in equation 2, the correction matrix C _i has individual errors Δx _i , Δy _i , Δz _i (an example of a first individual error) relating to each of the three translational components, and three rotation components. The individual errors δ _ix , δ _iy and δ _iz (an example of the second individual error) are included. The individual errors Δx _i , Δy _i and Δz _i include only static errors. On the other hand, the individual error δ _ix includes a static error and an attitude dependent error, and is, for example, as follows.

ここで、δ_０，ｉｘは静的誤差を表し、Ｄ_ｉｘは姿勢依存誤差（回転誤差として表現）を表す。個体誤差δ_ｉｙ、及びδ_ｉｚについても同様である。

Here, δ _{0, ix} represents a static error, and D _ix represents an attitude dependent error (represented as a rotation error). The same applies to the individual errors δ _iy and δ _iz .

  In this case, based on the observation data, the designer calculates Δ that minimizes the following Equation 4.

ここで、Ｐは、ハンド３２の計測位置であり、観測データに基づく。Σは、Ｐの全数分を累算することを表す。φ（θ_０，θ_１，...，θ_ｎ）は、各関節角度がθ_０，θ_１，...，θ_ｎであるときのハンド３２の理論位置である。φ（θ_０，θ_１，...，θ_ｎ）は、以下の通りである。尚、各関節角度がθ_０，θ_１，...，θ_ｎは、観測データに基づく。

Here, P is the measurement position of the hand 32, and is based on observation data. Σ represents accumulating the whole number of P. _{φ (θ 0, θ 1,} ..., θ n) , each joint angle is θ _0, θ _1, ..., the theoretical position of the hand 32 when a theta _n. φ (θ ₀ , θ ₁ ,..., θ _n ) is as follows. The joint angles θ ₀ , θ ₁ ,..., Θ _n are based on observation data.

ここで、行列Ｍ_ｉ'は、数２で示した通りである。但し、数５では、行列Ｍ_ｉ'は、静的誤差のみを含むものとする。即ち、Ｄ_ｉｘ＝０、Ｄ_ｉｙ＝０、及びＤ_ｉｚ＝０として、Δを算出（同定）する。Δは、以下のとおりである。

Here, the matrix M _i ′ is as shown in Equation 2. However, in Equation 5, the matrix M _i ′ includes only static errors. That is, Δ is calculated (identified) with D _ix = 0, D _iy = 0, and D _iz = 0. Δ is as follows.

また、数４において、Ｊはヤコビ行列であり、φ（数５）をΔで偏微分することで導出できる。

In Equation 4, J is a Jacobian matrix, which can be derived by partially differentiating φ (Equation 5) with Δ.

In step S704, the designer uses the computer to pose based on observation data obtained in a stationary state in which an external force is applied to the articulated robot 1 (for example, observation data obtained in the state of FIG. 8B and FIG. 8C). Calculate each element of dependency error. Specifically, the designer similarly calculates Δ, which minimizes the above equation 4, based on the observation data. In equation 5, the matrix M _i ′ includes static errors and attitude dependent errors as follows. At this time, the static errors Δx _i , Δy _i , Δz _i and δ _{0, ix} , δ _{0, i} y, δ _{0, iz} use the values identified in step S702.

上記の数４において、Δは、以下のとおりである。

In the above equation 4, Δ is as follows.

ここで、Ｄ_ｉｘ、Ｄ_ｉｙ、及びＤ_ｉｚは、以下のとおりである。

Here, _Dix , _Diy , and _Diz are as follows.

ここで、τ_ｉは、モータトルクの指令値である。この場合、上記の数４を最小とする弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚを求めることになる。尚、姿勢依存誤差Ｄ_ｉｘ、Ｄ_ｉｙ、及びＤ_ｉｚは、弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚが求まると、数８から分かるように、弾性係数ｋ_ｉｘ、ｋ_ｉｙ、及びｋ_ｉｚと、モータトルクの指令値τ_ｉとに基づいて算出できる。

Here, τ _i is a command value of motor torque. In this case, the elastic coefficients k _ix , k _iy and k _iz which minimize the above equation 4 will be obtained. Note that the posture dependent errors D _ix , D _iy , and D _iz are elastic coefficients k _ix , k _iy , and k _{iz as} can be seen from equation 8 when the elastic coefficients k _ix , k _iy , and k _iz are determined. It can be calculated based on the command value tau _i of the motor torque.

Thus, according to the calculation method of the individual error shown in FIG. 7, the individual error (static error and attitude dependent error) can be calculated based on the observation data. In the example shown in FIG. 7, the designer calculates the individual error (correction matrix C _i ) using a computer, but as described later, the processor (for example, the individual error calculator of the processor 100 described later) It is also possible to calculate 114).

  Next, a configuration example and an operation example of the hand force sense measuring device 70 will be described.

  The hand force sense measurement device 70 includes the processing device 100.

  FIG. 9 is a diagram showing an example of the hardware configuration of the processing apparatus 100. As shown in FIG.

  In the example illustrated in FIG. 9, the processing device 100 includes a control unit 101, a main storage unit 102, an auxiliary storage unit 103, a drive device 104, a network I / F unit 106, and an input unit 107.

  The control unit 101 is an arithmetic unit that executes a program stored in the main storage unit 102 or the auxiliary storage unit 103. The control unit 101 receives data from the input unit 107 or the storage device, processes it, and outputs it to the storage device or the like. Do.

  The main storage unit 102 is a read only memory (ROM) or a random access memory (RAM). The main storage unit 102 is a storage device for storing or temporarily storing programs and data such as an OS (Operating System) and application software which are basic software executed by the control unit 101.

  The auxiliary storage unit 103 is a hard disk drive (HDD) or the like, and is a storage device that stores data related to application software and the like.

  The drive device 104 reads a program from a recording medium 105, for example, a flexible disk, and installs the program in a storage device.

  The recording medium 105 stores a predetermined program. The program stored in the recording medium 105 is installed in the processing device 100 via the drive device 104. The installed predetermined program can be executed by the processing device 100.

  The network I / F unit 106 is an interface between the processing device 100 and a peripheral device having a communication function connected via a network established by a data transmission path such as a wired and / or wireless link.

  The input unit 107 includes a keyboard provided with cursor keys, numeric inputs, various function keys, and the like, a mouse, a touch pad, and the like.

  In the example shown in FIG. 9, various processes described below can be realized by causing the processing device 100 to execute a program. Alternatively, the program may be recorded on the recording medium 105, and the recording medium 105 on which the program is recorded may be read by the processing apparatus 100 to realize various processes described below. Note that various types of recording media can be used as the recording medium 105. For example, the recording medium 105 may be a recording medium for recording information optically, electrically or magnetically such as a CD (Compact Disc) -ROM, a flexible disc, a magneto-optical disc etc., an information such as a ROM, a flash memory etc. May be a semiconductor memory or the like for electrically recording. The recording medium 105 does not include a carrier wave.

  FIG. 10 is a functional block diagram showing an example of the function of the processing device 100. As shown in FIG. In FIG. 10, components (three-dimensional position measuring device 50 and the like) connected to the processing device 100 are also illustrated.

  The processing device 100 includes an observation data acquisition unit 90 as shown in FIG. The processing device 100 further includes a hand position measurement value acquisition unit 110, a joint angle acquisition unit 111, a motor torque acquisition unit 112, a link parameter calculation unit 113, an individual error calculation unit 114, a link parameter correction unit 115, and a Jacobian matrix calculation unit. 116 is included. The processing device 100 also includes a motion parameter calculator 117, a torque calculator 118, and an external force calculator 119. Each of the units 90 and 110 to 119 can be realized by the control unit 101 executing one or more programs stored in the main storage unit 102. The processing device 100 also includes an observation data storage unit 120, an individual error storage unit 121, and a motion parameter storage unit 122. The observation data storage unit 120, the individual error storage unit 121, and the motion parameter storage unit 122 can be realized by the auxiliary storage unit 103, for example.

  The observation data acquisition unit 90 acquires (collects) observation data in operation of the articulated robot 1 and stores the observation data in the observation data storage unit 120. For example, the observation data acquisition unit 90 detects the timing at which the articulated robot 1 has settled based on the information from the hand position measurement value acquisition unit 110, and acquires observation data at that timing. The observation data is as described above, and can be acquired from the hand position measurement value acquisition unit 110, the joint angle acquisition unit 111, and the motor torque acquisition unit 112.

  The hand position measurement value acquisition unit 110 acquires measurement values of the three-dimensional position of the hand 32 from the three-dimensional position measurement apparatus 50.

  The joint angle acquisition unit 111 acquires the detection value of the joint angle of each joint from each encoder 43.

  The motor torque acquisition unit 112 acquires from the motor drive unit 52 a command value (a command value of the motor torque) of the drive torque of the motor 41 related to each joint. The motor drive unit 52 may be realized by, for example, a motor controller (see FIG. 6).

The link parameter calculation unit 113 calculates link parameters based on the information from the joint angle acquisition unit 111. The link parameter calculation unit 113 calculates link parameters that do not include individual errors. That is, the link parameter calculation unit 113 calculates the matrix M _i . The matrix M _i is as described above.

The individual error calculation unit 114 calculates an individual error of the articulated robot 1 affecting the link parameter based on the information from the observation data storage unit 120. For example, the individual error calculation unit 114 calculates the correction matrix C _i . The method of calculating the correction matrix C _i is as described above. The individual error calculation unit 114 stores the calculated individual error in the individual error storage unit 121. The individual error calculation unit 114 may periodically recalculate (update) the individual error based on the latest observation data stored in the observation data storage unit 120 during operation. Alternatively, the individual error calculation unit 114 recalculates (updates) the individual error based on the latest observation data accumulated in the observation data storage unit 120 each time new observation data is acquired during actual operation. You may

The link parameter correction unit 115 corrects the link parameter calculated by the link parameter calculation unit 113 based on the information from the joint angle acquisition unit 111, the link parameter calculation unit 113, and the individual error storage unit 121. That is, the link parameter correction unit 115 calculates the matrix M _i ′. The method of calculating the matrix M _i ′ is as described above.

  The Jacobian matrix calculation unit 116 calculates the Jacobian matrix J1 based on the information from the joint angle acquisition unit 111 and the link parameter correction unit 115. The method of calculating the Jacobian matrix is as described above.

  The motion parameter calculation unit 117 calculates a motion parameter reflecting the individual error based on the individual error storage unit 121. That is, since the length of each link changes according to the individual error, the motion parameter calculation unit 117 calculates the motion parameter based on the individual error obtained from the individual error storage unit 121. The motion parameter is, for example, a parameter related to a kinetic equation of motion of the robot arm, and includes an inertia matrix, a non-linear term (centrifugal force, Coriolis force, etc.), and a gravity term (see Equation 10 below). The motion parameter calculation unit 117 stores the calculated motion parameter in the motion parameter storage unit 122. The movement parameter calculation unit 117 may periodically recalculate (update) the movement parameter based on the latest individual error stored in the individual error storage unit 121 during actual operation. Alternatively, each time the individual error in the individual error storage unit 121 is updated during actual operation, the individual error calculation unit 114 calculates the motion parameter based on the latest individual error stored in the individual error storage unit 121. It may be recalculated (updated).

  The torque calculation unit 118 calculates the drive torque of each motor 41 based on the motion parameter from the motion parameter storage unit 122 and the information from the joint angle acquisition unit 111. The driving torque can be calculated as follows based on the kinetic motion equation of the robot arm.

ここで、θは、以下のとおりである。
θ＝［θ_０，θ_１，...，θ_ｎ］
また、上に"〜"が付いたＭは、慣性行列を表し、上に"〜"が付いたＨは、非線形項を表し、上に"〜"が付いたＨは、重力項を表す。

Here, θ is as follows.
θ = [θ ₀ , θ ₁ ,..., θ _n ]
In addition, M with "-" at the top represents an inertia matrix, H with "-" at the top represents a non-linear term, and H with "-" at the top represents a gravity term.

The external force calculation unit 119 obtains the external force F applied to the hand 30 based on the information from the motor torque acquisition unit 112, the Jacobian matrix calculation unit 116, and the torque calculation unit 118. Specifically, first, the external force calculation unit 119 works outside based on the difference between the command value τ _i of the motor torque from the motor torque acquisition unit 112 and the calculated value τ _c from the torque calculation unit 118. The torque τ _e is calculated. That is, it is as follows.
τ _e = τ _i -τ _c
Here, tau _e, and tau _c, as with tau _i, respectively, a matrix of 1 × n containing torque values related to each joint. Then, based on the torque τ _e and the Jacobian matrix J1 obtained by the Jacobian matrix calculation unit 116, the external force calculation unit 119 obtains an external force F applied to the hand 30 as follows.

ここで、Ｊ１^Ｔは、ヤコビ行列Ｊ１の転置を表す。尚、数１１の式は、仮想仕事の原理に基づく。

Here, J1 ^T represents the transpose of the Jacobian J1. Equation 11 is based on the principle of virtual work.

According to the processing device 100 illustrated in FIG. 10, the individual error calculation unit 114 can calculate an individual error (a static error and a posture dependent error) based on observation data. In addition, the motion parameter calculation unit 117 can calculate the motion parameter in which the individual error is reflected based on the individual error. Then, since the external force calculation unit 119 calculates the external force F applied to the hand 30 based on the correction matrix C _i and the motion parameter, it is possible to calculate the highly accurate external force F in which the error component due to the individual error is reduced.

  FIG. 11 is a flowchart showing an example of processing executed by the processing device 100. The process shown in FIG. 11 is executed, for example, at predetermined intervals while the articulated robot 1 is in operation.

  In step S1100, the observation data acquisition unit 90 determines, based on the information from the hand position measurement value acquisition unit 110, whether or not the articulated robot 1 has settled. If it is determined that the articulated robot 1 has settled, the process proceeds to step S1102, and otherwise, the process proceeds to step S1104.

  In step S1102, the observation data acquisition unit 90 stores information obtained from the hand position measurement value acquisition unit 110, the joint angle acquisition unit 111, and the motor torque acquisition unit 112 in the observation data storage unit 120 as observation data.

  In step S1104, the individual error calculation unit 114 determines whether or not the individual error calculation condition is satisfied. Although the calculation condition of the individual error is arbitrary, it is satisfied, for example, when there is an update of the observation data of the observation data storage unit 120 when a predetermined time has passed since the previous calculation when there is an instruction from the user. Good. If the calculation condition of the individual error is satisfied, the process proceeds to step S1106. Otherwise, the process proceeds to step S1108.

  In step S1106, the individual error calculation unit 114 calculates an individual error based on the latest observation data stored in the observation data storage unit 120, and stores the calculated individual error in the individual error storage unit 121.

  In step S1108, the motion parameter calculation unit 117 determines whether the calculation condition of the motion parameter is satisfied. The calculation condition of the movement parameter is arbitrary, but is satisfied, for example, when there is an update of the individual error of the individual error storage unit 121 when a predetermined time has passed since the previous calculation when there is an instruction from the user Good. If the motion parameter calculation condition is satisfied, the process proceeds to step S1110. Otherwise, the process proceeds to step S1112. If the motion parameter calculation condition is satisfied when the individual error of the individual error storage unit 121 is updated, the process proceeds to step S1112 if the determination result in step S1104 is “NO”. Become. If the determination result of step S1104 is "YES", the process proceeds to step S1110 without passing through the determination process of step S1108.

  In step S1110, the motion parameter calculation unit 117 calculates a motion parameter based on the latest individual error stored in the individual error storage unit 121, and stores the calculated motion parameter in the motion parameter storage unit 122.

  In step S1112, the link parameter calculation unit 113 calculates link parameters based on the information from the joint angle acquisition unit 111.

In step S1114, the link parameter correction unit 115 calculates the matrix M _i ′ based on the information from the joint angle acquisition unit 111, the link parameter calculation unit 113, and the individual error storage unit 121.

In step S1116, the Jacobian matrix calculation unit 116 calculates the Jacobian matrix J1 based on the information from the joint angle acquisition unit 111 and the matrix M _i ′ obtained in step S1114. The calculation method of the Jacobian matrix J1 is as described above.

  In step S1118, the torque calculation unit 118 calculates the drive torque of each motor 41 based on the motion parameter from the motion parameter storage unit 122 and the information from the joint angle acquisition unit 111. The method of calculating the drive torque of each motor 41 is as described above.

  In step S1120, the external force F applied to the hand 30 is calculated based on the information from the motor torque acquisition unit 112, the Jacobian matrix J1 obtained in step S1116, and the driving torque of each motor 41 obtained in step S1118. The method of calculating the external force F is as described above.

According to the process shown in FIG. 11, during the actual operation of the articulated robot 1, the individual error calculation unit 114 calculates the correction matrix C _i according to the individual error (static error and posture dependent error) based on the observation data. It can be calculated. Thereby, the individual error can be updated while the articulated robot 1 is in operation. Then, since the external force calculation unit 119 calculates the external force F applied to the hand 30 based on the correction matrix C _i and the motion parameter updated during the actual operation of the articulated robot 1, the calculation accuracy of the external force F can be calculated It can be raised during.

  As mentioned above, although each Example was explained in full detail, it is not limited to a specific example, A various deformation | transformation and change are possible within the range described in the claim. In addition, it is also possible to combine all or a plurality of the components of the above-described embodiment.

For example, in the process shown in FIG. 11, the individual error storage unit 121 initially stores a static error (for example, see step S702 of the method shown in FIG. 7) of the individual errors, which is derived in advance. Good. Then, elastic coefficients k _ix , k _iy , and k _iz required for calculation of the posture dependent error may be derived and updated during actual operation of the articulated robot 1 (see, for example, step S704 of the method illustrated in FIG. 7). ).

1 Articulated robot
21 to 23 Link 30 hand 32 hand 40 drive mechanism 41 motor 70 hand force sense measurement device 90 observation data acquisition unit 100 processing device 110 hand position measurement value acquisition unit 111 joint angle acquisition unit 112 motor torque acquisition unit 113 link parameter calculation unit 114 Individual error calculation unit 115 Link parameter correction unit 116 Jacobian matrix calculation unit 117 Motion parameter calculation unit 118 Torque calculation unit 119 External force calculation unit 120 Observation data storage unit 121 Individual error storage unit 122 Motion parameter storage unit

Claims (6)

It acts on the hand of the articulated robot based on link parameters that geometrically represent the position and posture relationship between links in an articulated robot having a link mechanism, and individual errors of the articulated robot affecting the link parameters. a processing unit for calculating an external force to be seen including,
The individual error is derived based on observation data of the position of the hand when the articulated robot is actually moved,
The observation data includes the position of the hand of the articulated robot, a command value of driving torque to a motor for applying a torque to each joint of the articulated robot, and a joint angle of each joint of the articulated robot. ,
The individual error is calculated by calculating a parameter Δ that minimizes the following parameter 1

P is the position of the hand based on the observation data, Σ indicates that a plurality of samples of the observation data are accumulated, and φ (θ ₀ , θ ₁ ,..., Θ _n ) is each joint Represents the theoretical position of the hand when the angle is θ ₀ , θ ₁ ,..., Θ _n , and each joint angle θ ₀ , θ ₁ ,..., Θ _n is based on the observation data
The theoretical position φ (θ ₀ , θ ₁ ,..., Θ _n ) of the hand is represented based on the parameter Δ and the link parameter .   The hand force sense measurement device according to claim 1, wherein the individual error includes a first individual error relating to a translational error and a second individual error relating to a rotational error.   The hand force sense measurement device according to claim 2, wherein the second individual error includes a static error and a posture dependent error dependent on a posture of the articulated robot. The processing unit
An individual error calculation unit that calculates the individual error;
A storage unit that stores the individual error;
A link parameter correction unit that corrects the link parameter based on the individual error;
A motion parameter calculation unit that calculates a motion parameter based on the individual error;
A torque calculation unit that calculates a torque to be applied to each joint of the articulated robot based on the motion parameter;
An external force calculation unit that calculates the external force based on a command value of a drive torque to a motor that applies a torque to each joint of the articulated robot and the torque calculated by the torque calculation unit. The hand force sense measuring device according to any one of 1 to 3. Calculate link parameters that geometrically represent the position and orientation relationship between links in an articulated robot having a link mechanism,
Calculating individual errors of the articulated robot affecting the link parameters;
The external force acting on the hand of the articulated robot is calculated based on the calculated link parameter and the individual error.
Look at including it,
The individual error is derived based on observation data of the position of the hand when the articulated robot is actually moved,
The observation data includes the position of the hand of the articulated robot, a command value of driving torque to a motor for applying a torque to each joint of the articulated robot, and a joint angle of each joint of the articulated robot. ,
The individual error is calculated by calculating a parameter Δ that minimizes the following parameter 2

P is the position of the hand based on the observation data, Σ represents that a plurality of samples of the observation data are accumulated, and φ (θ ₀ , θ ₁ ,..., Θ _n ) is each joint angle θ _0, θ _{1, ...,} represent the theoretical position of the hand when a theta _n, each joint angle _{θ 0, θ 1, ...,} θ n is the observation data Based on
A computer-implemented method for measuring hand force , wherein a theoretical position φ (θ ₀ , θ ₁ ,..., Θ _n ) of the hand is represented based on the parameter Δ and the link parameter . Calculate link parameters that geometrically represent the position and orientation relationship between links in an articulated robot having a link mechanism,
Calculating individual errors of the articulated robot affecting the link parameters;
The external force acting on the hand of the articulated robot is calculated based on the calculated link parameter and the individual error.
Let the computer execute the process
The individual error is derived based on observation data of the position of the hand when the articulated robot is actually moved,
The observation data includes the position of the hand of the articulated robot, a command value of driving torque to a motor for applying a torque to each joint of the articulated robot, and a joint angle of each joint of the articulated robot. ,
The individual error is calculated by calculating a parameter Δ which minimizes the following Equation 3:

P is the position of the hand based on the observation data, Σ represents that a plurality of samples of the observation data are accumulated, and φ (θ ₀ , θ ₁ ,..., Θ _n ) is each joint angle θ _0, θ _{1, ...,} represent the theoretical position of the hand when a theta _n, each joint angle _{θ 0, θ 1, ...,} θ n is the observation data Based on
A hand force sense measurement program , wherein a theoretical position φ (θ ₀ , θ ₁ ,..., Θ _n ) of the hand is represented based on the parameter Δ and the link parameter .

Images