JP4988545B2

JP4988545B2 - External force detection method and detection apparatus

Info

Publication number: JP4988545B2
Application number: JP2007341556A
Authority: JP
Inventors: 俊明辻; 茂阿部; 裕良金子; 良輔羽生
Original assignee: Saitama University NUC
Current assignee: Saitama University NUC
Priority date: 2007-12-29
Filing date: 2007-12-29
Publication date: 2012-08-01
Anticipated expiration: 2027-12-29
Also published as: JP2009162599A

Description

本発明は、ロボットやマニピュレータ等の可動体に作用する外力の検出方法と、その検出装置に関し、簡単な構成で外力の大きさ及び作用点の位置を検出できるようにしたものである。 The present invention relates to a method for detecting an external force acting on a movable body such as a robot and a manipulator, and a detection apparatus for the method, and to detect the magnitude of the external force and the position of an action point with a simple configuration.

近年、ロボットは、医療、福祉、家庭内作業などの分野での利用が検討されているが、こうした人間と接触する環境では、ロボットの動きによって人間に危険が及ばないように十分な安全対策が必要となる。
そのため、ロボットの全身を触覚センサのシートで覆い、人間などへの接触を感知した場合に動作を停止する制御方式が検討されている。しかし、触覚センサは高価であり、ロボットの製造コストが上昇する。また、ロボットの各所に作用する外力の正確な合力が計測できない、と言う欠点も有している。 In recent years, robots are being considered for use in fields such as medical care, welfare, and home work. However, in such an environment where people come into contact with humans, sufficient safety measures must be taken so that the movement of the robot does not endanger humans. Necessary.
Therefore, a control method is being studied in which the entire body of the robot is covered with a tactile sensor sheet, and the operation is stopped when contact with a human being is detected. However, the tactile sensor is expensive, and the manufacturing cost of the robot increases. In addition, there is a disadvantage that the accurate resultant force of the external force acting on each part of the robot cannot be measured.

下記非特許文献１には、ロボットの体表面を剛体のカバーで囲い、このカバーとロボットとの接続を、力覚センサを介して行い、カバーに作用する外力を力覚センサで検出する外力検出装置が開示されている。この装置では、ロボット本体とカバーとが一点で接続され、その接続点に６軸力覚センサが配置されている。また、カバーは、４つの平面を有しているが、各平面には接触センサが貼り付けられている。
６軸力覚センサは、複数の歪みゲージを内蔵しており、３軸（ｘ、ｙ、ｚ軸）方向の力の成分と、３軸の回りのモーメントとが検出できるように構成されている。 Non-Patent Document 1 discloses an external force detection in which a body surface of a robot is surrounded by a rigid cover, the cover and the robot are connected via a force sensor, and an external force acting on the cover is detected by the force sensor. An apparatus is disclosed. In this apparatus, the robot body and the cover are connected at a single point, and a 6-axis force sensor is arranged at the connection point. The cover has four planes, and a contact sensor is attached to each plane.
The six-axis force sensor incorporates a plurality of strain gauges, and is configured to detect a force component in the three-axis (x, y, z-axis) directions and a moment around the three axes. .

この外力検出装置では、剛体のカバーの表面に作用する外力が全てロボット本体とカバーとの接続点に集中するため、その接続点に配置された６軸力覚センサにより、正確に外力ベクトルを検出することができる。
また、接触センサは、外力が作用した面の情報を提供し、６軸力覚センサで検出された応力及びモーメントの情報と、接触センサからの接触面情報とが組み合わせされて、カバーに作用した外力の大きさ及び作用点が算出される。
岩田浩康他「人間共存ロボットのための全身触覚インタフェース」日本ロボット学会誌Ｖｏｌ．２０Ｎｏ．５，ＰＰ．５４３〜５４９，２００２年7月 In this external force detection device, all external forces acting on the surface of the rigid cover are concentrated at the connection point between the robot body and the cover, so the external force vector is accurately detected by the 6-axis force sensor located at the connection point. can do.
The contact sensor provides information on the surface on which the external force is applied, and the information on the stress and moment detected by the 6-axis force sensor and the contact surface information from the contact sensor are combined to act on the cover. The magnitude and point of action of the external force are calculated.
Hiroyasu Iwata et al. “Whole-body tactile interface for human-symbiotic robots” Journal of the Robotics Society of Japan Vol. 20 No. 5, PP. 543-549, July 2002

しかし、非特許文献１に記載された外力検出装置は、接触面情報を得るための接触センサを必要としているため、低コスト化が難しい。
本発明は、こうした事情を考慮して創案したものであり、ロボットやマニピュレータ等の可動体に作用する外力の大きさ及び作用点を、接触センサ無しで検出することができる外力検出方法を提供し、また、その方法を実施する外力検出装置を提供することを目的としている。 However, since the external force detection device described in Non-Patent Document 1 requires a contact sensor for obtaining contact surface information, it is difficult to reduce the cost.
The present invention was devised in view of such circumstances, and provides an external force detection method capable of detecting the magnitude and point of an external force acting on a movable body such as a robot or a manipulator without a contact sensor. Moreover, it aims at providing the external force detection apparatus which enforces the method.

