【０００１】0001
【産業上の利用分野】本発明は、移動体に備えられ、種
々の動作を行うマニピュレータの位置及び姿勢を制御す
る方法及び装置に関する。BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method and apparatus for controlling the position and orientation of a manipulator that is provided on a moving body and performs various operations.
【０００２】0002
【従来の技術】工場での生産性の向上及び安全性の確保
を実現するために、種々の作業用のロボットが開発され
ており、例えば、その先端に手先効果器を設けた多関節
型のマニピュレータを、自律走行する移動体である台車
に搭載した移動ロボットがある。このような移動ロボッ
トにおいては、前記台車の走行制御を行いつつ前記マニ
ピュレータの手先効果器の位置・姿勢制御を行うことに
よって、作業対象物に対して台車の走行中にマニピュレ
ータにて作業を行うものが考えられている。この移動ロ
ボットでは、台車の走行中の位置及び方位の情報をマニ
ピュレータの位置・姿勢制御に取り込むことによって、
台車を走行させつつマニピュレータの手先効果器を、作
業対象物に対する目標の位置・姿勢に制御するようにな
っていた。この場合の位置・姿勢制御では、マニピュレ
ータの手先効果器の実際の位置・姿勢を検出することに
よって、作業対象物に対する目標の位置・姿勢とその実
際の位置・姿勢との偏差を求め、この偏差を零とするよ
うにマニピュレータを動作させるフィードバック制御が
行われていた。[Prior Art] In order to improve productivity and ensure safety in factories, robots for various tasks have been developed. There is a mobile robot in which a manipulator is mounted on a cart, which is an autonomous moving body. In such a mobile robot, the manipulator performs work on the work target while the trolley is running by controlling the position and posture of the hand effect device of the manipulator while controlling the running of the trolley. is considered. This mobile robot incorporates information about the position and orientation of the trolley while it is running into the position and attitude control of the manipulator.
While moving the cart, the manipulator's end effector was controlled to a target position and posture relative to the workpiece. In this case, position/posture control involves detecting the actual position/posture of the hand effector of the manipulator to find the deviation between the target position/posture and its actual position/posture with respect to the workpiece, and then Feedback control was used to operate the manipulator so that the value was zero.
【０００３】0003
【発明が解決しようとする課題】前述の如きフィードバ
ック制御を行う従来の移動ロボットにおいては、台車の
走行中に、作業対象物に対するマニピュレータの手先効
果器の目標の位置・姿勢と実際の位置・姿勢との偏差を
零とすべくマニピュレータが制御動作を行うが、この制
御動作中にも台車が走行しているので、前記制御動作中
にも台車の走行に従って手先効果器が台車の走行方向に
移動していく。このため、台車の走行中は、マニピュレ
ータの制御動作が行われるのにも関わらず、台車の走行
に従って新たな位置・姿勢の偏差が生じて行くので、位
置・姿勢の偏差は零になることはなく、台車の定速走行
中には、手先効果器の目標の位置・姿勢と実際の位置・
姿勢との間に定常偏差が生じるという難点があり、マニ
ピュレータの位置・姿勢制御の精度が悪いという問題が
あった。[Problems to be Solved by the Invention] In the conventional mobile robot that performs feedback control as described above, while the cart is running, it is necessary to compare the target position/orientation of the hand effector of the manipulator with respect to the workpiece and the actual position/orientation. The manipulator performs a control operation in order to make the deviation from I will do it. Therefore, while the trolley is running, new deviations in position and orientation occur as the trolley moves, despite the control operations of the manipulator, so the deviations in position and orientation will never become zero. While the trolley is running at a constant speed, the target position and posture of the hand effector and the actual position and
There is a problem that a steady deviation occurs between the position and the position, and the accuracy of the position and position control of the manipulator is poor.
【０００４】本発明は斯かる事情に鑑みてなされたもの
であり、マニピュレータの位置・姿勢の定常偏差を抑制
し、位置・姿勢制御の精度を向上させることを可能とす
るマニピュレータの位置・姿勢制御方法及び装置を提供
することを目的とする。The present invention has been made in view of the above circumstances, and provides a manipulator position/attitude control that suppresses steady-state deviations in the position/attitude of the manipulator and improves the accuracy of position/attitude control. An object of the present invention is to provide a method and apparatus.
【０００５】[0005]
【課題を解決するための手段】本発明に係るマニピュレ
ータの位置・姿勢制御方法は、種々の動作を行うマニピ
ュレータを備えた移動体の位置及び方位に応じて前記マ
ニピュレータの位置及び姿勢の目標値を定め、定めた目
標値を実現すべく、その目標値と前記マニピュレータの
実際の位置及び姿勢との偏差に関連して前記マニピュレ
ータの制御量を求め、求めた制御量の動作を実行させる
ことにより、前記マニピュレータの位置及び姿勢を制御
する方法であって、その移動状態に関連して前記移動体
の位置及び方位の時間微分値を求め、求めた移動体の位
置及び方位の時間微分値に基づいて前記マニピュレータ
の位置及び姿勢の目標値の時間微分値を求め、求めたマ
ニピュレータの位置及び姿勢の目標値の時間微分値と、
前記偏差とに基づいてマニピュレータの制御量を求める
ことを特徴とする。[Means for Solving the Problems] A method for controlling the position and orientation of a manipulator according to the present invention sets target values for the position and orientation of a manipulator according to the position and orientation of a moving body equipped with a manipulator that performs various operations. In order to realize the determined target value, the control amount of the manipulator is determined in relation to the deviation between the target value and the actual position and orientation of the manipulator, and the operation of the determined control amount is executed. A method for controlling the position and orientation of the manipulator, the method comprising: determining time differential values of the position and orientation of the moving body in relation to its movement state; and based on the determined time differential values of the position and orientation of the moving body. A time differential value of the target value of the position and posture of the manipulator is determined, and a time differential value of the determined target value of the position and posture of the manipulator;
The control amount of the manipulator is determined based on the deviation.
【０００６】本発明に係るマニピュレータの位置・姿勢
制御装置は、種々の動作を行うマニピュレータを備えた
移動体の位置及び方位に応じて前記マニピュレータの位
置及び姿勢の目標値を定める手段と、定めた目標値を実
現すべく、その目標値と前記マニピュレータの実際の位
置及び姿勢との偏差に関連して前記マニピュレータの制
御量を求める手段と、求めた制御量の動作を実行させる
手段とを備え、前記マニピュレータの位置及び姿勢を制
御する装置であって、その移動状態に関連して前記移動
体の位置及び方位の時間微分値を求める手段と、求めた
移動体の位置及び方位の時間微分値に基づいて前記マニ
ピュレータの位置及び姿勢の目標値の時間微分値を求め
る手段と、求めたマニピュレータの位置及び姿勢の目標
値の時間微分値と、前記偏差とに基づいてマニピュレー
タの制御量を求める手段とを具備することを特徴とする
。A manipulator position/attitude control device according to the present invention includes means for determining target values for the position and orientation of the manipulator according to the position and orientation of a moving body equipped with a manipulator that performs various operations; In order to realize the target value, the apparatus includes means for determining a controlled amount of the manipulator in relation to a deviation between the target value and the actual position and orientation of the manipulator, and means for executing the operation of the determined controlled amount, A device for controlling the position and orientation of the manipulator, the apparatus comprising means for determining the time differential values of the position and orientation of the moving body in relation to its movement state, and means for determining the time differential values of the determined position and orientation of the mobile body means for determining the time differential value of the target value of the position and orientation of the manipulator based on the determined value, and means for determining the control amount of the manipulator based on the determined time differential value of the target value of the position and orientation of the manipulator, and the deviation. It is characterized by comprising the following.
【０００７】[0007]
【作用】本発明にあっては、マニピュレータがその位置
及び姿勢の目標値を実現すべく制御動作を行う場合、マ
ニピュレータの制御量は、マニピュレータの位置及び姿
勢の目標値と、その実際の位置及び姿勢との偏差から得
られる制御量の要素及びマニピュレータの位置及び姿勢
の目標値の時間微分値から得られる制御量の要素にて構
成されている。マニピュレータの位置及び姿勢の目標値
の時間微分値に基づいて得られる制御量の要素は、その
移動状態に関連して前記移動体の位置及び方位の時間微
分値を求め、求めた移動体の位置及び方位の時間微分値
に基づいて前記マニピュレータの位置及び姿勢の目標値
の時間微分値を求め、該時間微分値に基づいて得られる
ものであり、即ち、移動体の移動によって生じる、前記
目標値からのマニピュレータの位置及び姿勢のずれ量を
予測してこれを補償する。[Operation] In the present invention, when the manipulator performs a control operation to realize the target values of its position and orientation, the control amount of the manipulator is determined by the target values of the position and orientation of the manipulator, and its actual position and orientation. It is composed of a control amount element obtained from the deviation from the attitude and a control amount element obtained from the time differential value of the target value of the position and attitude of the manipulator. The elements of the control amount obtained based on the time derivatives of the target values of the position and orientation of the manipulator are obtained by calculating the time derivatives of the position and orientation of the moving object in relation to its movement state, and calculating the calculated position of the moving object. The time differential value of the target value of the position and orientation of the manipulator is determined based on the time differential value of the position and orientation of the manipulator, and the target value is obtained based on the time differential value, that is, the target value is generated due to the movement of the moving object. The amount of deviation in the position and posture of the manipulator is predicted and compensated for.
【０００８】[0008]
【実施例】以下本発明をその実施例を示す図面に基づい
て具体的に説明する。図１は本発明に係るマニピュレー
タの位置・姿勢制御方法の実施に使用する移動ロボット
の斜視図である。DESCRIPTION OF THE PREFERRED EMBODIMENTS The present invention will be specifically explained below based on drawings showing embodiments thereof. FIG. 1 is a perspective view of a mobile robot used to implement the manipulator position/attitude control method according to the present invention.
【０００９】図中１は自律走行を行う移動体である台車
であり、該台車１は、箱型の車体１０を、左右一対の駆
動輪１１，１１ （右駆動輪のみ図示）と、前後左右に
配されたキャスタ１２，１２，１２，１２ （２つのみ
図示）とにて支持してなる。前記左駆動輪１１及び右駆
動輪１１は各別のモータ（図示せず）によって独立的に
駆動されるようになっている。台車１の車体１０の上部
には、連結された複数の関節２１，２１，…を有し、そ
の先端部に手先効果器２０を備えた多関節型のマニピュ
レータ２が備えられている。前記関節２１，２１，…の
夫々は、サーボモータ（図示せず）によって駆動される
ようになっている。前記左駆動輪１１及び右駆動輪１１
には夫々、その回転に応じてパルス信号を発振するパス
ジェネレータよりなる走行速度検出器（図示せず）が設
けられている。そして、前記マニピュレータ２の各関節
２１，２１，…には、その関節角度を検出する角度検出
器（図示せず）が設けられている。また、図中ΣＨ は
マニピュレータ２の手先効果器２０の位置・姿勢を表現
するために手先効果器２０に設定された座標系、Σｖ
は台車１に設定された移動座標系、Σｗ は作業対象物
３の位置・姿勢を表現するために作業対象物３に設定さ
れた作業座標系である。[0009] In the figure, reference numeral 1 denotes a cart which is a mobile body that runs autonomously. It is supported by casters 12, 12, 12, 12 (only two are shown) arranged on. The left driving wheel 11 and the right driving wheel 11 are independently driven by separate motors (not shown). The upper part of the vehicle body 10 of the truck 1 is provided with a multi-joint manipulator 2 having a plurality of connected joints 21, 21, . . . and having a hand effector 20 at its tip. Each of the joints 21, 21, . . . is driven by a servo motor (not shown). The left driving wheel 11 and the right driving wheel 11
Each is provided with a traveling speed detector (not shown) consisting of a path generator that oscillates a pulse signal in accordance with the rotation thereof. Each joint 21, 21, . . . of the manipulator 2 is provided with an angle detector (not shown) for detecting the joint angle. In addition, ΣH in the figure is a coordinate system set to the hand effector 20 to express the position and orientation of the hand effector 20 of the manipulator 2, Σv
is a movement coordinate system set on the cart 1, and Σw is a work coordinate system set on the work object 3 to express the position and orientation of the work object 3.
【００１０】まず、マニピュレータ２及び台車１の夫々
の基本的な運動学について説明する。ある座標系で表さ
れた６自由度を持つマニピュレータ２の手先効果器２０
の位置・姿勢を下記（１） 式のベクトルｒで表し、そ
の位置・姿勢を実現するマニピュレータ２の関節２１，
２１，…の関節角を下記（２） 式のベクトルｑで表す
。First, the basic kinematics of the manipulator 2 and the trolley 1 will be explained. A hand effector 20 of a manipulator 2 with 6 degrees of freedom expressed in a certain coordinate system
The position and orientation of the manipulator 2 are expressed by the vector r in equation (1) below, and the joints 21,
The joint angle of 21,... is expressed by the vector q in equation (2) below.
【００１１】[0011]
【数１】[Math 1]
【００１２】0012
【数２】[Math 2]
【００１３】前記手先効果器２０の位置・姿勢と、前記
関節角との間には、下記（３） 式に示される如く所定
の関数による順変換の関係があり、また、下記（４）
式に示される如く所定の関数による逆変換の関係がある
。[0013] There is a relationship between the position/posture of the hand effector 20 and the joint angle based on a predetermined function, as shown in equation (3) below, and also as shown in equation (4) below.
As shown in the equation, there is an inverse transformation relationship using a predetermined function.
【００１４】[0014]
【数３】[Math 3]
【００１５】[0015]
【数４】[Math 4]
【００１６】前記（３） 式を時間で微分すると、前記
位置・姿勢の時間微分値と前記関節角の時間微分値との
間には、下記（５） 式の如き関係がある。When the above equation (3) is differentiated with respect to time, there is a relationship as shown in the following equation (5) between the time differential value of the position/posture and the time differential value of the joint angle.
【００１７】[0017]
【数５】[Math 5]
【００１８】また、作業座標系Σｗ で表された台車１
の位置・方位は、見浪ほか：同次変換行列を用いた自律
移動ロボットの任意曲線コース誘導方法，日本ロボット
学会志，Ｖｏｌ．８ ，Ｎｏ．４，１９９０に発表され
た如く、下記（６） 式の如き同時変換行列 ｗＡｖ
にて表される。[0018] Also, the trolley 1 expressed in the work coordinate system Σw
The position and orientation of are determined by Minami et al.: Arbitrary curve course guidance method for autonomous mobile robots using homogeneous transformation matrices, Proceedings of the Robotics Society of Japan, Vol. 8, No. 4, 1990, the simultaneous transformation matrix wAv as shown in the following equation (6)
It is expressed as
【００１９】[0019]
【数６】[Math 6]
【００２０】次に、台車１の走行を考慮してマニピュレ
ータ２の手先効果器２０の位置を制御する方法について
説明する。下記（７） 式の如き作業座標系Σｗ で表
された手先効果器２０の位置・姿勢の目標軌道と、その
時間微分値とが与えられている場合を考える。Next, a method for controlling the position of the hand effector 20 of the manipulator 2 in consideration of the traveling of the cart 1 will be explained. Consider a case where a target trajectory of the position/orientation of the hand effector 20 expressed by a work coordinate system Σw as shown in equation (7) below and its time differential value are given.
【００２１】[0021]
【数７】[Math 7]
【００２２】この場合、移動座標系Σｖ で表された手
先効果器２０の位置ベクトル ｖｒｐｄは、前記（６）
式で表された台車１の位置・方位を考慮すると、下記
（８） 式を満たさなければならない。In this case, the position vector vrpd of the hand effector 20 expressed in the moving coordinate system Σv is determined by the above (6).
Considering the position and orientation of truck 1 expressed by the formula, the following formula (8) must be satisfied.
【００２３】[0023]
【数８】[Math. 8]
【００２４】前記（８） 式は、作業座標系Σｗ から
見た手先効果器２０の位置の目標軌道を表す位置ベクト
ル ｗｒｐｄは、移動座標系Σｖ で表された手先効果
器２０の位置ベクトルｖｒｐｄを作業座標系Σｗ から
見た位置ベクトルに変換したものと、作業座標系Σｗ
から見た台車１の移動座標系Σｖ の位置ベクトル ｗ
ｒｖ との和に等しいということを表している。即ち、
台車１の走行に伴って変化する、台車１の方位を表す行
列 ｗＲｖ 及び台車１の位置を表す位置ベクトル ｗ
ｒｖ に応じて手先効果器２０の位置を表す位置ベクト
ル ｖｒｐｄが前記（８） 式を満たしつつ変化すれば
、位置ベクトル ｗｒｐｄで表される手先効果器２０の
位置の目標軌道は実現される。Equation (8) above indicates that the position vector wrpd representing the target trajectory of the position of the hand effector 20 as seen from the work coordinate system Σw is the position vector vrpd of the hand effector 20 expressed in the moving coordinate system Σv. The position vector converted from the work coordinate system Σw and the work coordinate system Σw
The position vector of the moving coordinate system Σv of the trolley 1 as seen from w
This means that it is equal to the sum of rv. That is,
A matrix wRv representing the orientation of the truck 1 and a position vector w representing the position of the truck 1, which changes as the truck 1 travels.
If the position vector vrpd representing the position of the hand effector 20 changes according to rv while satisfying the above equation (8), the target trajectory of the position of the hand effector 20 represented by the position vector wrpd is realized.
【００２５】また、前記（８） 式を時間微分すると、
下記（９） 式が得られる。[0025] Furthermore, when the above equation (8) is differentiated with respect to time, we get
The following formula (9) is obtained.
【００２６】[0026]
【数９】[Math. 9]
【００２７】走行している台車１が、前記（８） 式に
おける手先効果器２０の位置の目標軌道（位置ベクトル
ｗｒｐｄ）及びその時間微分値を実現するためには、
手先効果器２０の位置ベクトル ｖｒｐｄ は前記（
８） を変形して得られる下記（１０）式を満たす必要
があり、その位置ベクトル ｖｒｐｄの時間微分値は前
記（９） を変形して得られる下記（１１）式を満たす
必要がある。但し、下記（１１）式における位置ベクト
ル ｖｒｐｄは下記（１０）式において求まるので既知
である。In order for the running trolley 1 to realize the target trajectory (position vector wrpd) of the position of the hand effector 20 in the above equation (8) and its time differential value,
The position vector vrpd of the hand effector 20 is expressed as (
8) It is necessary to satisfy the following equation (10) obtained by transforming the above equation (10), and the time differential value of the position vector vrpd needs to satisfy the following equation (11) obtained by transforming the above (9). However, the position vector vrpd in the following equation (11) is known because it can be found in the following equation (10).
【００２８】[0028]
【数１０】[Math. 10]
【００２９】[0029]
【数１１】[Math. 11]
【００３０】マニピュレータ２の手先効果器２０の姿勢
を表す３×３行列Ｒは、手先効果器２０の姿勢を表すベ
クトルｒｏ の定義に従って定まる関数ｆによって、下
記（１２）式の如く表される。A 3×3 matrix R representing the posture of the hand effector 20 of the manipulator 2 is expressed by a function f determined according to the definition of a vector ro representing the posture of the hand effector 20, as shown in equation (12) below.
【００３１】[0031]
【数１２】[Math. 12]
【００３２】また、前記（１２）式を時間微分すると、
下記（１３）式が得られる。[0032] Also, when the above equation (12) is differentiated with respect to time, we get
The following formula (13) is obtained.
【００３３】[0033]
【数１３】[Math. 13]
【００３４】作業座標系Σｗ で表された手先効果器２
０の姿勢の目標軌道を表すベクトル ｗｒｏｄが与えら
れているとき、作業座標系Σｗ で表された手先効果器
２０の目標姿勢を表す３×３行列 ｗＲｄ は、前記（
１２）式により定まる。台車１は走行しているので、こ
の目標姿勢を実現するためには、移動座標系Σｖ で表
された手先効果器２０の姿勢を表す行列 ｖＲＨ が台
車１の姿勢を考慮した下記（１４）式を満たさなければ
ならない。下記（１４）式は、前記行列 ｗＲｄ が、
作業座標系Σｗ で表された台車１の姿勢を表す行列
ｗＲｖ と、移動座標系Σｖ で表された手先効果器２
０の姿勢を表す行列 ｖＲＨ との積に等しいことを表
している。Hand effector 2 expressed in work coordinate system Σw
When a vector wrod representing the target trajectory with a posture of 0 is given, a 3×3 matrix wRd representing the target posture of the hand effector 20 expressed in the work coordinate system Σw is expressed as (
12) Determined by formula. Since the trolley 1 is running, in order to achieve this target posture, the matrix vRH representing the posture of the hand effector 20 expressed in the moving coordinate system Σv must be expressed by the following formula (14), which takes into account the attitude of the trolley 1. must be met. In the following equation (14), the matrix wRd is
A matrix representing the posture of truck 1 expressed in the work coordinate system Σw
wRv and the hand effector 2 expressed in the moving coordinate system Σv
This indicates that it is equal to the product of the matrix vRH representing the orientation of 0.
【００３５】[0035]
【数１４】[Math. 14]
【００３６】移動座標系Σｖ で表された手先効果器２
０の姿勢を表す行列 ｖＲＨ は、前記（１４）式を変
形することによって下記（１５）式の如く求められる。Hand effector 2 expressed in moving coordinate system Σv
The matrix vRH representing the orientation of 0 can be obtained as shown in the following equation (15) by transforming the equation (14).
【００３７】[0037]
【数１５】[Math. 15]
【００３８】また、前記（１２）式の逆関数を用いるこ
とによって、移動座標系Σｖ で表された手先効果器２
０の姿勢の目標軌道を表すベクトル ｖｒｏｄが下記（
１６）式の如く求められる。Furthermore, by using the inverse function of equation (12), the hand effector 2 expressed in the moving coordinate system Σv
The vector vrod representing the target trajectory of attitude 0 is as follows (
16) It can be obtained as shown in the equation.
【００３９】[0039]
【数１６】[Math. 16]
【００４０】前記（１４）式を微分すると、行列 ｗＲ
ｄ の時間微分値は下記（１７）式の如きものとなる。When the above equation (14) is differentiated, the matrix wR
The time differential value of d is as shown in equation (17) below.
【００４１】[0041]
【数１７】[Math. 17]
【００４２】下記（１７）式においては、行列 ｗＲｄ
の時間微分値は前記（１３）式より、行列 ｖＲＨ
は前記（１５）式より、行列 ｗＲｖ は前記（６）
式より既知であり、行列 ｗＲｖ の時間微分値は台車
１の走行速度が走行速度検出器によって測定できること
から、既知である。従って、行列 ｖＲＨ の時間微分
値は前記（１７）式を変形して得られる下記（１８）式
の如く求めることができる。In the following equation (17), the matrix wRd
From equation (13) above, the time differential value of is the matrix vRH
is from equation (15) above, and matrix wRv is from equation (6) above.
The time differential value of the matrix wRv is known from the equation, and the time differential value of the matrix wRv is known because the traveling speed of the bogie 1 can be measured by a traveling speed detector. Therefore, the time differential value of the matrix vRH can be obtained as shown in the following equation (18) obtained by modifying the above equation (17).
【００４３】[0043]
【数１８】[Math. 18]
【００４４】また、行列 ｖＲＨ の時間微分値は前記
（１３）式を用いて、移動座標系Σｖ で表された手先
効果器２０の姿勢の目標軌道を表すベクトル ｖｒｏｄ
をその変数とする関数により、下記（１９）式の如く表
すことができる。Further, the time differential value of the matrix vRH is determined by using the above equation (13), and the vector vrod representing the target trajectory of the posture of the hand effector 20 expressed in the moving coordinate system Σv
It can be expressed as the following equation (19) by a function whose variable is .
【００４５】[0045]
【数１９】[Math. 19]
【００４６】前記（１９）式における行列 ｖＲＨの時
間微分値は前記（１８）式にて求められるので、（１９
）式に行列 ｖＲＨ の時間微分値を代入して（１９）
式を解くことによってベクトル ｖｒｏｄの時間微分値
が得られる。Since the time differential value of the matrix vRH in the above equation (19) is obtained using the above equation (18), (19
) by substituting the time differential value of the matrix vRH into the equation (19)
By solving the equation, the time differential value of the vector vrod can be obtained.
【００４７】以上の如き手順によって、移動座標系Σｖ
で表された手先効果器２０の位置・姿勢の目標軌道と
、その時間微分値とが求められる。By the above procedure, the moving coordinate system Σv
The target trajectory of the position/orientation of the hand effector 20 expressed by , and its time differential value are determined.
【００４８】次に、マニピュレータ２の制御則について
説明する。前述の如く求められた、移動座標系Σｖ で
表された手先効果器２０の位置・姿勢の目標軌道及びそ
の時間微分値とを実現する、マニピュレータ２の関節２
１，２１，…の目標関節角を表すベクトルｑｄ 及びそ
の時間微分値（目標関節角速度）を表すベクトルとは、
前記（４） ，（５） 式を用いて夫々下記（２０），
（２１）式にて表される。なお、ここでは、マニピュレ
ータ２の姿勢は特異姿勢ではない場合を仮定している。Next, the control law for the manipulator 2 will be explained. The joint 2 of the manipulator 2 realizes the target trajectory of the position/orientation of the hand effector 20 expressed in the movement coordinate system Σv and its time differential value, which are obtained as described above.
The vector qd representing the target joint angles of 1, 21, ... and the vector representing its time differential value (target joint angular velocity) are:
Using equations (4) and (5) above, the following (20) and
It is expressed by the formula (21). Note that here, it is assumed that the posture of the manipulator 2 is not a singular posture.
【００４９】[0049]
【数２０】[Math. 20]
【００５０】[0050]
【数２１】[Math. 21]
【００５１】マニピュレータ２の各関節２１，２１，…
は全て、前記サーボモータを含むサーボ系を介して駆動
され、各関節２１，２１，…を駆動するためのサーボ系
への入力電圧を表すベクトルＶは、下記（２２）式で与
えられる。Each joint 21, 21, . . . of the manipulator 2
are all driven via a servo system including the servo motor, and a vector V representing the input voltage to the servo system for driving each joint 21, 21, . . . is given by the following equation (22).
【００５２】[0052]
【数２２】[Math. 22]
【００５３】前記（２２）式において、右辺第１項は、
各関節２１，２１，…の位置フィードバックを表してお
り、前記目標関節角と関節角の実測値との偏差に所定の
制御ゲインを乗じたものであり、また、右辺第２項は、
手先効果器２０の目標軌道の時間微分値であるフィード
フォワード項と、台車１の走行速度のフィードバック項
とを含んでおり、前記目標関節角速度に所定の制御ゲイ
ンを乗じたものである。In the above equation (22), the first term on the right side is
It represents the position feedback of each joint 21, 21,..., and is obtained by multiplying the deviation between the target joint angle and the actual joint angle value by a predetermined control gain, and the second term on the right side is:
It includes a feedforward term which is a time differential value of the target trajectory of the hand effector 20 and a feedback term of the traveling speed of the trolley 1, and is obtained by multiplying the target joint angular velocity by a predetermined control gain.
【００５４】次に、前述の如きマニピュレータ２の位置
・姿勢制御を実施するための制御系の構成について説明
する。図２はマニピュレータ２の位置・姿勢制御を行う
制御系の模式的ブロック図である。Next, the configuration of a control system for controlling the position and orientation of the manipulator 2 as described above will be explained. FIG. 2 is a schematic block diagram of a control system that controls the position and orientation of the manipulator 2. As shown in FIG.
【００５５】図中４１は、前記（７） 式の如き、作業
座標系Σｗで表された手先効果器２０の位置・姿勢の目
標軌道及びその時間微分値を設定する作業座標系目標軌
道設定部であり、該作業座標系目標軌道設定部４１で設
定された前記目標軌道及びその時間微分値の情報は、移
動座標系目標軌道決定部４３へ与えられるようになって
いる。また、台車走行情報演算部４２では、台車１の走
行状態に基づいて台車１の位置・姿勢， 走行速度及び
旋回角速度等の台車１の走行に関する情報を求めるよう
になっており、台車走行情報演算部４２で求められた台
車１の位置・姿勢， 走行速度及び旋回角速度の情報は
、移動座標系目標軌道決定部４３へ与えられるようにな
っている。In the figure, reference numeral 41 denotes a work coordinate system target trajectory setting unit for setting the target trajectory of the position and orientation of the hand effector 20 expressed by the work coordinate system Σw and its time differential value, as shown in equation (7) above. Information on the target trajectory and its time differential value set by the work coordinate system target trajectory setting section 41 is provided to the moving coordinate system target trajectory determining section 43. In addition, the bogie running information calculation unit 42 obtains information regarding the running of the bogie 1, such as the position/attitude of the bogie 1, running speed, and turning angular velocity, based on the running state of the bogie 1, and calculates the bogie running information. The information on the position/attitude, running speed, and turning angular velocity of the bogie 1 determined by the section 42 is provided to the moving coordinate system target trajectory determining section 43.
【００５６】移動座標系目標軌道決定部４３では、作業
座標系目標軌道設定部４１及び台車走行情報演算部４２
から与えられた情報に基づき、前述の如き手順で台車１
の位置・姿勢， 走行速度及び旋回角速度を考慮した、
移動座標系Σｖ で表された手先効果器２０の位置・姿
勢の目標軌道と、その時間微分値とを決定するようにな
っている。移動座標系目標軌道決定部４３で決定された
手先効果器２０の位置・姿勢の目標軌道とその時間微分
値との情報は、関節駆動情報決定部４４に与えられるよ
うになっている。The movement coordinate system target trajectory determining unit 43 includes a work coordinate system target trajectory setting unit 41 and a bogie running information calculating unit 42.
Based on the information given by
Considering the position/attitude of the vehicle, traveling speed, and turning angular velocity,
The target trajectory of the position and orientation of the hand effector 20 expressed in the movement coordinate system Σv and its time differential value are determined. Information about the target trajectory of the position and posture of the hand effector 20 determined by the moving coordinate system target trajectory determining section 43 and its time differential value is provided to the joint drive information determining section 44 .
【００５７】関節駆動情報決定部４４では、移動座標系
目標軌道決定部４３から与えられた情報に基づいて前記
（２０），（２１）式の如く、座標系目標軌道決定部４
３で決定された手先効果器２０の位置・姿勢の目標軌道
とその時間微分値とを実現するマニピュレータ２の関節
角を表すベクトルとその関節角速度を表すベクトルとを
決定するようになっている。関節駆動情報決定部４４で
決定された関節角とその関節角速度との情報は、サーボ
系入力電圧演算部４５に与えられるようになっている。The joint drive information determining section 44 determines the coordinate system target trajectory determining section 4 based on the information given from the moving coordinate system target trajectory determining section 43 as shown in equations (20) and (21) above.
A vector representing the joint angle of the manipulator 2 and a vector representing the joint angular velocity of the manipulator 2 that realize the target trajectory of the position and posture of the hand effector 20 determined in step 3 and its time differential value are determined. Information on the joint angle and its joint angular velocity determined by the joint drive information determining section 44 is provided to a servo system input voltage calculating section 45.
【００５８】サーボ系入力電圧演算部４５では、関節駆
動情報決定部４４から与えられた情報と、サーボ系を含
むマニピュレータ２からフィードバックされたマニピュ
レータ２の関節角の実測値とに基づいて、前記前記（２
２）式の如く、サーボ系への入力電圧を求めるようにな
っている。サーボ系入力電圧演算部４５で求められたサ
ーボ系への入力電圧は、サーボ系を含むマニピュレータ
２に与えられるようになっている。The servo system input voltage calculating section 45 calculates the above-mentioned joint angle based on the information given from the joint drive information determining section 44 and the actual measured value of the joint angle of the manipulator 2 fed back from the manipulator 2 including the servo system. (2
2) As shown in the formula, the input voltage to the servo system is determined. The input voltage to the servo system determined by the servo system input voltage calculating section 45 is applied to the manipulator 2 including the servo system.
【００５９】このように構成された制御系によって、マ
ニピュレータ２は、サーボ系入力電圧演算部４５から与
えられた入力電圧によって駆動制御され、その手先効果
器２０は、移動座標系目標軌道決定部４３で決定された
目標軌道上を移動する。With the control system configured as described above, the manipulator 2 is driven and controlled by the input voltage given from the servo system input voltage calculating section 45, and the hand effector 20 is driven by the moving coordinate system target trajectory determining section 43. Move on the target trajectory determined by.
【００６０】次に、前述の如きマニピュレータの位置・
姿勢制御方法を用いたマニピュレータの制御のシミュレ
ーション実験を行った結果について説明する。このシミ
ュレーション実験では、移動ロボットに対して、作業座
標系Σｗ のｙ軸方向へステップ的に６０ｍｍ／ｓの速
度を与え、その１秒後に移動ロボットを停止させるシミ
ュレーションを、従来の制御方法及び本発明の制御方法
の夫々について行った。図３はこのシミュレーション実
験で走行させた移動ロボットの走行状態を示す模式的斜
視図である。この図３においては、マニピュレータ２及
び台車１の位置・姿勢の初期状態を実線にて示し、これ
らの位置・姿勢の軌跡を所定時間毎に破線にて示してあ
る。Next, the position and position of the manipulator as described above are determined.
The results of a simulation experiment of manipulator control using the posture control method will be explained. In this simulation experiment, the mobile robot was given a speed of 60 mm/s in steps in the y-axis direction of the work coordinate system Σw, and the mobile robot was stopped one second later. Each control method was investigated. FIG. 3 is a schematic perspective view showing the running state of the mobile robot that was run in this simulation experiment. In FIG. 3, the initial states of the positions and postures of the manipulator 2 and the trolley 1 are shown by solid lines, and the loci of these positions and postures are shown at predetermined time intervals by broken lines.
【００６１】まず、従来の制御方法を行った結果につい
て説明する。従来の制御方法は、各関節２１，２１，…
の位置フィードバックのみでマニピュレータ２を制御す
る方法である。即ち、前記（２２）式における右辺の第
２項を零とした制御である。このような従来の制御方法
を行った場合の結果を図４に示す。First, the results of the conventional control method will be explained. In the conventional control method, each joint 21, 21,...
This is a method of controlling the manipulator 2 only by position feedback. That is, this is control in which the second term on the right side of equation (22) is set to zero. The results obtained when such a conventional control method is performed are shown in FIG.
【００６２】図４（ａ） 〜 （ｃ）は従来の制御方法
によるシミュレーション実験を行った場合の手先効果器
２０の位置・姿勢の夫々の偏差の時間的推移を示すグラ
フであり、図４（ａ） 〜 （ｃ）においては、縦軸に
位置・姿勢の偏差、横軸に経過時間を夫々とり、これら
の関係を、図４（ａ） には作業座標系Σｗ のｘ軸方
向について表し、図４（ｂ） には作業座標系Σｗ の
ｙ軸方向について表し、図４（ｃ） には作業座標系Σ
ｗ のｚ軸方向について表してある。即ち、図４（ａ）
においてはｘ軸方向の位置偏差をｅｐｘ、ｘ軸方向の
姿勢偏差をｅｏｘで表し、図４（ｂ） においてはｙ軸
方向の位置偏差をｅｐｙ、ｙ軸方向の姿勢偏差をｅｏｙ
で表し、図４（ｃ） においてはｚ軸方向の位置偏差を
ｅｐｚ、ｚ軸方向の姿勢偏差をｅｏｚで表してある。FIGS. 4(a) to 4(c) are graphs showing the time course of deviations in the position and orientation of the hand effector 20 when simulation experiments were conducted using the conventional control method. In a) to (c), the vertical axis represents the position/posture deviation, and the horizontal axis represents the elapsed time, and the relationship between these is shown in FIG. 4(a) in the x-axis direction of the work coordinate system Σw. Fig. 4(b) shows the y-axis direction of the work coordinate system Σw, and Fig. 4(c) shows the work coordinate system Σw.
It is expressed in the z-axis direction of w. That is, FIG. 4(a)
In Figure 4(b), the position deviation in the x-axis direction is expressed as epx, the attitude deviation in the x-axis direction is expressed as eox, and in Fig. 4(b), the position deviation in the y-axis direction is expressed as epy, and the attitude deviation in the y-axis direction is expressed as eoy.
In FIG. 4(c), the positional deviation in the z-axis direction is expressed as epz, and the attitude deviation in the z-axis direction is expressed as eoz.
【００６３】図４（ａ） 〜 （ｃ）から明らかな如く
、従来の制御方法ではｙ軸方向の位置偏差ｅｐｙ及びｚ
軸方向姿勢偏差ｅｏｚに定常偏差が発生した。As is clear from FIGS. 4(a) to 4(c), in the conventional control method, the positional deviations epy and z in the y-axis direction
A steady deviation occurred in the axial attitude deviation eoz.
【００６４】次に、本発明の制御方法を行った結果につ
いて説明する。本発明の制御方法を行った場合の結果を
図５に示す。Next, the results of implementing the control method of the present invention will be explained. FIG. 5 shows the results obtained when the control method of the present invention was applied.
【００６５】図５（ａ） 〜 （ｃ）は本発明の制御方
法によるシミュレーション実験を行った場合の手先効果
器２０の位置・姿勢の夫々の偏差の時間的推移を示すグ
ラフであり、図５（ａ） 〜 （ｃ）においては、縦軸
に位置・姿勢の偏差、横軸に経過時間を夫々とり、これ
らの関係を、図５（ａ） には作業座標系Σｗ のｘ軸
方向について表し、図５（ｂ） には作業座標系Σｗ
のｙ軸方向について表し、図５（ｃ） には作業座標系
Σｗ のｚ軸方向について表してある。即ち、図５（ａ
） においてはｘ軸方向の位置偏差をｅｐｘ、ｘ軸方向
の姿勢偏差をｅｏｘで表し、図５（ｂ） においてはｙ
軸方向の位置偏差をｅｐｙ、ｙ軸方向の姿勢偏差をｅｏ
ｙで表し、図５（ｃ） においてはｚ軸方向の位置偏差
をｅｐｚ、ｚ軸方向の姿勢偏差をｅｏｚで表してある。FIGS. 5(a) to 5(c) are graphs showing the temporal changes in the deviations in the position and posture of the hand effector 20 when a simulation experiment was conducted using the control method of the present invention. In (a) to (c), the vertical axis represents the position/posture deviation, and the horizontal axis represents the elapsed time, and these relationships are expressed in the x-axis direction of the work coordinate system Σw in Fig. 5(a). , Figure 5(b) shows the work coordinate system Σw
FIG. 5C shows the z-axis direction of the work coordinate system Σw. That is, FIG. 5(a)
), the position deviation in the x-axis direction is expressed as epx, the attitude deviation in the x-axis direction is expressed as eox, and in Fig. 5(b), y
The positional deviation in the axial direction is epy, and the attitude deviation in the y-axis direction is eo.
In FIG. 5(c), the positional deviation in the z-axis direction is expressed as epz, and the attitude deviation in the z-axis direction is expressed as eoz.
【００６６】図５（ａ） 〜 （ｃ）から明らかな如く
、本発明の制御方法では、従来の制御方法で発生したよ
うな定常偏差は発生しなかった。As is clear from FIGS. 5(a) to 5(c), in the control method of the present invention, the steady-state deviation that occurred in the conventional control method did not occur.
【００６７】[0067]
【発明の効果】以上詳述した如く、本発明に係るマニピ
ュレータの位置・姿勢制御方法及び装置では、マニピュ
レータの位置及び姿勢の制御量のうち、マニピュレータ
の位置及び姿勢の目標値の時間微分値に基づいて得られ
る制御量の要素は、移動体の移動によって生じていく、
前記目標値からのマニピュレータの位置及び姿勢のずれ
量を予測してこれを補償するので、その目標値に対する
マニピュレータの位置・姿勢の定常偏差を抑制でき、位
置・姿勢制御の精度を向上させることが可能となる等、
優れた効果を奏する。Effects of the Invention As described in detail above, in the manipulator position/attitude control method and device according to the present invention, the time differential value of the target value of the manipulator position and attitude is determined from among the control amount of the manipulator position and attitude. The elements of the control amount obtained based on this are caused by the movement of the moving object.
Since the amount of deviation of the position and orientation of the manipulator from the target value is predicted and compensated for, the steady deviation of the position and orientation of the manipulator from the target value can be suppressed, and the accuracy of position and orientation control can be improved. It becomes possible, etc.
It has excellent effects.
【図１】本発明に係るマニピュレータの位置・姿勢制御
方法の実施に使用する移動ロボットの斜視図である。FIG. 1 is a perspective view of a mobile robot used to implement a manipulator position/attitude control method according to the present invention.
【図２】マニピュレータの位置・姿勢制御を行う制御系
の模式的ブロック図である。FIG. 2 is a schematic block diagram of a control system that controls the position and orientation of a manipulator.
【図３】シミュレーション実験で走行させた移動ロボッ
トの走行状態を示す模式的斜視図である。FIG. 3 is a schematic perspective view showing the running state of a mobile robot that was run in a simulation experiment.
【図４】従来の制御方法によるシミュレーション実験を
行った場合の手先効果器の位置・姿勢の夫々の偏差の時
間的推移を示すグラフである。FIG. 4 is a graph showing the time course of deviations in the position and posture of the hand effector when a simulation experiment is conducted using a conventional control method.
【図５】本発明の制御方法によるシミュレーション実験
を行った場合の手先効果器の位置・姿勢の夫々の偏差の
時間的推移を示すグラフである。FIG. 5 is a graph showing the time course of deviations in the position and posture of the hand effector when a simulation experiment is performed using the control method of the present invention.
１ 台車２ マニピュレータ４２ 台車走行情報演算部４３ 移動座標系目標軌道決定部４４ 関節駆動情報決定部４５ サーボ系入力電圧演算部1 Trolley2 Manipulator42 Bogie travel information calculation unit43 Moving coordinate system target trajectory determination unit44 Joint drive information determination unit45 Servo system input voltage calculation section
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP16216291AJP3234928B2 (en) | 1991-06-05 | 1991-06-05 | Manipulator position / posture control device |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP16216291AJP3234928B2 (en) | 1991-06-05 | 1991-06-05 | Manipulator position / posture control device |
| Publication Number | Publication Date |
|---|---|
| JPH04360779Atrue JPH04360779A (en) | 1992-12-14 |
| JP3234928B2 JP3234928B2 (en) | 2001-12-04 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP16216291AExpired - Fee RelatedJP3234928B2 (en) | 1991-06-05 | 1991-06-05 | Manipulator position / posture control device |
| Country | Link |
|---|---|
| JP (1) | JP3234928B2 (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2016103303A1 (en)* | 2014-12-26 | 2016-06-30 | 川崎重工業株式会社 | Self-propelled articulated robot |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2016103303A1 (en)* | 2014-12-26 | 2016-06-30 | 川崎重工業株式会社 | Self-propelled articulated robot |
| JPWO2016103303A1 (en)* | 2014-12-26 | 2017-07-20 | 川崎重工業株式会社 | Self-propelled joint robot |
| CN107000199A (en)* | 2014-12-26 | 2017-08-01 | 川崎重工业株式会社 | Self-propelled joint Manipulator |
| US10338597B2 (en) | 2014-12-26 | 2019-07-02 | Kawasaki Jukogyo Kabushiki Kaisha | Self-traveling articulated robot |
| CN107000199B (en)* | 2014-12-26 | 2020-04-17 | 川崎重工业株式会社 | Self-propelled joint manipulator |
| Publication number | Publication date |
|---|---|
| JP3234928B2 (en) | 2001-12-04 |
| Publication | Publication Date | Title |
|---|---|---|
| US8774969B2 (en) | Method for allowing a manipulator to cover a predetermined trajectory, and control device for carrying out said method | |
| Yamamoto et al. | Modeling and compensation of the dynamic interaction of a mobile manipulator | |
| JPH05318354A (en) | Position-attitude control method of manipulator | |
| WO2007069401A1 (en) | Gait creation device of leg-type mobile robot | |
| Maulana et al. | Inverse kinematic implementation of four-wheels mecanum drive mobile robot using stepper motors | |
| JPH079606B2 (en) | Robot controller | |
| JP2006136962A (en) | Mobile robot | |
| Oftadeh et al. | A novel time optimal path following controller with bounded velocities for mobile robots with independently steerable wheels | |
| Li et al. | Design and analysis of a whole-body controller for a velocity controlled robot mobile manipulator | |
| Singh et al. | Control of closed-loop differential drive mobile robot using forward and reverse Kinematics | |
| Seraji | Configuration control of rover-mounted manipulators | |
| CN112873198A (en) | Control method of robot system | |
| Reister | A new wheel control system for the omnidirectional HERMIES-III robot | |
| JP3223288B2 (en) | Position and attitude control method for mobile manipulator | |
| Weiguo et al. | Optimal motion planning for a wheeled mobile robot | |
| JPH04360779A (en) | Manipulator position-attitude control method and device therefor | |
| Xu et al. | Trajectory tracking control of slidable-wheel omnidirectional mobile robot based on linear model predictive control | |
| Luo et al. | On-line adaptive control for minimizing slippage error while mobile platform and manipulator operate simultaneously for robotics mobile manipulation | |
| JP3316421B2 (en) | Mobile robot | |
| Widyotriatmo | Comparative study of stabilization controls of a forklift vehicle | |
| JP2006192564A (en) | Work assistance device and work assistance method | |
| JP2019109862A (en) | Drive steering device of unmanned carrier | |
| JPH04244389A (en) | Method and device for controlling position/attitude of manipulator | |
| Sato et al. | Longitudinal and turning manual operation control for a force sensorless power-assisted transport cart | |
| CN111994169A (en) | Motion control method of independently driven and steered performance trolley based on position compensation |
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |