JP3206765B2 - Control method of master-slave manipulator - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a method for controlling a unilateral master / slave manipulator in which a master arm is passive and a slave arm is active.

[0002]

2. Description of the Related Art Conventional methods for determining the attitude of a slave arm when the master arm and the slave arm have similar mechanical structures (hereinafter referred to as the same structure) can be classified into two methods. . One is a method of applying a control method such as a symmetric type between motors of corresponding joints. In the case where the control system of the master / slave is formed between the corresponding joints as described above, since the coordinate conversion to the joint coordinate system is not necessary, the problem described in the next section does not occur (FIG. 3). However, this method has a problem that the slave arm cannot easily set the compliance characteristic based on the coordinate system of the object. The other is to set a coordinate system based on the work target at the tip of the manipulator (for example, "Research on Control of Master-Slave Manipulator" Sato et al., The 7th Robotics Society of Japan Lecture No. 1211 ). By using the work coordinate system of the hand as a reference in this way, it is possible to easily set compliance, impedance, and the like according to the work coordinate system. This has advantages over the previously described joint-based method. However, in this method, since a motion command of the actuator is finally required, a motor position command of the joint coordinate system is determined by applying a mechanism reverse conversion method corresponding to each mechanism (FIG. 4). ). In the process of this inverse transformation, the problems described in the next section arise.

[0003]

The method using the mechanical inversion described in the prior art can be applied when the conversion from the hand to the joint coordinate system can be uniquely determined. For example, when converting the position and orientation of the robot hand into a joint angle in a three-dimensional space, the joint must have at least six degrees of freedom in order to achieve a total of six degrees of freedom, three degrees of freedom of position and three degrees of posture. Requires freedom of movement. Conversely, if there are joints with 6 degrees of freedom or more, there are a finite number of joint angle solutions in one or more sets that satisfy the position and orientation of the hand. However,
When the degree of freedom of the joint is greater than the degree of freedom of the hand, there is an infinite set of joint angles corresponding to one given hand position / posture. In such a manipulator, generally, it is not possible to determine the joint angle only from information on the position and orientation of the hand. A manipulator having more joint degrees of freedom than a hand degree of freedom is called a manipulator having redundant degrees of freedom, and is considered to be a manipulator that includes useless degrees of freedom in realizing the position and orientation of the hands. By taking advantage of the fact that various arm postures can be taken with respect to the hand position / posture, useful operations such as obstacle avoidance and wraparound can be performed. Therefore,
There is a need for a method of disassembling the motion from the hand to the joint, which effectively utilizes the redundancy degree of freedom. In particular, when the slave arm has a compliance characteristic or the like and takes a different posture between the master and the slave, the joint angle command of the slave as close as possible to the master posture must be determined. Therefore,
An object of the present invention is to provide a method for determining a joint angle command of a slave as close as possible to the master posture when the master and the slave take different postures.

[0004]

[MEANS FOR SOLVING THE PROBLEMS] To solve the above problems.
In addition, the present invention provides a master having a similar mechanical configuration to each other.
Arm and a slave arm.
The slave arm according to the external force applied to the hand of the arm.
Compliance that shifts the tip of the
A master-slave manipulator provided with
The joint angle command value of the slave arm is changed according to the external force.
Add the master arm to the slight deviation of the tip of the slave arm.
A the amount obtained by multiplying the inverse matrix of the Jacobian matrix obtained from the posture of the arm
It should be added to the joint angle of the arm.

[0005]

[Function] Since the master-slave arm of the same structure indicates the norm of the attitude of the slave, the slave arm takes a posture as close as possible to the master, thereby performing control using the characteristics of the same structure most effectively. be able to. By the way, the pseudo Jacobian matrix is a function matrix that represents the minute displacement relationship between the position / posture of the hand and the joint angle. Therefore, when the Jacobian matrix of the master is regarded as a function representing the joint angle posture of the master, the joint angle posture of the slave arm approximate to the master arm can be determined by using this matrix. Since the Jacobian used here is not the Jacobian of the slave arm but the Jacobian of the master arm, the convergence of the solution in the kinematic decomposition is guaranteed.

[0006]