本発明の外力検出方法は、可動体に作用する外力の大きさ及び作用点を検出する検出方法であって、前記可動体を、剛性を有する甲殻型カバーで覆い、前記甲殻型カバーに一点で接続する１または複数の接続体で該甲殻型カバーと前記可動体との間を接続するとともに、前記接続体の前記甲殻型カバーとの接続点に作用する応力またはモーメントを測定する測定手段を設け、前記甲殻型カバーの幾何形状のデータと、前記１または複数の測定手段により測定された応力またはモーメントに関する５種類の測定値とを用いて、次の（式１）により外力Ｆ^disの大きさ及び作用点Ｐ^disを算出し、算出した作用点Ｐ ^dis が前記甲殻型カバーの幾何形状の上に存在することを利用して作用点Ｐ ^dis の位置を特定することを特徴とする。
（式１）
Ｆ^dis ＋Ｆ^sum＝０
Ｆ^dis ×（Ｐ^dis − Ｐ^o ）＋Ｍ^o＝０
Ｆ^sum ＝ΣＦ^j
Ｍ^o ＝Σ{Ｆ^j ×（Ｐ^j − Ｐ^o）} ＋ ΣＭ^j
（但し、Ｆ^jは、前記測定手段で測定したｊ番目の前記接続体に作用する応力の測定値、Ｍ^jは、前記測定手段で測定したｊ番目の前記接続体に作用するモーメントの測定値、Ｐ^jは、ｊ番目の前記接続体の位置ベクトル、Ｐ^oは、モーメントを求める基準位置の位置ベクトル、Ｆ^sumは、前記測定手段の全てによって測定された応力の総和、Ｍ^oは、前記測定手段の全てによって測定されたモーメントの総和、また、前記接続体の数をｍとするとき、Σは、ｊ＝１からｍまでの加算を示している。）
この方法では、接触センサを用いずに、甲殻型カバーのあらゆる位置に作用した外力の大きさ及び作用点を検出することができる。 The external force detection method of the present invention is a detection method for detecting the magnitude and point of action of an external force acting on a movable body, wherein the movable body is covered with a rigid shell-type cover, and the shell-type cover has a single point. in one or more connectors for connecting with connecting the該甲shelled cover and the movable body, provided with a measuring means for measuring the stress or moment acting on the connecting point between the shell-type cover of the connector The magnitude of the external force F ^dis is expressed by the following (Equation 1) using the geometric data of the shell-shaped cover and the five kinds of measured values relating to the stress or moment measured by the one or more measuring means. And the point of action P ^dis is calculated, and the position of the point of action P ^dis is specified using the fact that the calculated point of action P ^dis exists on the geometric shape of the crustacean cover .
(Formula 1)
F ^dis + F ^sum = 0
F ^dis × (P ^dis −P ^o ) + M ^o = 0
F ^sum = ΣF ^j
M ^o = Σ {F ^j × (P ^j −P ^o )} + ΣM ^j
(Where F ^j is a measured value of stress acting on the j-th connected body measured by the measuring means, and M ^j is a measured value of moment acting on the j-th connected body measured by the measuring means. , P ^j is the position vector of the j-th connected body, P ^o is the position vector of the reference position for obtaining the moment, F ^sum is the ^sum of the stresses measured by all of the measuring means, and M ^o is the above-mentioned (The sum of moments measured by all of the measuring means, and when m is the number of connected bodies, Σ indicates an addition from j = 1 to m.)
In this method, it is possible to detect the magnitude and point of action of an external force that has acted on any position of the crustacean cover without using a contact sensor.

また、本発明の外力検出方法では、前記接続体の数を１に設定するとともに、前記接続体として、前記測定手段を兼ねる、３軸方向の応力成分及び２軸の回りのモーメント成分が測定可能なセンサまたは２軸方向の応力成分及び３軸の回りのモーメント成分が測定可能なセンサを使用し、次の（式２）により外力Ｆ^disの大きさ及び作用点Ｐ^disを算出することを特徴とする。
（式２）
Ｆ^dis ＋Ｆ1＝０
Ｆ^dis ×（Ｐ^dis − Ｐ¹ ）＋Ｍ¹＝０
（但し、Ｆ¹は、前記センサで測定した応力の測定値、Ｍ¹は、前記センサで測定したモーメントの測定値、Ｐ¹は、前記センサの位置ベクトルを示している。）
（式２）は、式１においてｍ＝１の場合に相当している。この方法では、５軸の測定が可能な力覚センサの１つを用いて、甲殻型カバーのあらゆる位置に作用した外力の大きさ及び作用点を検出することができる。 Further, in the external force detection method of the present invention, the number of the connecting bodies is set to 1, and as the connecting body, the stress component in the three-axis direction and the moment component around the two axes that also serve as the measuring means can be measured. A sensor capable of measuring a stress component in the biaxial direction and a moment component around the three axes, and calculating the magnitude of the external force F ^dis and the point of action P ^dis by the following (Equation 2). And
(Formula 2)
F ^dis + F1 = 0
F ^dis × (P ^dis −P ¹ ) + M ¹ = 0
(However, F ¹ is a measured value of stress measured by the sensor, M ¹ is a measured value of moment measured by the sensor, and P ¹ is a position vector of the sensor.)
(Expression 2) corresponds to the case of m = 1 in Expression 1. In this method, it is possible to detect the magnitude and point of action of an external force applied to any position of the crustacean cover using one of force sensors capable of measuring five axes .

また、本発明の外力検出方法では、前記接続体の数を２以上に設定するとともに、前記複数の接続体として、前記測定手段を兼ねる、１軸以上の方向の応力成分または１軸以上の軸の回りのモーメント成分が測定可能な複数のセンサであって、当該複数のセンサの測定可能な応力成分の軸数及び測定可能なモーメント成分の軸数の合計が５軸である複数のセンサを使用し、前記（式１）により外力Ｆ^disの大きさ及び作用点Ｐ^disを算出することを特徴とする。
例えば、３軸方向の応力が検出可能な３軸力覚センサ１個と、１軸方向の応力が検出可能な１軸力覚センサ２個とを使用して可動体と甲殻型カバーとを接続すれば、軸数の合計は５軸となる。また、１軸方向の応力が検出可能な１軸力覚センサを５個使用した場合も軸数の合計は５軸となる。このように、複数の力覚センサの検出可能な軸数の合計が５軸となるように設定すれば、甲殻型カバーのあらゆる位置に作用した外力の大きさ及び作用点を検出することができる。 Further, in the external force detection method of the present invention, the number of the connecting bodies is set to 2 or more, and the plurality of connecting bodies also serve as the measuring means, and the stress component in one or more axes or one or more axes. A plurality of sensors capable of measuring moment components around the sensor, wherein the total number of measurable stress component axes and measurable moment component axes is five axes. Then, the magnitude of the external force F ^dis and the action point P ^dis are calculated by the above (Equation 1).
For example, a movable body and a shell-type cover are connected by using one triaxial force sensor capable of detecting triaxial stress and two uniaxial force sensors capable of detecting uniaxial stress. In this case, the total number of axes is 5 . Also, when five uniaxial force sensors capable of detecting uniaxial stress are used, the total number of axes is five . Thus, if the total number of axes that can be detected by the plurality of force sensors is set to 5 axes , it is possible to detect the magnitude and point of action of the external force applied to any position of the crustacean cover. .

また、本発明の外力検出方法では、前記接続体として５本のワイヤを使用し、前記測定手段により前記ワイヤの張力を測定し、前記式のＦ^sum及びＭ^oを次の（式３）により算出することを特徴とする。
（式３）
Ｆ^sum ＝Σ（ｄ^j／｜ｄ^j｜）×（ｆ^j−ｆ^oj ）
Ｍ^o ＝Σ ｐ^j×（ｄ^j／｜ｄ^j｜）×（ｆ^j−ｆ^oj ）
（但し、ｐ^jは、ｊ番目の前記ワイヤの可動体側の端点位置、ｄ^jは、ｊ番目の前記ワイヤのｐ^jを基準とした甲殻型カバー側の端点位置、ｆ^jは、前記測定手段で測定したｊ番目の前記ワイヤの張力の測定値、ｆ^ojは、外力が作用しない状態でのｊ番目の前記ワイヤの張力初期値、また、前記ワイヤの数をｍとするとき、Σは、ｊ＝１からｍまでの加算を示している。）
この方法では、多軸力覚センサを用いずに、甲殻型カバーのあらゆる位置に作用した外力の大きさ及び作用点を検出することが可能であり、低コストで外力の検出を行うことができる。 Further, the external force detection method of the present invention, using a 5-wire as the connection member, the tension of the wire is measured by the measuring means, the F ^sum and M ^o in the formula by the following (Equation 3) It is characterized by calculating.
(Formula 3)
F ^sum = Σ (d ^j / | d ^j |) × (f ^j −f ^oj )
M ^o = Σ p ^j × (d ^j / | d ^j |) × (f ^j −f ^oj )
(Where p ^j is the end point position of the j-th wire on the movable body side, d ^j is the end point position of the j-th wire on the crust-type cover side with reference to p ^j , and f ^j is the measuring means The measured value of the tension of the j-th wire measured in step ^foj is the initial value of the tension of the j-th wire when no external force is applied, and when the number of wires is m, Σ is (The addition from j = 1 to m is shown.)
In this method, it is possible to detect the magnitude and point of action of the external force applied to any position of the crustacean cover without using a multi-axis force sensor, and the external force can be detected at low cost. .

また、本発明の外力検出装置は、可動体に作用する外力の大きさ及び作用点を検出する外力検出装置であって、前記可動体を覆う剛性を有する甲殻型カバーと、前記甲殻型カバーに一点で接続して該甲殻型カバーと前記可動体との間を接続する１または複数の接続体と、前記接続体の前記甲殻型カバーとの接続点に作用する応力またはモーメントを測定する測定手段と、を備え、前記甲殻型カバーの幾何形状のデータと、前記１または複数の測定手段により測定された応力またはモーメントに関する５種類の測定値とを用いて、（式１）により外力Ｆ^disの大きさ及び作用点Ｐ^disを算出し、算出した作用点Ｐ ^dis が前記甲殻型カバーの幾何形状の上に存在することを利用して作用点Ｐ ^dis の位置を特定することを特徴とする。
この装置は、接触センサを用いずに、甲殻型カバーのあらゆる位置に作用した外力の大きさ及び作用点を検出することができる。 The external force detection device of the present invention is an external force detection device that detects the magnitude and point of action of an external force acting on a movable body, and includes a shell-type cover that has rigidity to cover the movable body, and the shell-type cover. Measuring means for measuring a stress or moment acting on a connection point between one or a plurality of connection bodies connected at a single point to connect the shell-type cover and the movable body, and the shell-type cover of the connection body And using the data of the geometric shape of the shell-shaped cover and the five types of measured values relating to the stress or moment measured by the one or more measuring means, the external force F ^dis The size and the action point P ^dis are calculated, and the position of the action point P ^dis is specified by using the calculated action point P ^dis existing on the geometric shape of the shell-type cover .
This device can detect the magnitude and point of action of an external force applied to any position of the crustacean cover without using a contact sensor.

また、本発明の外力検出装置は、前記接続体に、前記測定手段を兼ねる、３軸方向の応力成分及び２軸の回りのモーメント成分が測定可能なセンサまたは２軸方向の応力成分及び３軸の回りのモーメント成分が測定可能なセンサを用いて構成することができる。
この装置では、甲殻型カバーが、５軸の測定が可能な力覚センサの１個を用いて可動体に一点支持される。 In the external force detection device of the present invention, the connecting body also serves as the measuring means, a sensor capable of measuring a triaxial stress component and a moment component around two axes , or a biaxial stress component and triaxial. It is possible to configure using a sensor capable of measuring the moment component around.
In this device, the crustacean cover is supported at one point on the movable body using one of the force sensors capable of measuring five axes .

また、本発明の外力検出装置は、前記接続体に、前記測定手段を兼ねる、１軸以上の方向の応力成分または１軸以上の軸の回りのモーメント成分が測定可能な複数のセンサであって、当該複数のセンサの測定可能な応力成分の軸数及び測定可能なモーメント成分の軸数の合計が５軸である複数のセンサを用いて構成することができる。
この装置は、甲殻型カバーが力覚センサによって多点支持される。 Moreover, the external force detection device of the present invention is a plurality of sensors capable of measuring a stress component in one or more axes or a moment component around one or more axes that also serves as the measurement means on the connection body. The plurality of sensors can be configured by using a plurality of sensors in which the total number of measurable stress component axes and measurable moment component axes is five axes .
In this device, the shell-type cover is supported at multiple points by a force sensor.

また、本発明の外力検出装置は、前記接続体に、５本のワイヤを用いて構成することができる。
この装置は、力覚センサを用いていないため、低コスト化を図ることができる。
また、本発明の外力検出装置は、ロボット、マニピュレータ、工作機械、車椅子、乗物などの外力検出に用いることができる。 Moreover, the external force detection apparatus of this invention can be comprised using five wires for the said connection body.
Since this device does not use a force sensor, cost reduction can be achieved.
In addition, the external force detection device of the present invention can be used for external force detection of robots, manipulators, machine tools, wheelchairs, vehicles, and the like.

本発明では、ロボットやマニピュレータなどに作用する外力の大きさや作用点を、触覚センサを用いること無く、検出することができるため、コストパフォーマンスの向上、配線の削減、メンテナンスの簡易化等の効果が得られる。 In the present invention, the magnitude and point of action of an external force acting on a robot, manipulator, etc. can be detected without using a tactile sensor, so there are advantages such as improved cost performance, reduced wiring, and simplified maintenance. can get.

本発明の実施形態について、図面に基づいて説明する。
図１は、移動ロボットとそれに装着される甲殻型カバーを示し、図２は、甲殻型カバーに作用する力を説明している。図３は、多自由度マニピュレータに多点支持された甲殻型カバーを示し、図４は、この甲殻型カバーに作用する力を説明している。また、図５は、ワイヤでロボット本体に接続された甲殻型カバーを示している。図６は、実験に用いた移動ロボット及び甲殻型カバーを示し、図７及び図８は、実験結果を示している。 Embodiments of the present invention will be described with reference to the drawings.
FIG. 1 shows a mobile robot and a shell-type cover attached to the mobile robot, and FIG. 2 explains forces acting on the shell-type cover. FIG. 3 shows a shell-type cover supported by a multi-degree-of-freedom manipulator at multiple points, and FIG. 4 explains the forces acting on this shell-type cover. FIG. 5 shows a shell-type cover connected to the robot body with a wire. FIG. 6 shows the mobile robot and crustacean cover used in the experiment, and FIGS. 7 and 8 show the experimental results.

図１に示すように、移動ロボット１０に、６軸力覚センサ１２を介して、剛性の甲殻型カバー１１を装着し、甲殻型カバー１１を移動ロボット１０に固定する。
甲殻型カバー１１で覆われた移動ロボット１０が移動中に人や障害物に接触すると、移動ロボット１０が人や障害物から受ける外力は、全て甲殻型カバー１１に作用し、甲殻型カバー１１と移動ロボット１０との接続点に配置された６軸力覚センサ１２に集中し、６軸力覚センサ１２によって検出される。
６軸力覚センサ１２の検出情報は、制御部（不図示）に送られて、外力の大きさや作用点が算出される。制御部は、移動ロボット１０を外部から監視・制御する制御装置の中に在り、あるいは、移動ロボット１０の中に在っても良い。いずれにしろ、制御部は、外力の大きさ及び作用点を算出するために、甲殻型カバー１１の幾何形状や移動ロボット１０との接続位置に関するデータを保持している。 As shown in FIG. 1, a rigid crustacean cover 11 is attached to a mobile robot 10 via a 6-axis force sensor 12, and the crustacean cover 11 is fixed to the mobile robot 10.
When the mobile robot 10 covered with the crustacean cover 11 touches a person or an obstacle while moving, all the external forces that the mobile robot 10 receives from the person or the obstacle act on the crustaceum cover 11, It concentrates on the 6-axis force sensor 12 arranged at the connection point with the mobile robot 10 and is detected by the 6-axis force sensor 12.
The detection information of the 6-axis force sensor 12 is sent to a control unit (not shown), and the magnitude and action point of the external force are calculated. The control unit may be in a control device that monitors and controls the mobile robot 10 from the outside, or may be in the mobile robot 10. In any case, the control unit holds data relating to the geometric shape of the crustacean cover 11 and the connection position with the mobile robot 10 in order to calculate the magnitude and point of action of the external force.

図２に示すように、甲殻型カバー１１に作用する外力のベクトルをＦ^dis、外力の作用点の位置ベクトルをＰ^disで表し、また、６軸力覚センサ１２で測定された力ベクトルをＦ¹、６軸力覚センサ１２の位置ベクトルをＰ¹、６軸力覚センサ１２で測定されたモーメントをＭ¹で表す。
このとき、力の釣り合いを示す（数１）が成り立ち、また、モーメントの釣り合いを示す（数２）が成り立つ。
Ｆ^dis ＋Ｆ¹＝０（数１）
Ｆ^dis ×（Ｐ^dis − Ｐ¹ ）＋Ｍ¹＝０（数２）
（数１）から、Ｆ^disの成分は、
Ｆ^dis _x＝−Ｆ¹ _x （数３）
Ｆ^dis _y＝−Ｆ¹ _y （数４）
Ｆ^dis _z＝−Ｆ¹ _z （数５）
と算出することができる。 As shown in FIG. 2, the vector of the external force acting on the crustacean cover 11 is represented by F ^dis , the position vector of the point of action of the external force is represented by P ^dis , and the force vector measured by the 6-axis force sensor 12 is represented by F ^dis ¹ , the position vector of the 6-axis force sensor 12 is represented by P ¹ , and the moment measured by the 6-axis force sensor 12 is represented by M ¹ .
At this time, (Equation 1) indicating a balance of forces is established, and (Equation 2) indicating a balance of moments is established.
F ^dis + F ¹ = 0 ( ^Equation 1)
F ^dis × (P ^dis −P ¹ ) + M ¹ = 0 (Equation 2)
From ( ^Equation 1), the component of F ^dis is
F ^dis _x = −F ¹ _x (Equation 3)
F ^dis _y = −F ¹ _y (Equation 4)
F ^dis _z = −F ¹ _z (Equation 5)
Can be calculated.

また、（数２）は、次のように展開することができる。
Ｍ¹ _x＝Ｆ^dis _z（Ｐ^dis _y− Ｐ¹ _y ）−Ｆ^dis _y（Ｐ^dis _z− Ｐ¹ _z ）（数６）
Ｍ¹ _y＝Ｆ^dis _x（Ｐ^dis _z− Ｐ¹ _z ）−Ｆ^dis _z（Ｐ^dis _x− Ｐ¹ _x ）（数７）
Ｍ¹ _z＝Ｆ^dis _y（Ｐ^dis _x− Ｐ¹ _x ）−Ｆ^dis _x（Ｐ^dis _y− Ｐ¹ _y ）（数８）
ここで、（数６）（数７）を用いて、
Ｐ^dis _z− Ｐ¹ _z＝｛Ｍ¹ _x＋Ｆ^dis _z（Ｐ^dis _y− Ｐ¹ _y ）｝／Ｆ^dis _y
＝｛−Ｍ¹ _y＋Ｆ^dis _z（Ｐ^dis _x− Ｐ¹ _x）｝／Ｆ^dis _x （数９）
更に（数３）（数４）（数５）を用いて、
Ｐ^dis _z＝｛（Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _y）Ｐ^dis _y
＝｛（−Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _x）Ｐ^dis _x
（数１０）
と表すことができる。 (Expression 2) can be expanded as follows.
^{_{^{_{M 1 x = F dis z (}}}} P dis y - P 1 y) -F dis y (P dis z - P 1 z) ( 6)
M ¹ _y = F ^dis _x (P ^dis _z −P ¹ _z ) −F ^dis _z (P ^dis _x −P ¹ _x ) (Expression 7)
M ¹ _z = F ^dis _y (P ^dis _x −P ¹ _x ) −F ^dis _x (P ^dis _y −P ¹ _y ) (Equation 8)
Here, using (Equation 6) (Equation 7),
P ^dis _z −P ¹ _z = {M ¹ _x + F ^dis _z (P ^dis _y −P ¹ _y )} / F ^dis _y
= {− M ¹ _y + F ^dis _z (P ^dis _x −P ¹ _x )} / F ^dis _x ( ^Equation 9)
Furthermore, using (Equation 3), (Equation 4), and (Equation 5),
P ^dis _z = {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y } + P ¹ _z + (F ¹ _z / F ¹ _y ) P ^dis _y
^{_{= {(- M 1 y -F}} 1 z P 1 x) / F 1 x} + P 1 z + (F 1 z / F 1 x) P dis x
(Equation 10)
It can be expressed as.

（数１０）において、Ｆ¹ _x、Ｆ¹ _y、Ｆ¹ _z、Ｍ¹ _x、Ｍ¹ _yの５種類は６軸力覚センサ１２の検出情報から得られる。また、Ｐ¹ _x、Ｐ¹ _y、Ｐ¹ _zは、６軸力覚センサ１２の位置情報であり、既知である。従って、（数１０）では、Ｐ^dis _z、Ｐ^dis _y及びＰ^dis _xだけが未知数であり、（数１０）は、ｙ−ｚ平面に投影された直線が、
Ｐ^dis _z＝｛（Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _y）Ｐ^dis _y
であり、ｘ−ｚ平面に投影された直線が、
Ｐ^dis _z＝｛（−Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _x）Ｐ^dis _x
である３次元空間の直線を表している。
力の作用点は、（数１０）で表される直線と甲殻型カバー１１との交点として求めることができ、甲殻型カバー１１の形状から規定されるＰ^dis _z、Ｐ^dis _y及びＰ^dis _xの関係式と（数１０）とからＰ^dis _z、Ｐ^dis _y及びＰ^dis _xの値を算出することができる。 In (Expression 10), five types of F ¹ _x , F ¹ _y , F ¹ _z , M ¹ _x , and M ¹ _y are obtained from detection information of the 6-axis force sensor 12. P ¹ _x , P ¹ _y , and P ¹ _z are position information of the 6-axis force sensor 12 and are known. Therefore, in ( ^Equation 10), only P ^dis _z , P ^dis _y and P ^dis _x are unknowns, and (Equation 10) shows that the straight line projected on the yz plane is
P ^dis _z = {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y } + P ¹ _z + (F ¹ _z / F ¹ _y ) P ^dis _y
And the straight line projected on the xz plane is
P ^dis _z = {(− M ¹ _y −F ¹ _z P ¹ _x ) / F ¹ _x } + P ¹ _z + (F ¹ _z / F ¹ _x ) P ^dis _x
Represents a straight line in a three-dimensional space.
The point of action of the force can be obtained as an intersection of the straight line represented by (Equation 10) and the shell-type cover 11, and P ^dis _z , P ^dis _y and P ^dis _x defined by the shape of the shell-type cover 11. The values of P ^dis _z , P ^dis _y, and P ^dis _x can be calculated from the relational expression (10) and (Equation 10).

ここで注目すべきは、甲殻型カバーの幾何形状のデータが保持されている場合に、６軸力覚センサ１２の５種類の検出情報から作用点が同定できる点である。ここではＦ¹ _x、Ｆ¹ _y、Ｆ¹ _z、Ｍ¹ _x、Ｍ¹ _yの５種類の検出情報を使用したが、（数４）（数５）（数６）（数７）（数８）を用いて算出する場合は、Ｆ¹ _y、Ｆ¹ _z、Ｍ¹ _x、Ｍ¹ _y、Ｍ¹ _zの５種類の検出情報を使用して作用点を同定することができる。
そのため、移動ロボット１０を甲殻型カバー１１で覆い、この甲殻型カバー１１を、３軸方向の応力成分及び２軸以上の軸の回りのモーメント成分が測定可能なセンサ、あるいは、２軸以上の方向の応力成分及び３軸の回りのモーメント成分が測定可能なセンサを介して、移動ロボット１０上に一点支持するように構成すれば、（数１）（数２）の式を用いて、センサの測定結果から、移動ロボット１０に作用する外力の大きさ及び作用点を算出することができる。 What should be noted here is that the action point can be identified from the five types of detection information of the six-axis force sensor 12 when data of the geometric shape of the crustacean cover is held. Here, five types of detection information F ¹ _x , F ¹ _y , F ¹ _z , M ¹ _x , and M ¹ _y are used, but (Equation 4) (Equation 5) (Equation 6) (Equation 7) (Equation 7) When calculating using 8), the action point can be identified using five types of detection information of F ¹ _y , F ¹ _z , M ¹ _x , M ¹ _y , and M ¹ _z .
Therefore, the mobile robot 10 is covered with a shell-type cover 11, and the shell-type cover 11 is a sensor capable of measuring a stress component in three axial directions and a moment component around two or more axes, or in two or more directions. If the sensor is configured to support one point on the mobile robot 10 through a sensor capable of measuring the stress component and the moment component about three axes, the equation of (Equation 1) and (Equation 2) can be used. From the measurement result, the magnitude and point of action of the external force acting on the mobile robot 10 can be calculated.

次に、甲殻型カバーを多点支持する場合について説明する。
図３（ａ）の多自由度を持つロボットアームやマニピュレータ２０に対して、図３（ｂ）や図３（ｃ）に示すように、甲殻型カバー２１を被覆する場合は、甲殻型カバー２１を一点で支持するよりも、図３（ｄ）に示すように、複数の力覚センサ２２、２３、２４で多点支持する方が適している。 Next, a case where the shell-type cover is supported at multiple points will be described.
As shown in FIGS. 3 (b) and 3 (c), the shell-type cover 21 is applied to the robot arm or manipulator 20 having multiple degrees of freedom shown in FIG. 3 (a). It is more suitable to support multiple points with a plurality of force sensors 22, 23, 24, as shown in FIG.

このように、甲殻型カバー２１を複数の力覚センサ２２、２３、２４を介してマニピュレータ２０上に多点支持した場合は、図４に示すように、甲殻型カバー１１に対し、作用点Ｐ^disに作用する外力Ｆ^disと、各力覚センサ２２、２３、２４の位置Ｐ¹、Ｐ²、Ｐ³における反力Ｆ¹、Ｆ²、Ｆ³とが加わることになる。
このとき、力の釣り合いから（数１１）が成り立ち、モーメントの釣り合いから（数１２）が成り立つ。
Ｆ^dis ＋Ｆ¹＋Ｆ²＋Ｆ³＝０（数１１）
Ｆ²×(Ｐ²−Ｐ¹)＋Ｆ³×(Ｐ³−Ｐ¹)＋Ｆ^dis×(Ｐ^dis−Ｐ¹)＝０
（数１２） In this way, when the crustacean cover 21 is supported on the manipulator 20 via the plurality of force sensors 22, 23, 24, the point of action P is applied to the crustacean cover 11 as shown in FIG. and the external force F ^dis acting on ^dis, so that the position P ^1, P ^2, the reaction force F ¹ in P ^{^3,} F ^2, F ³ of the respective force sensors 22, 23 and 24 is applied.
At this time, (Equation 11) holds from the balance of forces, and (Equation 12) holds from the balance of moments.
F ^dis + F ¹ + F ² + F ³ = 0 (Equation 11)
F ² × (P ² −P ¹ ) + F ³ × (P ³ −P ¹ ) + F ^dis × (P ^dis −P ¹ ) = 0
(Equation 12)

この場合、例えば、力覚センサ２２として３軸方向の応力成分が測定可能なセンサを用い、力覚センサ２３として１軸方向の応力成分が測定可能なセンサを用い、また、力覚センサ２４として１軸方向の応力成分が測定可能なセンサを用いる、と言うように、力覚センサ２２、２３、２４によって測定可能な軸数の合計が５軸以上であれば、一点支持の場合と同様に、甲殻型カバー２１の幾何形状や力覚センサ２２、２３、２４の位置情報が既知であるとき、（数１１）（数１２）を用いて、甲殻型カバー２１のあらゆる位置に作用する外力の大きさ及び作用点を求めることができる。 In this case, for example, a sensor capable of measuring a stress component in three axial directions is used as the force sensor 22, a sensor capable of measuring a stress component in one axial direction is used as the force sensor 23, and the force sensor 24 is used as the force sensor 24. If the total number of axes that can be measured by the force sensors 22, 23, 24 is 5 or more, as in the case of using a sensor capable of measuring a stress component in one axis direction, the same as in the case of one-point support. When the geometric shape of the crustacean cover 21 and the position information of the force sensors 22, 23, 24 are known, (Equation 11) (Equation 12) The size and point of action can be determined.

また、甲殻型カバー２１の支持に用いる力覚センサの個数をｍ個にまで拡張すると、力の釣り合いから（数１３）（数１４）が成り立ち、モーメントの釣り合いから（数１５）（数１６）が成り立つ。
Ｆ^dis ＋Ｆ^sum＝０（数１３）
Ｆ^sum ＝ΣＦ^j （数１４）
Ｆ^dis ×（Ｐ^dis − Ｐ^o ）＋Ｍ^o＝０（数１５）
Ｍ^o ＝Σ{Ｆ^j ×（Ｐ^j − Ｐ^o）} ＋ ΣＭ^j （数１６）
ここで、Ｆ^jは、ｊ番目の力覚センサが検知した力、Ｍ^jは、ｊ番目の力覚センサが検知したモーメント、Ｐ^jは、ｊ番目の力覚センサの位置ベクトル、Ｐ^oは、モーメントを求める基準位置の位置ベクトル、Ｆ^sumは、各力覚センサにより検出される力の総和、Ｍ^oは、各力覚センサにより検出されるモーメントの総和、また、Σは、ｊ＝１からｍまでの加算を示している。 Further, when the number of force sensors used for supporting the shell-shaped cover 21 is expanded to m, (Equation 13) and (Equation 14) are established from the balance of forces, and (Equation 15) and (Equation 16) are obtained from the balance of moments. Holds.
F ^dis + F ^sum = 0 ( ^Equation 13)
F ^sum = ΣF ^j (Equation 14)
F ^dis × (P ^dis −P ^o ) + M ^o = 0 (Equation 15)
M ^o = Σ {F ^j × (P ^j −P ^o )} + ΣM ^j (Equation 16)
Here, F ^j is the force detected by the j-th force sensor, M ^j is the moment detected by the j-th force sensor, P ^j is the position vector of the j-th force sensor, and P ^o is , the position vector of the reference position for determining the moment, F ^sum is the total sum of the force detected by the respective force sensors, M ^o is the sum of the moments detected by the respective force sensors, also, sigma is j = 1 The addition from m to m is shown.

この場合も、ｍ個の力覚センサによって測定できる軸数の合計が５軸以上であれば、一点支持の場合と同様に、甲殻型カバーの幾何形状や力覚センサの位置情報が既知であるとき、（数１３）〜（数１６）を用いて、甲殻型カバーのあらゆる位置に作用した外力の大きさ及び作用点を求めることができる。
なお、（数１３）〜（数１６）の算出に際し、検知できない成分は全て０とする。例えば、ｊ番目の力覚センサが３軸方向の応力成分のみの測定が可能な３軸力覚センサである場合は、Ｍ^j＝０となる。 Also in this case, if the total number of axes that can be measured by the m force sensors is 5 or more, the geometric shape of the crustacean cover and the position information of the force sensor are known as in the case of one-point support. Sometimes, using (Equation 13) to (Equation 16), the magnitude and point of action of the external force acting on any position of the crustacean cover can be obtained.
Note that in the calculation of (Equation 13) to (Equation 16), all components that cannot be detected are set to 0. For example, when the j-th force sensor is a triaxial force sensor capable of measuring only the stress component in the triaxial direction, M ^j = 0.

また、この場合、検知し得ない軸方向の力が力覚センサの応答値に干渉しないことが必要である。例えば、３軸力覚センサであれば、モーメントが力応答に干渉しないようにユニバーサルジョイントなどを介して甲殻型カバーを保持する必要がある。
なお、一点支持における（数１）（数２）は、（数１３）〜（数１６）において、ｍ＝１とした場合に相当する。 In this case, it is necessary that the axial force that cannot be detected does not interfere with the response value of the force sensor. For example, in the case of a three-axis force sensor, it is necessary to hold the crustace cover via a universal joint or the like so that the moment does not interfere with the force response.
In addition, (Equation 1) and (Equation 2) in one-point support correspond to the case where m = 1 in (Equation 13) to (Equation 16).

次に、甲殻型カバーを複数本のワイヤで多点支持する場合について説明する。
図５に示すように、甲殻型カバー３０とロボット本体４０とを５本以上のワイヤ３１、３２、３３、３４、３５で接続し、各ワイヤの張力を検出して、甲殻型カバー３０に作用する外力の大きさ及び作用点を求める。なお、ワイヤに加わる張力は、例えば、ワイヤに沿わせた線状のテンションメータの両端をワイヤに固定したり、ワイヤ表面やワイヤ内部に歪ゲージを取り付けたりして計測することができる。
この場合の外力検出は、甲殻型カバー３０を支持するセンサとして、１軸のセンサを用いたものに相当する。従って、甲殻型カバー３０とロボット本体４０との接続に５本以上のワイヤを使用すれば、（数１３）〜（数１６）を用いて、甲殻型カバー３０のあらゆる位置に作用した外力の大きさ及び作用点を求めることができる。 Next, a case where the crustacean cover is supported at multiple points by a plurality of wires will be described.
As shown in FIG. 5, the shell-type cover 30 and the robot body 40 are connected by five or more wires 31, 32, 33, 34, and 35, and the tension of each wire is detected to act on the shell-type cover 30. Determine the magnitude and point of action of the external force to be applied. The tension applied to the wire can be measured, for example, by fixing both ends of a linear tension meter along the wire to the wire, or by attaching strain gauges on the wire surface or inside the wire.
The external force detection in this case corresponds to a sensor that uses a single-axis sensor as a sensor that supports the crustace cover 30. Therefore, if five or more wires are used for connection between the crustace cover 30 and the robot body 40, the magnitude of the external force acting on any position of the crustace cover 30 using (Equation 13) to (Equation 16). And the point of action can be determined.

ただ、この場合、各ワイヤの張力の総和Ｆ^sum及びモーメントの総和Ｍ^oは、（数１７）（数１８）で表される。
Ｆ^sum ＝Σ（ｄ^j／｜ｄ^j｜）（ｆ^j−ｆ^oj ）（数１７）
Ｍ^o＝Σ ｐ^j×（ｄ^j／｜ｄ^j｜）（ｆ^j−ｆ^oj ）（数１８）
ここで、ｐ^jは、ｊ番目のワイヤのロボット本体４０側の端点位置、ｄ^jは、ｊ番目のワイヤのｐ^jを基準とした甲殻型カバー３０側の端点位置、ｆ^jは、ｊ番目のワイヤの張力、ｆ^ojは、外力が作用しない状態でのｊ番目のワイヤの張力初期値、また、前記ワイヤの数をｍとするとき、Σは、ｊ＝１からｍまでの加算を示している。 However, in this case, the sum M ^o sum F ^sum and moment of the tension of each wire is represented by (Equation 17) (Equation 18).
F ^sum = Σ (d ^j / | d ^j |) (f ^j −f ^oj ) ( ^Equation 17)
M ^o = Σ p ^j × (d ^j / | d ^j |) (f ^j −f ^oj ) ( ^Equation 18)
Here, p ^j is the end point position of the j-th wire on the robot body 40 side, d ^j is the end point position on the crustace cover 30 side with respect to p ^j of the j-th wire, and f ^j is the j-th position. The wire tension, f ^oj is the initial value of the tension of the j-th wire when no external force is applied, and when the number of wires is m, Σ indicates the addition from j = 1 to m ing.

この場合、外力の作用点を検出するためには、張力が０より大きいワイヤが５本以上存在する必要である。外力によって一部のワイヤが弛むことが想定される場合は、大目にワイヤを張る必要がある。また、甲殻型カバー３０は、外力によって大きく変位しないことが必要である。変位が大きいとｄ^jが一定とみなせなくなる。
なお、ワイヤの代わりにロッドを使ってもよい。ただし、ロボット本体４０及び甲殻型カバー３０との接合部は、両方とも３軸のフリージョイントにする必要がある In this case, in order to detect the action point of the external force, it is necessary that there are five or more wires having a tension greater than zero. When it is assumed that some wires are loosened by an external force, it is necessary to stretch the wires roughly. Further, the crustace type cover 30 needs not to be greatly displaced by an external force. If the displacement is large, d ^j cannot be regarded as constant.
A rod may be used instead of a wire. However, the joint part between the robot body 40 and the crustace type cover 30 must be a triaxial free joint.

図６、図７、図８は、本発明の外力検出方法の有効性を確認するために実施した実験の模様を示している。
実験に用いた移動ロボットを図６（ａ）に示し、甲殻型カバーを図６（ｂ）に示している。甲殻型カバーは、厚さ３ｍｍのアクリル板で形成された外形寸法５００ｍｍ×５００ｍｍ×５００ｍｍの立方体であり、移動ロボットに被せるため、底面だけは塞がれていない。重さ１２ｋｇである。移動ロボットには、甲殻型カバーを一点支持する位置に６軸力覚センサ１２を配置している。 6, 7, and 8 show patterns of experiments performed to confirm the effectiveness of the external force detection method of the present invention.
The mobile robot used in the experiment is shown in FIG. 6A, and the crustace type cover is shown in FIG. 6B. The crustacean cover is a cube having an outer dimension of 500 mm × 500 mm × 500 mm formed of an acrylic plate having a thickness of 3 mm, and is covered with a mobile robot, so that only the bottom surface is not blocked. It weighs 12kg. In the mobile robot, a 6-axis force sensor 12 is arranged at a position where one point of the crustace type cover is supported.

実験では、図７に示すように、移動ロボットに被せた甲殻型カバーの一点を指で押し、その押圧位置を求めた。算出された押圧位置は表示画面に表示されている。
図８は、この押圧位置と検出位置との関係を定量的に測定した結果を示している。図８（ａ）では、実際の押圧位置（Actual position）を菱形で、検出位置（Detected position）を＋で表示している。また、図８（ｂ）では、押圧位置（Actual position）及び検出位置（Detected position）を座標で表示している。
この実験結果から、甲殻型カバーの各位置に作用した外力の作用点は、本発明の外力検出方法により、高精度で検出できることが分かる。 In the experiment, as shown in FIG. 7, a point on the crustacean cover placed on the mobile robot was pressed with a finger, and the pressed position was obtained. The calculated pressing position is displayed on the display screen.
FIG. 8 shows the result of quantitatively measuring the relationship between the pressed position and the detected position. In FIG. 8A, the actual pressed position (Actual position) is indicated by a diamond, and the detected position (Detected position) is indicated by +. Moreover, in FIG.8 (b), the press position (Actual position) and the detected position (Detected position) are displayed by the coordinate.
From this experimental result, it can be seen that the point of action of the external force applied to each position of the crustace type cover can be detected with high accuracy by the external force detection method of the present invention.

このように、本発明では、ロボット本体やマニピュレータ本体に、力覚センサ等の測定手段を介して甲殻型カバーを固定する。甲殻型カバーは、６自由度が全て測定手段により拘束され、ロボットに作用する外力の全ては甲殻型カバーで受け止められる。測定手段は、本体と甲殻型カバーとの接続箇所に作用する応力及びモーメントを検出し、制御部は、外力を受けてもモーメントが釣り合っていると仮定して、測定結果を基に、外力の総和及び作用点を算出する。 Thus, in the present invention, the crustacean cover is fixed to the robot body or manipulator body via the measuring means such as a force sensor. All six degrees of freedom of the crustace type cover is constrained by the measuring means, and all external forces acting on the robot are received by the crust type cover. The measurement means detects the stress and moment acting on the connection point between the main body and the crustace cover, and the control unit assumes that the moment is balanced even when subjected to external force, and based on the measurement result, Calculate the sum and action point.

本発明では、測定手段の測定可能な応力成分の軸数及び測定可能なモーメント成分の軸数の合計が５軸であれば、その測定値から外力の総和と作用点とを求めることができる。これは、換言すれば、拘束した甲殻型カバーの６自由度の内、５自由度について測定手段で測定することになる。
例えば、本体と甲殻型カバーとを、１個の３軸力覚センサと２個の１軸力覚センサとを介して接続すれば、外力の総和と作用点とが検出できる。また、５個の１軸力覚センサを介して接続しても、外力の総和と作用点とが検出できる。また、１個の５軸力覚センサを介して接続しても、外力の総和と作用点とが検出できる。ただし、計測しない６軸目の成分が他の５軸の成分に干渉しないことが条件となる。 In the present invention, if the total number of axes of measurable stress components and measurable moment components of the measuring means is five, the sum of external forces and the point of action can be obtained from the measured values. In other words, among the 6 degrees of freedom of the constrained shell-type cover, 5 degrees of freedom are measured by the measuring means.
For example, if the main body and the crustacean cover are connected via one three-axis force sensor and two one-axis force sensors, the sum of external forces and the point of action can be detected. Moreover, even if it connects via five 1 axis | shaft force sensors, the sum total of an external force and an action point can be detected. Moreover, even if it connects via one 5-axis force sensor, the sum total of an external force and an action point can be detected. However, the condition is that the sixth axis component that is not measured does not interfere with the other five axis components.

前述する実験では、本体と甲殻型カバーとを１個の６軸力覚センサを介して接続しているが、６軸力覚センサの検出情報のうち、５軸分の情報（ｆｘ，ｆｙ，ｆｚ，ｍｘ，ｍｙ）を利用して作用点を算出し、Ｚ軸周りのモーメントｍｚの情報は捨てている。この６軸力覚センサは、軸間の干渉が発生しないような構造になっているため、ｍｚは他の５軸の応答値に影響を与えない。 In the experiment described above, the main body and the crustacean cover are connected via a single 6-axis force sensor. Of the detection information of the 6-axis force sensor, information for five axes (fx, fy, The action point is calculated using fz, mx, my), and information on the moment mz around the Z axis is discarded. Since this 6-axis force sensor has a structure that does not cause interference between axes, mz does not affect the response values of the other 5 axes.

この甲殻型カバーを用いた外力検出装置でロボットの全てのリンクを覆うことにより、ロボットのあらゆる箇所に生じる外力とその作用点を計測することが可能となる。
また、この外力検出装置は、外力の大きさを定量的に把握できるため、例えば、撫でるように触られた場合と、叩かれた場合とで異なるロボット制御を実行することができる。
なお、ここではロボットやマニピュレータの外力検出について説明したが、本発明は、工作機械や車椅子、乗物等の可動体に作用する外力の検出にも広く利用できる。 By covering all the links of the robot with the external force detection device using the shell-shaped cover, it is possible to measure the external force generated at every part of the robot and its action point.
Moreover, since this external force detection apparatus can grasp | ascertain the magnitude | size of external force quantitatively, it can perform different robot control, for example, when it is touched like a stroke and when it is hit.
Although the external force detection of the robot or manipulator has been described here, the present invention can be widely used to detect an external force acting on a movable body such as a machine tool, a wheelchair, or a vehicle.

（実施例１）
箱形状の甲殻型カバーを１個の５軸力覚センサで支持する場合の外力作用点の算出方法の一例について説明する。
図９（ａ）に示すように、箱状甲殻型カバー１１は、ロボット本体４０に取り付けた５軸力覚センサ１２の先端に固定し、この先端をＰ^o（モーメントを求める基準位置）とする。
図９（ｂ）に示すように、箱状甲殻型カバー１１の中心をＰ¹＝（Ｐ¹ _x，Ｐ¹ _y，Ｐ¹ _z）とし、箱状甲殻型カバーの上面の高さをＰ¹ _z+Ｐ^c _zとし、また、前後面の位置をそれぞれＰ¹ _y+Ｐ^c _y, Ｐ¹ _y-Ｐ^c _y、左右面の位置をそれぞれＰ¹ _x+Ｐ^c _x, Ｐ¹ _x-Ｐ^c _xとする。
（数１０）に
Ｐ^dis _z=Ｐ¹ _z+Ｐ^c _z （数１９）
を代入すれば、(数１０）で表される直線が、カバー上面を拡張した水平面と交差する点の座標Ｐ^dis _x、Ｐ^dis _yを下記のように導出できる。
Ｐ^dis _y＝（Ｆ¹ _y／Ｆ¹ _z）[Ｐ^c _z-{(Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y}] （数２０）
Ｐ^dis _x＝（Ｆ¹ _x／Ｆ¹ _z）[Ｐ^c _z-{(-Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x}] （数２１）
ここで、
Ｐ¹ _y-Ｐ^c _y≦Ｐ^dis _y≦Ｐ¹ _y+Ｐ^c _y
Ｐ¹ _x-Ｐ^c _x≦Ｐ^dis _x≦Ｐ¹ _x+Ｐ^c _x （数２２）
Ｆ¹ _z＞0
が全て成り立てば、外力の作用点はカバー上面であると決定できる。その場合、外力作用点は（数１９）（数２０）（数２１）で導出される。
（数２２）が成り立たない場合には、外力が前後面または左右面に作用していると判断されるため、次の手順に移る。 Example 1
An example of a method for calculating an external force action point when a box-shaped shell-type cover is supported by one 5-axis force sensor will be described.
As shown in FIG. 9 (a), a box-like crustaceans type cover 11 is fixed to the distal end of the 5-axis force sensor 12 attached to the robot body 40 to the tip and P ^o (the reference position for determining the moment) .
As shown in FIG. 9B, the center of the box-shaped shell-type cover 11 is P ¹ = (P ¹ _x , P ¹ _y , P ¹ _z ), and the height of the upper surface of the box-shaped shell-type cover is P ^1. _z + P ^c _z , the front and rear plane positions are P ¹ _y + P ^c _y and P ¹ _y -P ^c _y , and the left and right plane positions are P ¹ _x + P ^c _x and P ¹ _x -P, respectively. _Let ^{c be} _x .
P ^dis _z = P ¹ _z + P ^c _z (Equation 19)
Is substituted, the coordinates P ^dis _{x and} P ^dis _y of the point where the straight line represented by (Equation 10) intersects the horizontal plane extending the cover upper surface can be derived as follows.
P ^dis _y = (F ¹ _y / F ¹ _z ) [P ^c _z − {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y }] (Equation 20)
P ^dis _x = (F ¹ _x / F ¹ _z ) [P ^c _z − {(− M ¹ _y −F ¹ _z P ¹ _x ) / F ¹ _x }] (Expression 21)
here,
P ¹ _y -P ^c _y ≦ P ^dis _y ≦ P ¹ _y + P ^c _y
P ¹ _x -P ^c _x ≦ P ^dis _x ≦ P ¹ _x + P ^c _x (Equation 22)
F ¹ _z > 0
If all of the above holds, the point of action of the external force can be determined to be the upper surface of the cover. In this case, the external force action point is derived from (Equation 19), (Equation 20), and (Equation 21).
If (Equation 22) does not hold, it is determined that an external force is acting on the front and rear surfaces or the left and right surfaces, and the process proceeds to the next procedure.

次に、（数１０）の直線とカバー前後面、またはその拡張面が交差した場合を想定する。
Ｆ¹ _y＞０（数２３）
が成り立つ場合にはカバー前面に力が作用していると考え、（数１０）に
Ｐ^dis _y=Ｐ¹ _y+Ｐ^c _y （数２４）
を代入する。すると(数１０）で表される直線が、カバー前面を拡張した鉛直面と交差する点の座標Ｐ^dis _x、Ｐ^dis _zを下記のように導出できる。
Ｐ^dis _z＝｛（Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _y）(Ｐ¹ _y+Ｐ^c _y)
（数２５）
Ｐ^dis _x＝（Ｆ¹ _x／Ｆ¹ _z）[Ｐ^dis _z-｛（−Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x｝-Ｐ¹ _z]
（数２６）
（数２３）が成り立たない場合には、カバー後面に力が作用していると考え、（数１０）に
Ｐ^dis _y=Ｐ¹ _y-Ｐ^c _y （数２７）
を代入する。カバー後面を拡張した鉛直面と交差する点の座標Ｐ^dis _x、Ｐ^dis _zを下記のように導出できる。
Ｐ^dis _z＝｛（Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _y）(Ｐ¹ _y-Ｐ^c _y)
（数２８）
Ｐ^dis _x＝（Ｆ¹ _x／Ｆ¹ _z）[Ｐ^dis _z-｛（−Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x｝-Ｐ¹ _z]
（数２９）
ここで、
Ｐ¹ _z-Ｐ^c _z≦Ｐ^dis _z≦Ｐ¹ _z+Ｐ^c _z
Ｐ¹ _x-Ｐ^c _x≦Ｐ^dis _x≦Ｐ¹ _x+Ｐ^c _x （数３０）
が成り立てば、外力の作用点はカバー前後面であると決定できる。その場合、外力作用点は（数２８）（数２９）（数３０）で導出される。
（数３０）が成り立たない場合には、外力が左右面に作用していると判断されるため、次の手順に移る。 Next, it is assumed that the straight line of (Equation 10) intersects with the front and rear surfaces of the cover, or the extended surface.
F ¹ _y > 0 (Equation 23)
When the above holds, it is considered that a force is acting on the front surface of the cover, and P ^dis _y = P ¹ _y + P ^c _y (Equation 24)
Is assigned. Then, the coordinates P ^dis _{x and} P ^dis _z of the point where the straight line represented by (Equation 10) intersects the vertical plane extending the front surface of the cover can be derived as follows.
P ^dis _z = {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y } + P ¹ _z + (F ¹ _z / F ¹ _y ) (P ¹ _y + P ^c _y )
(Equation 25)
P ^dis _x = (F ¹ _x / F ¹ _z ) [P ^dis _z − {(− M ¹ _y −F ¹ _z P ¹ _x ) / F ¹ _x } −P ¹ _z ]
(Equation 26)
If (Equation 23) does not hold, it is considered that a force is acting on the rear surface of the cover, and P ^dis _y = P ¹ _y -P ^c _y (Equation 27)
Is assigned. The coordinates P ^dis _{x and} P ^dis _z of the point that intersects the vertical plane extending the rear surface of the cover can be derived as follows.
P ^dis _z = {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y } + P ¹ _z + (F ¹ _z / F ¹ _y ) (P ¹ _y −P ^c _y )
(Equation 28)
P ^dis _x = (F ¹ _x / F ¹ _z ) [P ^dis _z − {(− M ¹ _y −F ¹ _z P ¹ _x ) / F ¹ _x } −P ¹ _z ]
(Equation 29)
here,
^{_{^{_{P 1 z -P c z ≦ P}}}} dis z ≦ P 1 z + P c z
P ¹ _x -P ^c _x ≦ P ^dis _x ≦ P ¹ _x + P ^c _x (Equation 30)
Is established, the point of action of the external force can be determined to be the front and back surfaces of the cover. In that case, the external force action point is derived by (Equation 28) (Equation 29) (Equation 30).
If (Equation 30) does not hold, it is determined that an external force is acting on the left and right sides, and the process proceeds to the next procedure.

次に（数１０）の直線とカバー左右面、またはその拡張面が交差した場合を想定する。
Ｆ¹ _x＞０（数３１）
が成り立つ場合にはカバー前面に力が作用していると考え、（数１０）に
Ｐ^dis _x=Ｐ¹ _x+Ｐ^c _x （数３２）
を代入する。すると(数１０）で表される直線が、カバー前面を拡張した鉛直面と交差する点の座標Ｐ^dis _y、Ｐ^dis _zを下記のように導出できる。
Ｐ^dis _z＝｛（−Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _x）(Ｐ¹ _x+Ｐ^c _x)
（数３３）
Ｐ^dis _y＝（Ｆ¹ _y／Ｆ¹ _z）[Ｐ^dis _z-{（Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y｝-Ｐ¹ _z]
（数３４） Next, it is assumed that the straight line of (Equation 10) intersects with the cover left and right surfaces or its extended surface.
F ¹ _x > 0 (Equation 31)
When the above holds, it is considered that a force is acting on the front surface of the cover, and P ^dis _x = P ¹ _x + P ^c _x (Equation 32)
Is assigned. Then, the coordinates P ^dis _{y and} P ^dis _z of the point where the straight line represented by (Equation 10) intersects the vertical plane extending the front surface of the cover can be derived as follows.
P ^dis _z = {(− M ¹ _y −F ¹ _z P ¹ _x ) / F ¹ _x } + P ¹ _z + (F ¹ _z / F ¹ _x ) (P ¹ _x + P ^c _x )
(Expression 33)
P ^dis _y = (F ¹ _y / F ¹ _z ) [P ^dis _z − {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y } −P ¹ _z ]
(Equation 34)

（数３１）が成り立たない場合には、カバー後面に力が作用していると考え、（数１０）に
Ｐ^dis _x=Ｐ¹ _x-Ｐ^c _x （数３４）
を代入する。カバー後面を拡張した鉛直面と交差する点の座標Ｐ^dis _y、Ｐ^dis _zを下記のように導出できる。
Ｐ^dis _z＝｛（−Ｍ¹ _y−Ｆ¹ _zＰ¹ _x）／Ｆ¹ _x｝＋Ｐ¹ _z＋（Ｆ¹ _z／Ｆ¹ _x）(Ｐ¹ _x-Ｐ^c _x)
（数３５）
Ｐ^dis _y＝（Ｆ¹ _y／Ｆ¹ _z）[Ｐ^dis _z-{（Ｍ¹ _x−Ｆ¹ _zＰ¹ _y）／Ｆ¹ _y｝-Ｐ¹ _z]
（数３６）
ここで、
Ｐ¹ _z-Ｐ^c _z≦Ｐ^dis _z≦Ｐ¹ _z+Ｐ^c _z
Ｐ¹ _y-Ｐ^c _y≦Ｐ^dis _y≦Ｐ¹ _y+Ｐ^c _y （数３７）
が成り立てば、外力の作用点はカバー左右面であると決定できる。
それ以外の場合は原理的に存在しないため、例外処理として接触位置不定とする。 If (Equation 31) does not hold, it is considered that a force acts on the rear surface of the cover, and P ^dis _x = P ¹ _x -P ^c _x (Equation 34)
Is assigned. The coordinates P ^dis _{y and} P ^dis _z of the point that intersects the vertical plane extending the cover rear surface can be derived as follows.
P ^dis _z = {(− M ¹ _y −F ¹ _z P ¹ _x ) / F ¹ _x } + P ¹ _z + (F ¹ _z / F ¹ _x ) (P ¹ _x −P ^c _x )
(Equation 35)
P ^dis _y = (F ¹ _y / F ¹ _z ) [P ^dis _z − {(M ¹ _x −F ¹ _z P ¹ _y ) / F ¹ _y } −P ¹ _z ]
(Equation 36)
here,
^{_{^{_{P 1 z -P c z ≦ P}}}} dis z ≦ P 1 z + P c z
P ¹ _y −P ^c _y ≦ P ^dis _y ≦ P ¹ _y + P ^c _y (Equation 37)
Is established, the point of action of the external force can be determined to be the left and right sides of the cover.
In other cases, the contact position is indefinite as exception processing because there is no principle.

（実施例２）
箱形状の甲殻型カバーを１個の３軸力覚センサと２個の１軸力覚センサとを用いて支持する場合は、図１０（ａ）に示すように、３軸力覚センサ２５により甲殻型カバー１１の上面内面を支持し、１軸力覚センサ２６、２７により甲殻型カバー１１の側面内面を支持する。図１０（ｂ）に示すように、３軸力覚センサ２５は、ｘ，ｙ軸周りに自由に回転するユニバーサルジョイント４１を介して甲殻型カバー１１に固定する。また、１軸力覚センサ２６、２７は、ｘ，ｙ，ｚ軸周りに自由に回転するユニバーサルジョイント４３、４４を介してスライドガイド４２、４４に固定し、スライドガイド４２、４４を甲殻型カバー１１の内面に当接させる。甲殻型カバー１１のｘ軸に平行な側面に当接するスライドガイド４２は、ｘ，ｚ軸方向に移動可能であり、また、甲殻型カバー１１のｙ軸に平行な側面に当接するスライドガイド４４は、ｙ，ｚ軸方向に移動可能である。
また、３軸力覚センサ２５の先端をＰ^o（モーメントを求める基準位置）とする。 (Example 2)
When the box-shaped shell-type cover is supported by using one three-axis force sensor and two one-axis force sensors, the three-axis force sensor 25 is used as shown in FIG. The inner surface of the upper surface of the shell type cover 11 is supported, and the inner surface of the side surface of the shell type cover 11 is supported by the uniaxial force sensors 26 and 27. As shown in FIG. 10B, the triaxial force sensor 25 is fixed to the crustacean cover 11 via a universal joint 41 that freely rotates around the x and y axes. The uniaxial force sensors 26 and 27 are fixed to the slide guides 42 and 44 via universal joints 43 and 44 that freely rotate around the x, y, and z axes. 11 is brought into contact with the inner surface. The slide guide 42 that contacts the side surface parallel to the x axis of the crustacean cover 11 is movable in the x and z axis directions, and the slide guide 44 that contacts the side surface parallel to the y axis of the crustace cover 11 , Y and z axis directions are movable.
The tip of the three-axis force sensor 25 is defined as P ^o (reference position for obtaining moment).

この外力検出装置では、３軸力覚センサ２５が計測する力をｆ₁＝（ｆ_1x,ｆ_1y,ｆ_1z）、１軸力覚センサ２７が計測する力をｆ₂、１軸力覚センサ２６が計測する力をｆ₃とすると、外力Ｆ^disの各成分は次のようになる。
Ｆ^dis _x＝ｆ_1x＋ｆ₂
Ｆ^dis _y＝ｆ_1y＋ｆ₃
Ｆ^dis _z＝ｆ_1z
また、図１０（ｂ）に示すように、甲殻型カバー１１の上面から１軸力覚センサ２６、２７までの距離をＰ_zとすると、各力覚センサの測定値から求められるモーメントＭ⁰の成分Ｍ⁰ _x、Ｍ⁰ _yは次のようになる。
Ｍ⁰ _x＝ｆ₃Ｐ_z
Ｍ⁰ _y＝ｆ₂Ｐ_z
なお、Ｍ⁰ _zは計測しない（外力作用点の導出には必要がない）。
このＭ⁰を（数１５）に代入し、（数６）〜（数１０）のように展開することによって外力の作用点Ｐ^disを算出することができる。 In this external force detection device, the force measured by the three-axis force sensor 25 is f ₁ = (f _1x , f _1y , f _1z ), the force measured by the one-axis force sensor 27 is f ₂ , and the one-axis force sensor. When the force 26 measures is f ₃ , each component of the external force F ^dis is as follows.
F ^dis _x = f _1x + f ₂
F ^dis _y = f _1y + f ₃
F ^dis _z = f _1z
Further, as shown in FIG. 10B, when the distance from the upper surface of the crustacean type cover 11 to the uniaxial force sensors 26 and 27 is P _z , the moment M ⁰ obtained from the measured value of each force sensor is The components M ⁰ _x and M ⁰ _y are as follows.
M ⁰ _x = f ₃ P _z
M ⁰ _y = f ₂ P _z
Note that M ⁰ _z is not measured (not necessary for deriving the external force action point).
By substituting this M ⁰ into (Equation 15) and developing it as shown in (Equation 6) to (Equation 10), the action point P ^{dis of the} external force can be calculated.

本発明は、ロボットやマニピュレータ、工作機械、車椅子、乗物等の可動体に作用する外力を検出するために広く利用することができる。 The present invention can be widely used to detect external forces acting on movable bodies such as robots, manipulators, machine tools, wheelchairs, and vehicles.

本発明の実施形態における移動ロボット及び甲殻型カバーを示す図The figure which shows the mobile robot and crustacean cover in embodiment of this invention 図１の甲殻型カバーに作用する力の説明図Explanatory drawing of the force which acts on the shell type cover of FIG. 本発明の実施形態におけるマニピュレータに多点支持された甲殻型カバーを示す図The figure which shows the shell-type cover supported by the manipulator in the embodiment of this invention multipoint 図３の甲殻型カバーに作用する力の説明図Explanatory drawing of the force which acts on the shell type cover of FIG. 本発明の実施形態におけるワイヤでロボット本体に接続された甲殻型カバーを示す図The figure which shows the shell-shaped cover connected to the robot main body with the wire in embodiment of this invention 実験に用いた移動ロボット及び甲殻型カバーを示す図Diagram showing mobile robot and crustacean cover used in the experiment 甲殻型カバーを手で押圧して作用点を画面に表示する実験を示す図The figure which shows the experiment which displays the point of action on the screen by pushing the crustace type cover with the hand 甲殻型カバーの押圧位置と算出された作用点との関係を示す図The figure which shows the relationship between the pressed position of the crustacean type cover and the calculated action point 実施例１の甲殻型カバーの支持について説明する図The figure explaining the support of the shell type cover of Example 1 実施例２の甲殻型カバーの支持について説明する図The figure explaining the support of the shell type cover of Example 2

Explanation of symbols

１０移動ロボット
１１甲殻型カバー
１２６軸力覚センサ
２０マニピュレータ
２１甲殻型カバー
２２力覚センサ
２３力覚センサ
２４力覚センサ
２５３軸力覚センサ
２６１軸力覚センサ
２７１軸力覚センサ
３０甲殻型カバー
３１ワイヤ
３２ワイヤ
３３ワイヤ
３４ワイヤ
３５ワイヤ
４０ロボット本体
４１ユニバーサルジョイント
４２スライドガイド
４３ユニバーサルジョイント
４４スライドガイド
４５ユニバーサルジョイント DESCRIPTION OF SYMBOLS 10 Mobile robot 11 Crustaceal cover 12 6-axis force sensor 20 Manipulator 21 Crustaceal cover 22 Force sensor 23 Force sensor 24 Force sensor 25 3-axis force sensor 26 1-axis force sensor 27 1-axis force sensor 30 Crustacean cover 31 Wire 32 Wire 33 Wire 34 Wire 35 Wire 40 Robot body 41 Universal joint 42 Slide guide 43 Universal joint 44 Slide guide 45 Universal joint

Claims

A detection method for detecting the magnitude and point of action of an external force acting on a movable body,
The movable body is covered with a shell-shaped cover having rigidity, and the shell-shaped cover and the movable body are connected by one or a plurality of connecting bodies that are connected to the shell-shaped cover at a single point . Measuring means for measuring stress or moment acting on a connection point with the crustace cover is provided, and five types of data on the geometric shape of the crustace cover and stress or moment measured by the one or more measurement means are provided. The magnitude of the external force F ^dis and the action point P ^dis are calculated by the following formula using the measured values of the above and using the fact that the calculated action point P ^dis exists on the geometric shape of the shell-shaped cover. An external force detection method characterized by specifying a position of an action point P ^dis .
F ^dis + F ^sum = 0
F ^dis × (P ^dis −P ^o ) + M ^o = 0
F ^sum = ΣF ^j
M ^o = Σ {F ^j × (P ^j −P ^o )} + ΣM ^j
(Where F ^j is a measured value of stress acting on the j-th connected body measured by the measuring means, and M ^j is a measured value of moment acting on the j-th connected body measured by the measuring means. , P ^j is the position vector of the j-th connected body, P ^o is the position vector of the reference position for obtaining the moment, F ^sum is the ^sum of the stresses measured by all of the measuring means, and M ^o is the above-mentioned (The sum of moments measured by all of the measuring means, and when m is the number of connected bodies, Σ indicates an addition from j = 1 to m.)

2. The external force detection method according to claim 1, wherein the number of the connection bodies is set to 1, and the connection body also serves as the measurement unit, and the stress component in the triaxial direction and the moment component around the two axes. Or a sensor capable of measuring a stress component in the biaxial direction and a moment component around the three axes, and calculating the magnitude of the external force F ^dis and the point of action P ^dis by the following equation: To detect external force.
F ^dis + F1 = 0
F ^dis × (P ^dis −P ¹ ) + M ¹ = 0
(However, F ¹ is a measured value of stress measured by the sensor, M ¹ is a measured value of moment measured by the sensor, and P ¹ is a position vector of the sensor.)

2. The external force detection method according to claim 1, wherein the number of the connection bodies is set to 2 or more, and the plurality of connection bodies also serve as the measurement unit, and the stress component in one or more directions or one axis. A plurality of sensors capable of measuring moment components around the above axes, wherein the total number of measurable stress component axes and measurable moment component axes of the plurality of sensors is five axes . A method of detecting an external force, wherein a sensor is used to calculate the magnitude of the external force F ^dis and the action point P ^dis by the above formula.

2. The external force detection method according to claim 1, wherein five wires are used as the connection body , the tension of the wire is measured by the measuring means, and ^Fsum and ^Mo of the formula are calculated by the following formulas. An external force detection method characterized by:
F ^sum = Σ (d ^j / | d ^j |) × (f ^j −f ^oj )
M ^o = Σ p ^j × (d ^j / | d ^j |) × (f ^j −f ^oj )
(Where p ^j is the end point position of the j-th wire on the movable body side, d ^j is the end point position of the j-th wire on the crust-type cover side with reference to p ^j , and f ^j is the measuring means The measured value of the tension of the j-th wire measured in step ^foj is the initial value of the tension of the j-th wire when no external force is applied, and when the number of wires is m, Σ is (The addition from j = 1 to m is shown.)

An external force detection device that detects the magnitude and point of an external force acting on a movable body,
A shell-shaped cover having rigidity to cover the movable body;
One or a plurality of connecting bodies connecting the 該甲shelled cover and the movable body are connected at one point to the crustacean type cover,
Measuring means for measuring a stress or moment acting on a connection point of the connection body with the crustace cover ;
With
The magnitude of the external force F ^dis by the detection method according to claim 1, using the geometric data of the crustacean cover and five types of measured values related to stress or moment measured by the one or more measuring means. And the point of action P ^dis is calculated, and the position of the point of action P ^dis is specified using the fact that the calculated point of action P ^dis exists on the geometric shape of the shell-shaped cover. .

6. The external force detection device according to claim 5, wherein the connecting body also serves as the measuring unit, and is a sensor capable of measuring a stress component in three axes and a moment component around two axes , or a stress component in two axes. And an external force detection device comprising a sensor capable of measuring moment components about three axes.

The external force detection device according to claim 5, wherein the connection body also serves as the measurement unit, and a plurality of sensors capable of measuring a stress component in one or more axes or a moment component around one or more axes. An external force detection device comprising a plurality of sensors in which the total number of measurable stress component axes and measurable moment component axes of the plurality of sensors is five .

It is an external force detection apparatus of Claim 5, Comprising: The said connection body consists of five wires, The external force detection apparatus characterized by the above-mentioned.

The external force detection device according to claim 5, wherein the movable body is a robot.

The external force detection device according to claim 5, wherein the movable body is a manipulator.

The external force detection device according to claim 5, wherein the movable body is a machine tool.

The external force detection device according to claim 5, wherein the movable body is a wheelchair.

The external force detection device according to claim 5, wherein the movable body is a vehicle.