JPS62212702A - Robot track deciding device - Google Patents

Abstract

PURPOSE:To decide a robot track with high accuracy by using an accelerometer to measure the acceleration of each link and adding a specific arithmetic function to a controller. CONSTITUTION:A means 15 for detecting the angular velocity of revolution and an accelerometer package 16 are provided at the part of a link 3. The package 16 contains accelerometers set for each of the three mutually orthogo nal axes (X3-Z3) which can deliver signals corresponding to the accelerations (ax-az) of movable links 1-3 respectively. In addition reference rotational angle detecting means 41-43 are provided, and the signals (omegax-omegaz) received from a gyropackage 15, signals (ax-az) from the package 16, and signals (psi41, theta42 and phi43) from the reference rotational angle means 41-43 are introduced to a controller 20 for operating. With the above arrangement the coordinates of the tip of a robot arm can be determined exactly, which permits accurate working for the robot.

Description

【発明の詳細な説明】 イ、「発明の目的」 （産業上の利用分野） 本発明は、回転する３つのリンクを持つ３自由度のロボ
ットアームの先端座標値を求めるロボット用軌道決定装
置に間するものである。DETAILED DESCRIPTION OF THE INVENTION A. Object of the Invention (Industrial Field of Application) The present invention is directed to a robot trajectory determining device for determining the tip coordinates of a three-degree-of-freedom robot arm having three rotating links. It is something that will take place.

（従来の技術〉 ロボットは、通常、コンピュータの命令に従って、アー
ムを動かし、各種の作業を行なうが、このアームがコン
ピュータの命令通りの空間的位置に移動しているか否か
を監視することが必要である。そうでないと、期待する
作業な行なうことができないからである１本明細書で論
じる装置は、このロボットアームの現在位置を把握する
装置である。更に詳しく述べると、昭和６１年３月４日
に出願した「ロボットアーム姿勢測定ｉ＊ｔｌＪ　　（
以下先願と記す）を利用して、アームの先端座標値を正
確に決定することができる装置である。(Prior art) Robots usually move their arms and perform various tasks according to instructions from a computer, but it is necessary to monitor whether the arms are moving to spatial positions as instructed by the computer. Otherwise, the expected work cannot be performed.1 The device discussed in this specification is a device for grasping the current position of this robot arm.More specifically, in March 1988 “Robot Arm Posture Measurement i*tlJ (
This is a device that can accurately determine the coordinate value of the tip of the arm using the prior application (hereinafter referred to as the earlier application).

第３図を用いて従来のロボット用軌道決定装置を説明す
る。第３図は３つの自由度を持つロボットアームを示し
たものである。同図において、リンク０は、ロボットア
ームを台座に固定する部材であり、その座標系をくＺｏ
　、’Ｉｏ　＋　Ｚｏ　）とする。このリンク０は回転
しない。A conventional robot trajectory determination device will be explained using FIG. Figure 3 shows a robot arm with three degrees of freedom. In the figure, link 0 is a member that fixes the robot arm to the pedestal, and its coordinate system is
, 'Io + Zo). This link 0 does not rotate.

リンク１の部材は、モータ４の駆動力によりリンクＯの
１軸ＣＺｏ軸）回りを回転（回転角Ｖ）するもので、そ
の座標系を（χＩ　＋　Ｖ　Ｉ　＋　Ｚ　Ｉ　＞とする
。The members of the link 1 are rotated (rotation angle V) around one axis of the link O (CZo axis) by the driving force of the motor 4, and the coordinate system thereof is set as (χI + V I + Z I >).

リンク２の部材は、モータ５の駆動力によりリンク１０
２軸（ｙ＋軸）回りを回転（回転角θ）するもので、そ
の座標系を（χ２１　ｙ２　＋　Ｚ２　）とする。The members of the link 2 are moved to the link 10 by the driving force of the motor 5.
It rotates (rotation angle θ) around two axes (y+axis), and its coordinate system is (χ21 y2 + Z2).

リンク３の部材は、モータ６の駆動力によりリンク２の
３軸（χ２軸）回りを回転（回転角φ）するもので、そ
の座標系を（Ｘコ＋　ｙ３　＊　Ｚ３　）とする。The members of the link 3 are rotated (rotation angle φ) around the three axes (χ2 axis) of the link 2 by the driving force of the motor 6, and the coordinate system thereof is (Xco+y3*Z3).

第３図のロボットは、以上の３つのモータ４゜５．６に
よりリンク１，２．３を回転させ、その回転色事、θ、
φを制御回路（図示せず）で指令することにより任意の
位置にアームの先端部を移動させることができる。The robot shown in Fig. 3 rotates links 1, 2.3 using the three motors 4°5.6, and the rotational changes θ,
By commanding φ using a control circuit (not shown), the tip of the arm can be moved to an arbitrary position.

従来は、回転角ψ、θ、φを知るために各モータ４，５
．６の軸にパルス発生器く図示せず）を取付けている。Conventionally, in order to know the rotation angles ψ, θ, φ, each motor 4, 5
．． A pulse generator (not shown) is attached to the shaft of 6.

このパルス発生器ば、各モータ４゜５．６が一定角度、
正回転又は負回転するごとにアップ（ｕｐ）パルス又は
ダウン（ｄｏｗｎ）パルスを出力する。そして、これを
パルス／デジタル変換器８，９．１０でデジタル信号に
変換して軍。With this pulse generator, each motor 4°5.6 is at a constant angle,
An up pulse or a down pulse is output every time the motor rotates in a positive direction or in a negative direction. This is then converted into a digital signal by pulse/digital converters 8, 9 and 10 and used by the military.

θ、φを求めている。We are looking for θ and φ.

そして、この東、θ、φを基にして、次の演算を行なう
ことによりアーム先端の座標値を決めている。Then, based on the east, θ, and φ, the coordinate value of the tip of the arm is determined by performing the following calculation.

第３図において、リンクＯの半径をｒＱ、リンク２のア
ームの長さを’２＋　リンク３のアームの長さをｒ３と
する。なお、リンク１はリンク２を回転させるためのも
のである。In FIG. 3, the radius of link O is rQ, the arm length of link 2 is '2+', and the arm length of link 3 is r3. Note that the link 1 is for rotating the link 2.

ここで、リンク１の座標系からリンクＯの座標系へ変換
する座標変換行列Ｃ１は、一般に（ａｌ）式で表わすこ
とができる。Here, the coordinate transformation matrix C1 that transforms the coordinate system of link 1 to the coordinate system of link O can generally be expressed by equation (al).

また、リンク２の座標系からリンク１の座標系へ変換す
る座標変換行列Ｃ１１は、一般に（ａ２）式で表わすこ
とができる。Further, the coordinate transformation matrix C11 for transforming the coordinate system of link 2 to the coordinate system of link 1 can generally be expressed by equation (a2).

また、リンク３の座標系からリンク２の座標系へ変換す
る座標変操行列Ｃ；は、一般に（ａ３）式で表わすこと
ができる。Further, a coordinate transformation matrix C for converting the coordinate system of link 3 to the coordinate system of link 2 can generally be expressed by equation (a3).

以上から、リンク３の座標系からリンク０の座標系へ変
換する座標変換行列Ｃ０は（ａ４）式で表わすことがで
きる。From the above, the coordinate transformation matrix C0 for transforming the coordinate system of link 3 to the coordinate system of link 0 can be expressed by equation (a4).

・・・　（ａ４） リンクＯの座標系（ＺＯ＋　’／　Ｏ＊　ＺＯ）の原点
からリンク３の座標系（ｘ３　、　！／３　＋　Ｚ３　
）の原点までの距離ベクトルＰｏｓは（ａ５）式で表わ
すことができる。... (a4) From the origin of link O's coordinate system (ZO+ '/O* ZO) to link 3's coordinate system (x3, !/3 + Z3
) can be expressed by equation (a5).