EXAMPLES Specific examples will be described below. FIG. 1 is a block diagram of a control system in a case where impedance control based on feedback of a force sensor mounted on a slave arm hand is performed as a force control method on the slave side in an embodiment of the present invention. For the joint angle θ _m of the passive master, the forward transformation unit 1 obtains the position and orientation X _m of the hand in the work coordinate system. The slave force detection value and the slave joint angle θ _s are input to the force coordinate conversion unit 2 to obtain a force F _ex in the working coordinate system. The F _ex and said X _m are input to the impedance control operation unit 3 to obtain the position and orientation error e _k between the master slave. The calculation formula for obtaining e _k is as follows.

[0007]

(Equation 1)

For the joint angle θ _m of the master arm, the pseudo Jacobian arithmetic unit 4 obtains the (pseudo) inverse matrix J _m ⁺ of the Jacobian of the slave arm. The e _k and the J _m ⁺ are input to a multiplier 5, multiplied, and then added to an adder 6 to a joint angle θ _m of a master arm to obtain a joint position command θ _sr of a slave arm. The above flow is represented by the following equations. θ _sr = θ _m + J _m ⁺ e _(k) θ _sr : Slave joint angle command (n · 1) θ _m : Master joint angle (n · 1) J _m ⁺ : Jacobian (pseudo) inverse matrix of slave arm ( e) Position / posture error between master and slave (work coordinate system)
(M · 1) where (•) represents the dimensions of the matrix and the vector. n: degrees of freedom of joints m: degrees of freedom of control of the hand

That is, the slave arm is slightly displaced from the master arm due to compliance characteristics. The error e in the working coordinate system at that time is converted into a minute displacement in the joint coordinate system using the Jacobian of the master arm.
Since the reference posture is the joint angle of the master arm, the joint angle command of the slave arm can be determined by adding a small displacement to each joint angle of the master arm. Since the Jacobian used here uses the Jacobian of the master arm instead of the Jacobian of the slave arm, the convergence of the solution in the kinematic decomposition is guaranteed. This makes it possible to determine a joint angle command for the posture of the slave arm that is similar to the posture of the master arm. FIG. 2 shows the result when the present algorithm is applied to a three-degree-of-freedom redundant arm. In the stick diagram, the dotted line indicates the posture of the master arm,
The solid line shows (a) the result of the kinematic decomposition performed by the method of the present algorithm, and (b) shows the result obtained by fixing the root first axis and finding the mechanical inverse solution with the remaining two axes. , -X
2 shows two types of results when the hand moves in the axial direction. (C) is an evaluation of the result of the coordinate transformations of (a) and (b) by the sum of squares of the joint angle displacement, and (a) shows that the joint displacement is smaller and the approximation characteristics to the master arm are better. Understand.

[0010]

As described above, according to the present invention, in a master-slave manipulator in which the slave arm has a flexible control characteristic with respect to the external force such that the position of the slave arm is shifted by the external force, Follow-up control can be performed as close as possible to the master arm. In particular, when applied to an arm having a redundant degree of freedom, unilateral master-slave control that effectively uses obstacle avoidance and wraparound can be performed, and workability can be greatly improved.

[Brief description of the drawings]

FIG. 1 is a control block diagram in a case where the present algorithm is applied to a unilateral master / slave arm whose slave arm is impedance-controlled.

FIG. 2 is a graph showing a change in arm posture and an evaluation graph when coordinate conversion of a redundant arm is performed by an algorithm of the present invention and when coordinate conversion is performed by another method;

FIG. 3 is a block diagram of a non-redundant unilateral master-slave control method capable of finding a solution by inverse mechanics

FIG. 4 is a block diagram of a conventional redundant unilateral master / slave control method for controlling joints with the same structure.

[Explanation of symbols]

 Reference Signs List 1 forward conversion unit 2 force coordinate conversion unit 3 impedance control calculation unit 4 pseudo Jacobian calculation unit 5 multiplier 6 adder

Claims (1)

(57) [Claims] 1. A master having a similar mechanical structure to each other.
Arm and a slave arm.
The slave arm according to the external force applied to the hand of the arm.
Compliance that shifts the tip of the
In the master-slave manipulator provided, the joint angle command value of the slave arm is changed according to the external force.
The small deviation of the tip of the slave arm
Multiplied by the inverse of the Jacobian matrix found from
The amount added to the joint angle of the master arm
Of master-slave manipulator with features
Law.