ＰＯ３−ｒｏ　ｅｘｔ＋ｒ２ｔｘｚ＋ｒ”３ｅｘ３　　
　　（ａ５）ここで、ｅ　ｘｒ＊　ｅ　ＸＺＩ　ｅ　Ｘ
３は、それぞれれリンク１の座標系（χ＋　＋　Ｖ＋　
、Ｚ＋　）のχ１軸、リンク２の座標系（！２ｒ　’１
２＊　Ｚ２　）のｘ２軸、リンク３の座標系（χ３　＋
　Ｖ３　＋　Ｚ３　）のχ３軸、に関する単位ベクトル
である。ま・た、ｅχＢｅｘｚ。PO3-ro ext+r2txz+r”3ex3
(a5) Here, e xr* e XZI e X
3 is the coordinate system of link 1 (χ+ + V+
, Z+ )'s χ1 axis, link 2's coordinate system (!2r '1
2 * Z2 ) x2 axis, link 3 coordinate system (χ3 +
It is a unit vector regarding the χ3 axis of V3 + Z3). Ma-ta, eχBexz.

ｅＸＪは、（ａｌ）〜（ａ４）式によりリンクＯの座標
系（χｇ　＋　’１０　＋　ｚｏ　）に変換することが
できる。eXJ can be converted into the coordinate system (χg + '10 + zo) of link O using equations (al) to (a4).

ｔｘｔ−ｅｘｏａ　ＣＯ５ｖ＋ｔ’ｙ、ｅ　ｓｉｎ　　
’４１　　　　　　　　（ａ６）ｅ　ｘｚ−ｅ　ｘｏ　
ΦＣＯ５ｖ串ＣＯ３θ＋　ｓｉｎ　ｖ　−ｃｏｓθ・ｅ
ｙｏ−ｆ、ｒＯ−ｓｉｎθ　（ａｌ）ｅ　：ｃ３−　ｅ
　Ｘｌ）−ＣＯ３ｖ”　ＣＯ５θ＋　ｓｉｎ　１−１／
　−ｃｏｓ　θ・ｅｙｏ−ｅ、ｒｏ−３ｉｎ　θ　　（
ａ８）従って、Ｐ　０３は（ａ９）式で表わすことがで
きる。txt-exoa CO5v+t'y,e sin
'41 (a6)e xz-e xo
ΦCO5v skewer CO3θ+ sin v −cosθ・e
yo-f, rO-sinθ (al)e:c3-e
Xl)-CO3v” CO5θ+ sin 1-1/
-cos θ・eyo-e, ro-3in θ (
a8) Therefore, P 03 can be expressed by formula (a9).

ｐ　０３　−　　（ｒ　　ｏ　ｃｏｓ　　ｖ　＋ｒ２　
　ｃｏｓ　　１４／ｃｏｓ　　θ＋　ｒ　３　ｃｏｓ　
Ｖｃｏｓθ）ｅｘ。p 03 − (ro cos v + r2
cos 14/cos θ+ r 3 cos
Vcosθ)ex.

＋　（ｒ（、ｓｔｎ　ｖ＋　ｒ２　ｓｔｎ　Ｖｃｏｓθ
＋　ｒ＝　ｓｉｎ　Ｖｃｏｓθ＞ｅｙ。+ (r(, stn v+ r2 stn Vcosθ
+r=sin Vcosθ>ey.

＋（−ｒ２ｓｉｎθ−ｒ３ｓｉｎθ）ｆｚ。+(-r2sinθ-r3sinθ)fz.

・・・（ａ９） 従って、リンク３の先端の位置、即ち座標系〈χ３　＋
　ｙ３　＋　Ｚ３　）の原点は次のように表わすことが
できる。...(a9) Therefore, the position of the tip of link 3, that is, the coordinate system <χ3 +
The origin of y3 + Z3) can be expressed as follows.

ｐ、、　−ｒ’Ｑ　ＣＯ３ｖ＋　ｒ２ＣＯ３ｖＣＯ８θ
＋　ｒ　３　ｃｏｓ　ｖｃｏｓ　　θ ・・・（ａ　１０） ｐ、、　−ｒＯｓｉｎ　Ｖ＋　ｒ２ｓｉｎ　！／ｃｏｓ
　θ＋　ｒ３ｓｉｎ　ｖｃｏｓθ ・・−（ａｌｌ） ｐ、、−−（ｒ２＋ｒ３　）ｓｉｎ　θ　　　　　−（
ａ　１２）ここでθは第１図で示された方向を負にとっ
ているので、リンク２．リンク３が上に持上がるときｐ
　　−−（ｒ２　＋ｒ３）ｓｉｎθ＞Ｑ　　　　（ａ１
３）となる。p,, -r'Q CO3v+ r2CO3vCO8θ
+ r 3 cos vcos θ ... (a 10) p,, -rOsin V+ r2sin ! /cos
θ+ r3sin vcosθ ・・−(all) p,,−(r2+r3)sin θ−(
a12) Here, θ is negative in the direction shown in FIG. 1, so link 2. When link 3 is lifted up, p
--(r2 +r3) sin θ>Q (a1
3).

〔発射が解決しようとする問題点〕[Problems that launch attempts to solve]

しかし、以上のような手段は、位置の測定精度に改善の
余地がある。However, with the above means, there is room for improvement in position measurement accuracy.

上述より第３図装置の位置決定の精度は、アームの長さ
’Ｏｒ　’２　Ｔ　’３の不正確さをΔｒｏ＋Δ’２＋
Δｒ３とし、パルス発生器の誤差を八ψ。From the above, the accuracy of positioning of the device in Figure 3 is determined by the inaccuracy of the arm length 'Or '2 T '3 as Δro + Δ'2 +
Δr3, and the error of the pulse generator is 8ψ.

Δθ、Δφとすると、次式で求まる。Letting Δθ and Δφ, it can be found by the following formula.

＋Δｒ３ＣＯ８Ｖｃｏｓθ） △　、△　△　、ハ　　△ −（ｒｏ　Ｓｌｎ　Ｖ＋　ｒ２　ｓｉｎ　Ｖｃｏｓθハ
、△ハ ＋　ｒ　３ｓｉｎ　、、Ｍ／ｃｏｓθ）Δ軍−（ｒ２ｃ
ｏｓ　Ｖｓｉｎθ ハ、ハ ＋　ｒ　３　ＣＯ３Ｖｓ＋ｎθ）Δθ ・・・（ａ１４） さ０ Δｐ０３Ｍ＝（Δｒｏ　ｓｉｎ　ｖ＋Δｒ２ｓｉｎ　Ｖ
ｃｏＳ　　θ△　　△ ＋　　（ｒｏ　　ＣＯ５ｖ＋　ｒ２　ｃｏｓ　　ｖｃｏ
ｓ　　θ＋　ｒ　３　　ｃｏｓ　　ｖｃｏｓ　　θ）６
里、　／＼　、　／＼ −（ｒ２　Ｓｉｎ　　Ｖｓ＋ｎ　　θ ＋全３　ｓｉｎ　Ｑｓｉｎ　名Ａ　／３・・・　（ａ　
　１５） −（ｒ２　　＋ｒ３　）ｃｏｓ　　θ　・Δθ・・・　
（ａ　　１６） ここでハ印は、誤差〈不正確さ）を含まない値の意味で
ある。+Δr3CO8Vcosθ) △ , △ △ , C △ − (ro Sln V+ r2 sin Vcos θ Ha, △ Ha + r 3 sin ,, M/cos θ) Δ army − (r2c
os Vsinθ Ha, Ha + r 3 CO3Vs+nθ)Δθ...(a14) Sa0 Δp03M=(Δro sin v+Δr2sin V
coS θ△ △ + (ro CO5v+ r2 cos vco
s θ+ r 3 cos vcos θ)6
Sato, /\ , /\ -(r2 Sin Vs+n θ +Total 3 sin Qsin Name A /3... (a
15) −(r2 +r3) cos θ ・Δθ...
(a 16) Here, the symbol C means a value that does not include error (inaccuracy).

以上から従来装置の問題点を整理すると次の通りである
。From the above, the problems of the conventional device can be summarized as follows.

（１）　　リンクＯの半径、リンク２．リンク３の長さ
測定の不正確さによる誤差が位置測定精度に影響する。(1) Radius of link O, link 2. Errors due to inaccuracies in the length measurement of the link 3 affect the position measurement accuracy.

（２）　　パルス発生器に起因する回転角誤差が位置測
定精度に影響する。(2) Rotation angle errors caused by the pulse generator affect position measurement accuracy.

（３）位置誤差を修正する機能がない。(3) There is no function to correct position errors.

ここで位置誤差を具体的に述べると、例えば、Δ’２”
１Ｃ１Δｒ３−１Ｃ１１１，θ−−４５°。To specifically describe the position error here, for example, Δ'2"
1C1Δr3−1C111, θ−−45°.

Δθ−０，０２’の時、 Δｐ、３−−２　ｘｓｉｎ　　（−４５°）　−２Ｘ　
（ｒ２＋　ｒ３　）　ｃｏｓ　　（−４５°）　Ｘ　０
９０２　Ｘ　Ｏ，０１７−１，４（１−０，０００４８
ｘ　（ｒ　２　＋　ｒ　３　）　ＣＩｇｔ＼　　　　／
＼ ここで、ｒ２＋ｒ３−１５０Ｃ１１とするとΔｌ）、、
　−１，４ＣＩ　−０，０７ＣＩＩｚ　　１，４ｃ１と
なる。このように２０方向の誤差１．４Ｃ１１は、高精
度な位置決定の作業の際には、大きな問題となる。先願
０発明は、上記（２）項の“パルス発生器に起因する回
転角誤差”を改善したものであるが、本発明はこの先願
の発明を利用し、更にリンク１゜２．３の回転角の測定
に止まらず、ロボットアームの先端位置座標値を得るよ
うにしたものである。When Δθ-0,02', Δp, 3--2 xsin (-45°) -2X
(r2+r3) cos (-45°) X 0
902 X O,017-1,4(1-0,00048
x (r 2 + r 3 ) CIgt\ /
\Here, if r2+r3-150C11, Δl),,
-1,4CI -0,07CIIz 1,4c1. As described above, the error of 1.4C11 in 20 directions becomes a big problem when performing highly accurate position determination work. The invention of the prior application 0 improves the "rotation angle error caused by the pulse generator" in item (2) above, but the present invention utilizes the invention of the prior application and further improves the link 1゜2.3. The system is designed to not only measure the rotation angle but also obtain the coordinate values of the tip position of the robot arm.

本発明の目的は、高精度でロボットアームの先端位置の
測定を行なうことができるロボット用軌道決定装置を提
供することである。An object of the present invention is to provide a robot trajectory determination device that can measure the tip position of a robot arm with high precision.

口、「発明の構成」 （問題点を解決するための手段） 本発明は、上記問題点を解決するために回転する３つの
リンクを持つロボットアームと、各リンクが一定角度回
転するごとに信号を出力する基準回転角検出手段（４１
，４２，４３）と、各リンクの回転角速度を検出する手
段（１５）と、前記回転する３つのリンクの回転角（ｖ
、θ。``Structure of the Invention'' (Means for Solving the Problems) In order to solve the above-mentioned problems, the present invention provides a robot arm having three rotating links, and a signal that is sent each time each link rotates by a certain angle. Reference rotation angle detection means (41
, 42, 43), a means (15) for detecting the rotational angular velocity of each link, and a means (15) for detecting the rotational angular velocity of the three rotating links (v
, θ.

φ）に対応した信号を演算するＩｌｌ　ｍ装置とを備え
た装置において、 各リンクの加速度を計測する加速度計（１６）を具備し
、 前記１１Ｊ御ａ置が以下の演算機能（Ａ＞をも備えるよ
うにしたものである。The device is equipped with an accelerometer (16) that measures the acceleration of each link, and the 11J Oki also has the following calculation function (A>). This is something I prepared for.

（Ａ）　　制御Ｉ装置の演算機能 前記加速度計（１６）から信号を導入し、この信号を２
回積分してロボットアームの端部にあるリンク３の先端
位置座標値を弾出する演算機能と、ロボットアームが停
止した時に加速度計誤差（Ａａ３）の推定値を演算する
機能。(A) Arithmetic function of control I device Introduces a signal from the accelerometer (16) and converts this signal into 2
A calculation function that calculates the coordinate value of the tip position of the link 3 at the end of the robot arm by integrating it once, and a function that calculates the estimated value of the accelerometer error (Aa3) when the robot arm stops.

（実施例） 以下、図面を用いて本鑓明を詳しく説明する。(Example) Hereinafter, Honyamyo will be explained in detail using drawings.

第１図は、本発明の一実施例を示したブロック図である
。また、第２図は第１図装置の動作フＤ−を表わした図
である。第１図の構成は第３図に示した従来例と似てい
る。即ち、ロボットアームは、リンクＯ〜３で構成され
る。そして、リンク０にてロボットアームは台座に固定
される。リンク１はモータ４（第１図では図示を省略）
により１軸（Ｚｏ軸）まわりを回転し、リンク２はモー
タ５（第１図では図示を省略〉により２軸（Ｖ＋軸）ま
わりを回転し、リンク３はモータ６（第１図では図示を
省略）により３軸（χ２軸）まわりを回転する。FIG. 1 is a block diagram showing one embodiment of the present invention. Further, FIG. 2 is a diagram showing the operation stage D- of the apparatus shown in FIG. The configuration of FIG. 1 is similar to the conventional example shown in FIG. That is, the robot arm is composed of links O to 3. Then, the robot arm is fixed to the pedestal at link 0. Link 1 is motor 4 (not shown in Figure 1)
The link 2 rotates around the second axis (V+ axis) by the motor 5 (not shown in Figure 1), and the link 3 rotates around the second axis (V+ axis) by the motor 6 (not shown in Figure 1). (omitted) rotates around three axes (χ2 axis).

一方、興なる点は次の通りである。On the other hand, the interesting points are as follows.

第１は、従来のパルス発生器と・パルス／デジタル変換
器８，９．１０に代えて、リンク３の部分に回転角速成
検出手段１５を設けた点である。この回転角速度検出手
段１５として、本明細書ではジャイロ・パッケージを用
いた例で説明する。このジャイロ・パッケージ１５は、
例えば、直交する３つの軸（χ３．ｙコ、Ｚ３）のそれ
ぞれについてジャイロが配置された構成であり、各可動
リンク１゜２．３の回転角速度（ωχ、ωｙ、ωＺ）に
対応した信号を出力することができるものである。なお
、このジャイロ・パッケージ１５は先願の発明装置でも
用いている。The first is that a rotational angular velocity detection means 15 is provided in the link 3 in place of the conventional pulse generator and pulse/digital converters 8, 9, and 10. In this specification, an example using a gyro package will be described as the rotational angular velocity detection means 15. This gyro package 15 is
For example, a gyro is arranged for each of three orthogonal axes (χ3, y, Z3), and a signal corresponding to the rotational angular velocity (ωχ, ωy, ωZ) of each movable link 1°2. It is something that can be output. Note that this gyro package 15 is also used in the invented device of the previous application.

第２は、リンク３に加速度計・パッケージ１６を設けた
点である。この加速度計・パッケージ１６は、上記ジャ
イロ・パッケージと同様、例えば直交する３つの軸（χ
３　ｒ　ｙ３　＋　Ｚ３　）のそれぞれについて加速度
計が配置された構成であり、各可動リンク１．２．３の
加速度（ａχ＊　ａｙ　、ａｘ　）に対応した信号を出
力することができるものである。The second point is that the link 3 is provided with an accelerometer/package 16. This accelerometer package 16, like the above-mentioned gyro package, has, for example, three orthogonal axes (χ
3ry3 + Z3), and is configured such that an accelerometer is arranged for each of the movable links 1.2.3, and can output a signal corresponding to the acceleration (aχ*ay, ax) of each movable link 1.2.3.

この加３１度計・パッケージ１６は本発明で新たに設け
た要素である。This addition 31 degree meter/package 16 is a newly provided element in the present invention.

第３は、補正演算のための推定値を得るため、各リンク
１．２．３が一定角度回転するごとに信号を出力する基
準回転角検出手段４１．４２．４３を設けた点である。Third, in order to obtain estimated values for correction calculations, reference rotation angle detection means 41, 42, and 43 are provided which output signals every time each link 1.2.3 rotates by a certain angle.

この基準回転角検出手段を第４図に示す。この第４図は
第１図のリンク０とリンク１に設けられた基準回転角検
出手段４１部の断面図であり、５０は回転するリンク１
に取付けられた発光素子、６０はリンク０に取付けられ
た受光素子である。このような構成にしておくとリンク
が回転して基準点く受光素子６０が設けられた点）を通
過時点に電気信号が発生するので、制御装Ｗ１２０の基
準回転角検出部２１は、この信号を検知し、先願に記載
した動作で回転角誤差ベクトルε０１回転急速度行列ベ
クトルδ１１）ｔｓの推定を行なうことができる。この
基準回転角検出手段４１．４２．４３は先願の発明装置
でも用いられている。This reference rotation angle detection means is shown in FIG. This FIG. 4 is a sectional view of the reference rotation angle detection means 41 provided in the link 0 and link 1 in FIG.
60 is a light-emitting element attached to link 0, and 60 is a light-receiving element attached to link 0. With this configuration, an electrical signal is generated when the link rotates and passes the reference point (the point where the light receiving element 60 is provided), so the reference rotation angle detection unit 21 of the control device W120 detects this signal. can be detected, and the rotational angle error vector ε01 rotational speed matrix vector δ11)ts can be estimated by the operation described in the prior application. This reference rotation angle detection means 41, 42, 43 is also used in the invention device of the earlier application.

Ｗ４４は、上記ジャイロ・パッケージ１５からの信号（
ωχ、ωｙ、ωＺ）と、加速度計・パッケージ１６から
の信号（ａχ＋　ａｙ＋　ａＺ　）と、上記基準回転角
検出手段４１．４２．４３からの信号（ｔＪ／４１゜θ
４２．φ４３）を導入し、これに演゛算を加えるｉｄＪ
　ｙａ装置２０を設けた点である。この制御装置２０は
、例えばコンピュータ等で構成される。第１図では、本
発明を分り易く説明するため、制御装ｆ１２０の部分を
、制８４４１２０が行なう動作のブロック２１〜３３で
示している。なお、動作ブロック２１〜２８は先願の発
明装置でも備えており、本発明で新たに設けた動作ブロ
ックは３０〜３３である。W44 is the signal from the gyro package 15 (
ωχ, ωy, ωZ), the signal from the accelerometer/package 16 (aχ+ay+aZ), and the signal from the reference rotation angle detection means 41, 42, 43 (tJ/41°θ
42. idJ which introduces φ43) and adds an operation to it.
This is because the ya device 20 is provided. This control device 20 is composed of, for example, a computer. In FIG. 1, in order to explain the present invention in an easy-to-understand manner, the control device f120 is shown as blocks 21 to 33 of operations performed by the controller 844120. Note that the operation blocks 21 to 28 are also provided in the invention device of the prior application, and the operation blocks 30 to 33 are newly provided in the present invention.

なお、本発明ではこの制ｉｌｌ装置２０の演算機能が発
明の要部であるが、上述したように本発明は先願の発明
く回転色事、θ、φを正確に得ることができる発明）を
更に発展させて、ロボットアームの先端座標値を正確に
得るものであるから、制御装置２０の演算機能のうち、
先願で説明した機能については、その機能の説明を省略
する。即ち、説明を省略する機能は （ａ）　　座標変換するための変換行列を得る機能（ｂ
）　　各可動リンクの回転角〈ψ、θ、φ）を算出する
機能 （Ｃ）　変換行列Ｃ３の誤差を補正する機能＜６）　　
回転角誤差ベクトルｔｏ＋回転角速度ベクトルδＷ（Ｓ
の推定を行なう機能 本発明の詳細な説明すると、先願の発明を利用して正確
な回転角（ｖ、θ、φ）信号を得て、更にこの回転角の
情報へ加えて、新たに加速度計からの情報をも１りる。In the present invention, the arithmetic function of the illumination device 20 is the main part of the invention, but as mentioned above, the present invention is based on the invention of the earlier application (an invention that can accurately obtain the rotation angle, θ, and φ). is further developed to accurately obtain the tip coordinate value of the robot arm, so among the calculation functions of the control device 20,
Description of the functions described in the prior application will be omitted. That is, the functions whose explanation will be omitted are (a) the function of obtaining a transformation matrix for coordinate transformation (b)
) Function to calculate the rotation angle 〈ψ, θ, φ) of each movable link (C) Function to correct the error of transformation matrix C3 <6)
Rotation angle error vector to+rotation angular velocity vector δW(S
To explain the details of the present invention, the invention of the previous application is used to obtain accurate rotation angle (v, θ, φ) signals, and in addition to this rotation angle information, new acceleration Also get information from the meter.

そして、この加速度信号を２回積分することで、アーム
先端の移動距離を算出できるので、各アームの長さｒｏ
＋ｒ２＋’３の値を用いることなくロボットアーム先端
の位置座標を得ることができるようにしたものである。Then, by integrating this acceleration signal twice, the moving distance of the arm tip can be calculated, so the length of each arm ro
This allows the position coordinates of the tip of the robot arm to be obtained without using the value of +r2+'3.

（ｅ）　　ロボットアーム先端座標を演算で得る機能 リンク３の先端位置の運動は、リンクＯの固定座標系（
ｙＯＴ　ＦＯ＋　ｚｏ　）で表わされるものとする。リ
ンク○の座標系（Ｚｏ　、　Ｖｏ　＋　Ｚｏ　）の原点
からリンク３の座標系（χｓ　ｒ　’／３ｒ　Ｚ３　）
の原点までの距離ベクトルをＰＯ２とすると運動方程式
は（ｅｌ）式で表わされる。(e) Function to obtain robot arm tip coordinates by calculation The movement of the tip position of link 3 is based on the fixed coordinate system of link O (
yOT FO+ zo ). From the origin of link ○'s coordinate system (Zo, Vo + Zo) to link 3's coordinate system (χs r '/3r Z3)
When the distance vector to the origin of is PO2, the equation of motion is expressed by equation (el).

＃　ＪＪ　ｏ　＝　ａ　６−　（Ｗｔｏ　＋　２　ＷＬ
Ｘ　）　Ｕ　。# JJ o = a 6- (Wto + 2 WL
X)U.

＋　　ｇ　ｏ　　・、＋　ｕ　１　　　　　・・・　　
（ｅｌ）ｆｒＰｈｉ　　”　Ｕ　ｏ　−Ｗａｏ　Ｐｓｉ
　　　　　　　　　’−（ｅ２）ここでｖｏはリンク３
の先端の速度ベクトルをリンクＯの座標系（ｙＯ＊　ｙ
Ｏｒ　ｚｏ　）で表現したもの ａＯはリンク３の先端の慣性加速度ベクトル（加速度計
出力）をリンク０の座標系（χＯ＋ｙｏ、Ｚｏ）で表現
したもの ｇｏは重力加速度ベクトルをリンクＯの座標系（ｙＯ＊
　ｙＯ＊　ｚｏ　）で表現したものｐ、ａ　　はＰｏａ
をリンクＯの座標系（χＯ＋　ｙＯ＋Ｚｏ）で表現した
もの また、ここではリンク３の先端は、質ｆｌ集中系として
の質点として考えている。+ go ・, + u 1 ...
(el)frPhi” U o -Wao Psi
'-(e2) where vo is link 3
The velocity vector at the tip of is linked to the coordinate system of O (yO* y
aO is the inertial acceleration vector (accelerometer output) at the tip of link 3 expressed in the coordinate system of link 0 (χO+yo, Zo); go is the gravitational acceleration vector expressed in the coordinate system of link O (yO *
p, a expressed as yO* zo) is Poa
is expressed in the coordinate system of link O (χO+yO+Zo).Here, the tip of link 3 is considered as a mass point as a quality fl concentrated system.

また、Ｗｅｏ　、Ｗｂｅは（ｅ３）、（ｅ４）式で与え
られる。Furthermore, Weo and Wbe are given by equations (e3) and (e4).

するリンクＯの座標系（Ｚｏ　、　’ｉｏ　、　Ｚｏ　
）の角速度ベクトルである。The coordinate system of the link O (Zo, 'io, Zo
) is the angular velocity vector of

Ｏχ９１ｏ　　　ｓ。Oχ91o s.

また、Ｗ工（ωＬｅ　’ωＬｅ　＋ωＬイ）は慣性空間
に対する地球自転角速度ベクトルである。Further, W(ωLe 'ωLe +ωL) is the earth's rotational angular velocity vector with respect to inertial space.

また、（ｅｌ）式のｕｒは、誤差修正のための操作量ベ
クトルをリンクＯの座標系（χ０＋２１０＋Ｚｏ）で表
現したものである。Further, ur in equation (el) is a manipulated variable vector for error correction expressed in the coordinate system of link O (χ0+210+Zo).

上記（ｅｌ）　、　　（ｅ２）式において、リンク０の
座標系（ｒｏ　＊　’１０　ｒ　Ｚｏ　）は、地球に固
定したものであるから、Ｗｃｏ　＝　Ｏであり、また、
各リンクの動きに比べて地球の自転角速度は無視できる
程小さいから、（ｅ５）式が成立つ。In the above equations (el) and (e2), the coordinate system of link 0 (ro * '10 r Zo) is fixed to the earth, so Wco = O, and
Since the earth's rotational angular velocity is negligibly small compared to the movement of each link, equation (e5) holds true.

Ｗ、Ｊ＞　１Ｊｊ＝　　　　　　　　　　　　　　　　
（ｅ５　）ｌＪｈ３はリンク０の座標系に対するリンク
３の角速度ベクトルをリンクＯの座標系（ｙＯ＋　Ｖ　
Ｏ＋Ｚｏ）で表現したものである。W, J> 1Jj=
(e5) lJh3 is the angular velocity vector of link 3 with respect to the coordinate system of link 0, which is the coordinate system of link O (yO+V
O+Zo).

更に、ａｏ　）２ＷＬｃｌｊｏ　　　　　　　　　　（
ｅ６）とみなせるから、上記（ｅｌ）、（′ｅ２）式は
、次のように簡単になる。Furthermore, ao )2WLcljo (
e6), the above equations (el) and ('e2) can be simplified as follows.

ｆｒ　Ｕ　ｏ　＝　ａｏ　＋　（Ｊｏ　＋　ｕ＋　　　
　　　　　　（ｅ７）；ｆＦ　’ｏ３　−　ＩＪ　Ｏ（
ｅ８）（ｅ７）式を（ｅ８）式に代入して ７Ｆ、３　＝　ａｏ　＋Ｑ６　＋ｕ　ｌ（ｅ９）ここで
、次の（ｅ　１０）〜（ｅ１３）式が成立っている。fr U o = ao + (Jo + u+
(e7); fF 'o3 - IJ O(
e8) Substituting equation (e7) into equation (e8), 7F, 3 = ao + Q6 + u l (e9) Here, the following equations (e10) to (e13) hold true.

ａｏ　””Ｃ３ａｘ　　　　　　　　　　　　（ｅ　１
０）ａ３は加速度形の出力である。ao ””C3ax (e 1
0) a3 is an acceleration type output.

１ｇ　　　ｆｒ　　　ｚａ よつて、リンク３の先端位置（ｐ　　、　ｐ　　＋　ｐ
、３＞は、（ｅ　１４）式で表わされる。1g fr za Therefore, the tip position of link 3 (p, p + p
, 3> is expressed by equation (e 14).

・・・　（ｅ１４） 加速度計１Ｂは、測定誤差Δａ３を有する。また、ａ３
をリンクＯの座標系（χＯ＋　７１０　＋　Ｚｏ　）へ
座標変換する際の変換行列Ｃコにもジャイロ誤差に起因
する誤差ΔＣ１を含んでいる。そこで、この誤差を修正
のための操作量ベクトルｕ蒐を計算する。この計算は第
１図の操作量演算部３２で行なう。Ｕ＋はｕ＋のリンク
Ｏの座標系（Ｚｏ　＋　２’　ｏ　＋Ｚｏ）で表現した
ものである。(e14) The accelerometer 1B has a measurement error Δa3. Also, a3
The conversion matrix C when converting the coordinates into the coordinate system of the link O (χO+710+Zo) also includes an error ΔC1 due to the gyro error. Therefore, a manipulated variable vector uyaku is calculated to correct this error. This calculation is performed by the manipulated variable calculating section 32 shown in FIG. U+ is expressed in the coordinate system of link O of u+ (Zo + 2' o +Zo).

上記した座標変換行列の誤差ΔＣ３は、リンク０の座標
系（Ｚｏ　、　７Ｊｏ　＊　Ｚｏ　）での回転角誤差ベ
クトルε（εχ。、εｊ１０ｒ　εＺＯ）を用いて次の
ように表わすことができる。The error ΔC3 of the coordinate transformation matrix described above can be expressed as follows using the rotation angle error vector ε(εχ., εj10r εZO) in the coordinate system of link 0 (Zo, 7Jo*Zo).

ここで、回転角誤差行列ＥＯは（８１６）式で表わされ
る。Here, the rotation angle error matrix EO is expressed by equation (816).

つまり上述の（ｅ　１４）式のａＯは、次のような誤差
δａＯを含む。In other words, aO in the above equation (e14) includes the following error δaO.

δａｏ−ｃ：Δａ３ＥＯＣ３ａ３　　　　（８１７）こ
の誤差によって、もし、Ｕ、−Ｏなら（ｅ１４）式によ
り計算されるＰＩＪ　は、次のような増大傾向を有する
位置誤差δＰａｌ　　を含む。δao-c: Δa3EOC3a3 (817) Due to this error, if U, -O, PIJ calculated by equation (e14) includes a position error δPal that has the following increasing tendency.

δＦ：　＜ｔ＞−δＰＩＴ３＜ｏ＞ ・・・（ｅ　１８） また、速度誤差δＶＯΦ）は次式で表わされる。δF:　<t>−δPIT3<o> ...(e18) Further, the speed error δVOΦ) is expressed by the following equation.

・・・　（ｅ１９） ここで本発明に係る装置では、先願の発明を用０ている
ため、（ｅ　１７）〜（ｅ１９）式の回転角誤差ＥＯは
、極めて小さい値である。(e19) Here, since the device according to the present invention does not use the invention of the prior application, the rotation angle error EO in equations (e17) to (e19) is an extremely small value.

また、位置の不正確さの初期値δＰａｌ　ＣＯ）と、速
度の不正確さの初期値δＵ　ｏ　（’）は正確にキャリ
ブレーションすることができるものであるから、δＰ′
（の〜０、δｖｏ（Ａ〜０と見做すこでき、Ｊ ３をキャリブレーションすることにある。そこで、本発
明では、ロボットアームが一旦停止した際に上記加速度
計の誤差Δａ３をキャリブレーションするようにしてい
る。In addition, since the initial value of positional inaccuracy δPal CO) and the initial value of velocity inaccuracy δU o (') can be accurately calibrated, δP′
(~0, δvo(A~0), and the purpose is to calibrate J3. Therefore, in the present invention, when the robot arm once stops, the error Δa3 of the accelerometer is calibrated. That's what I do.

このキャリブレーションは具体的に次のようにして行な
う。ロボットアームは、常時一定方向へ連続運動をして
いるわけでなく、移動方向を反転させる場合が多い。こ
の際、−瞬その動作が停止するので、ロボットアームの
実際の速度はＯとなるが、加速度計１６の出力に基づい
て計算された速度は、一般にＯとならず、成る（ぬにな
っている。This calibration is specifically performed as follows. A robot arm does not always move continuously in a fixed direction, but often reverses its direction of movement. At this time, the actual speed of the robot arm is O because the movement stops for an instant, but the speed calculated based on the output of the accelerometer 16 generally does not become O, but becomes There is.

その理由は、加速度計に誤差があるからである。The reason is that there is an error in the accelerometer.

従って、本発明ではこのようにロボットアームの動作が
一旦停止した際に、加速度計の誤差を修正する（キャリ
ブレーションする）ようにしている。Therefore, in the present invention, when the operation of the robot arm is once stopped, the error in the accelerometer is corrected (calibrated).

即ち、計算により得られる速度１７．をモニタしておき
、ロボット７−ムが停止した時のｖＯ（０が加速度計の
誤差Δａ、によって発生した速度誤差であるから、以下
のようにして加速度計誤差Δａ、を推定する。That is, the speed obtained by calculation 17. is monitored, and since vO (0 is the speed error caused by the accelerometer error Δa) when the robot 7-m stops, the accelerometer error Δa is estimated as follows.

ロボットアームが停止した時刻を１１　＋＋　とすると
、針環による速度υｏ　（ｔｉ＋ｌ）について次式が成
立する。Assuming that the time when the robot arm stopped is 11 ++, the following equation holds for the speed υo (ti+l) due to the needle ring.

ここで、 δｖｏ（亡、４．）は速度誤差、 υ、（４はスタート時の速度であるから０ΔｌＪ、（ｔ
ｌ）は初期キャリブレーションによりＯ４０は停止時で
あるからＯ Δ４３は加速度耐の誤差 であるから上記ｖｏ　　（ｉｉ　＋＋　）は（Ｃ２２）
式にここで 丼Ｃ３（′ｔ）＝　Ｇ　３　（ｔ）Ｗ、、”’　（ｅ　
２３）となる。Here, δvo(death, 4.) is the speed error, υ, (4 is the speed at the start, so 0ΔlJ, (t
l) is due to initial calibration, O40 is at the time of stop, so O Δ43 is an error in acceleration resistance, so the above vo (ii ++) is (C22)
In the formula, C3('t)=G3(t)W,,"'(e
23).

ク　　　　　　Ｊ Ｃ３（ｔ）−ｅｘｐ　　（Ｗｌ）Ｊ　）　・Ｃ３（？）
　　　　−（６２６）（ｅ　２３）　、　　（ｅ　２６
）式の関係より、Ｃ８２２）式のここで、（Ｃ２２）式
のｌＪｏの代りにノイズを平滑したＤｏの平均を用いる
と（ｅ２ｇ）式が得られる。C3(t)-exp (Wl)J) ・C3(?)
-(626) (e 23) , (e 26
) From the relationship in equation (C822), equation (e2g) can be obtained by using the average of Do obtained by smoothing the noise in place of lJo in equation (C22).

・・・　（Ｃ２８） これより、加速度計の誤差Δａ３の推定値Δとは次式と
なる。(C28) From this, the estimated value Δ of the error Δa3 of the accelerometer is expressed by the following equation.

Δａ３　＝　　［Ｃ：　　ｅ’）　　　Ｃ：　　＠Ｊ］
　　−’　　Ｗ、、Ｊ　　ｌ）＜ｔＬ＋／　　）・・・
（Ｃ２９） ここで、ロボットアームが時刻ｔｕにて、停止したとす
る。このとき、ｖｏのサンプル数はｍｉ！目とする。本
発明では、平滑化して誤差を推定しているので（Ｃ２９
）式は次式で表わされる。Δa3 = [C: e') C: @J]
−' W,, J l)<tL+/ )...
(C29) Here, assume that the robot arm stops at time tu. At this time, the number of samples of vo is mi! Eyes. In the present invention, since the error is estimated by smoothing (C29
) is expressed by the following equation.

岬し３嘉［Ｃ二輪−ＣＩに）］　−１Ｗ、ゴｈυＯω・
・・（Ｃ３０） これにより、誤差の修正操作ＩＩ　ｕ　Ｆ’は（Ｃ３１
）式％式％（３１） このＩＬ　ｒにより（Ｃ９）式の誤差の変化分は（Ｃ３
２）式となる。Misakishi 3 Ka [C motorcycle-CI] -1W, gohυOω・
...(C30) As a result, the error correction operation II u F' is (C31
) formula % formula % (31) With this IL r, the change in error of formula (C9) is (C3
2) Equation becomes.

ｇδ７）、：（ｔ）　−Ｃ３Δａ３＋ｕ＋（ｔ）−Ｇ３
　Δａｓ　　−Ｃ３Δ旦ヨー ”＝Ｏ（Δｄ３〜Δ工Ｌ）　　　−（ｅ　３２　）ゆえ
に δＰ、’；　（＋）七〇　（δｐｐｉ　（の七〇）δｖ
ｏ（ｔ）ｚＯ（δυｏ（０）〜０）以上のように本発明
によれば、リンク３の先端位置を正確に求めることがで
きるが、次に具体的に加速度計の誤差Δａ３　（Δａ工
３．Δａグ３．Δａ１３）により＜ｕ＋　＝Ｏとして）
、どの程度位置の誤差が出るかを説明する。gδ7), :(t) −C3Δa3+u+(t)−G3
Δas −C3Δdanyo”=O(Δd3~ΔworkingL) −(e 32 ) Therefore, δP,'; (+)70 (δppi (70) δv
o(t)zO(δυo(0)~0) As described above, according to the present invention, the tip position of the link 3 can be accurately determined. 3. Δa 3. Δa13) so that <u+ = O)
, explain how much positional error occurs.

今、加速度計の誤差を Δ　ａ、７　　−　１０’　　ｇ　　、　　Δ　ａ７ａ
　　　−ＯＮ　　　Δ　ａＪ３−０と仮定する。また、
説明を簡単化するために、アームは回転角型のみ回転す
ると仮定する。つまり、θ−〇、φ−０とする。このと
き座標変換行列Ｃ３は次式で表わされる。Now, let the error of the accelerometer be Δ a, 7 − 10' g, Δ a7a
-ON Δ aJ3-0. Also,
To simplify the explanation, it is assumed that the arm rotates only in the rotation angle type. In other words, θ-0 and φ-0. At this time, the coordinate transformation matrix C3 is expressed by the following equation.

従って、（ｅ２０）式より次式が得られる。Therefore, the following equation is obtained from equation (e20).

ψ−ｄｌＪ／／ｄτである。ψ−dlJ//dτ.

今、ｃｏｓ（１４／ｌ）が最大値（１）と仮定してココ
テ、’４’−３２４００°／ｈ（かなリノ低速回転）と
すると、 ＩＤ ［δ［）］ｍａｘ＝［δｐ、ｊ］　＊＊＊　％　０．４
　ｕ＋Ｊ となる。これは、従来の装置より約２桁端度が向上して
いることを表わしている。Now, assuming that cos (14/l) is the maximum value (1) and set to '4'-32400°/h (kana lino low speed rotation), ID [δ[)] max = [δp, j] ***% 0.4
It becomes u+J. This represents an improvement of approximately two orders of magnitude over the conventional device.

更に、本発明では？！＋＝Ｏ（加速度計の誤差Δａ３の
キャリブレーション操作潰）であるからｕ、　　（ｅ）
　−−０３（ｔ）Δ１Ｌの効果により、後述する如（０
，０４ＩＩの精度が期待される。Furthermore, in the present invention? ! +=O (accelerometer error Δa3 calibration operation failure), so u, (e)
--03(t) Due to the effect of Δ1L, (0
,04II accuracy is expected.

なお、以上では、ジャイロ１５を用いてアームの回転角
速度を求めるように説明したが、ジャイロに限定するわ
けでなく、ジャイロの代りに加速度計を用いて回転角速
度を測定しても上の説明と同様に、本発明の動作を行な
わせることができる。In addition, although the explanation above uses the gyro 15 to determine the rotational angular velocity of the arm, the above explanation is not limited to the gyro, and even if the rotational angular velocity is measured using an accelerometer instead of the gyro. Similarly, the operations of the present invention can be performed.

また、加速度計を力センサ（フォースセンザ）でＷ１換
えても本発明は成立する。Furthermore, the present invention can be applied even if the accelerometer is replaced by a force sensor (W1).

また、本発明を作業用ロボットに限定するものではなく
、例えば搬送用ロボットに適用しても良い。Further, the present invention is not limited to working robots, and may be applied to, for example, transport robots.

ハ、「発明の効果」 以上述べたように、本発明によれば、次の効果が得られ
る。C. "Effects of the Invention" As described above, according to the present invention, the following effects can be obtained.

■　ロボットアームの先端座標値、が正確に決定できる
ので従来装置に比べて高精度な作業を実現できる。■ The coordinates of the tip of the robot arm can be determined accurately, allowing for more precise work than with conventional equipment.

■　１０（９の加速度計を用いることにより位置誤差は
最大でも約０．０４　ｃｍに押えることができ、従来よ
り２桁向上する。■ By using a 10 (9) accelerometer, the position error can be kept to a maximum of about 0.04 cm, which is two orders of magnitude better than the conventional method.

■　また、加速度計の誤差のキャリブレーション（Ｕ　
＋による）により加速度計の精度を更に１桁向上させる
ことができ、１０−７となる。■ Also, calibrate the accelerometer error (U
+), the accuracy of the accelerometer can be further improved by one order of magnitude to 10-7.

【図面の簡単な説明】[Brief explanation of drawings]

第１図は本発明に係るロボット用軌道決定装置のブロッ
ク図、第２図は本発明に係る装置のフローチャート、第
３図は従来装置の構成図、第４図は基準回転角検出手段
の構成例を示した図である。 Ｏ〜３・・・リンク、４〜６・・・モータ、１５・・・
回転角速度検出手段（ジャイロ・パッケージ）、１６・
・・加速度計、２０・・・制御装置、４１．４２．４３
・・・基準回転角検出手段。FIG. 1 is a block diagram of a robot trajectory determining device according to the present invention, FIG. 2 is a flowchart of the device according to the present invention, FIG. 3 is a configuration diagram of a conventional device, and FIG. 4 is a configuration of a reference rotation angle detection means. It is a figure showing an example. O~3...Link, 4~6...Motor, 15...
Rotational angular velocity detection means (gyro package), 16.
...accelerometer, 20...control device, 41.42.43
...Reference rotation angle detection means.

Claims (1)

【特許請求の範囲】 回転する３つのリンクを持つロボットアームと、各リン
クが一定角度回転するごとに信号を出力する基準回転角
検出手段（４１、４２、４３）と、各リンクの回転角速
度を検出する手段（１５）と、前記回転する３つのリン
クの回転角（Ψ、θ、φ）に対応した信号を演算する制
御装置と、を備えた装置において、 各リンクの加速度を計測する加速度計（１６）を具備し
、 前記制御装置が以下の演算機能（Ａ）をも備えるように
したことを特徴とするロボット用軌道決定装置。 （Ａ）制御装置の演算機能 前記加速度計（１６）から信号を導入し、この信号を２
回積分してロボットアームの端部にあるリンク３の先端
位置座標値を算出する演算機能と、ロボットアームが停
止した時に加速度計誤差（Δａ＿３）の推定値を演算す
る機能。[Claims] A robot arm having three rotating links, reference rotation angle detection means (41, 42, 43) that outputs a signal every time each link rotates by a certain angle, and a rotation angular velocity of each link. An accelerometer that measures the acceleration of each link in an apparatus comprising a detecting means (15) and a control device that calculates a signal corresponding to the rotation angle (Ψ, θ, φ) of the three rotating links. A trajectory determination device for a robot, comprising (16), wherein the control device also includes the following calculation function (A). (A) Arithmetic function of the control device Introduces a signal from the accelerometer (16) and converts this signal into 2
A calculation function that calculates the coordinate value of the tip position of the link 3 at the end of the robot arm by integrating it once, and a function that calculates the estimated value of the accelerometer error (Δa_3) when the robot arm stops.