KR20110109413A - Constraint Space Calculation Method Using Position Sensor of Articulated Manipulator - Google Patents

Abstract

The present invention relates to a method for calculating a confined space using a position sensor of an articulated manipulator. More particularly, the present invention relates to a method of calculating a joint angle by attaching a position sensor to a manipulator, and manipulator due to an external obstacle or a link collision of the manipulator itself. The present invention relates to a limited space calculation method for calculating spatial information in which movement of a motion is limited. In the method of calculating the limited space using the position sensor of the articulated manipulator, a method for controlling the motion of the manipulator is calculated by calculating a space in which the motion is restricted through the position sensor of the articulated manipulator. Generating a target position of the manipulator in the device to drive the manipulator; After calculating the joint angle of the manipulator through the position sensor, by converting the joint angle into the attitude vector of the manipulator end, and calculating the attitude error compared to the target attitude vector of the manipulator, the driven manipulator has Calculating a space in which movement is limited due to insufficient degrees of freedom; After calculating the joint angle of the manipulator through the position sensor, the joint angle is calculated by comparing the joint angle with the target joint angle of the manipulator, and summed with the calculated attitude error so that the driven manipulator is external. Calculating a space in which movement is limited due to an obstacle or a link collision of the manipulator; And optimizing the joint arrangement of the manipulator using the calculated constraint space information.

Description

Constrained space calculation method using position sensor of articulated manipulator {RESTRICTION SPACE CALCULATION METHOD USING POSITION SENSORS OF MULTI DEGREES-OF-FREEDOM MANIPULATOR}

The present invention relates to a method for calculating a confined space using a position sensor of an articulated manipulator. More particularly, the present invention relates to a method of calculating a joint angle by attaching a position sensor to a manipulator, and manipulator due to an external obstacle or a link collision of the manipulator itself. The present invention relates to a limited space calculation method for calculating spatial information in which movement of a motion is limited.

With advances in the technology of remote control robots, it is possible to work remotely in environments that have been difficult for humans to access in the past. . In order to move a manipulator such as a robot according to a human intention, a manipulation signal is input through a master device which is moved by a human-robot interaction such as a haptic device, or recently, an electromyogram (EMG) or an electroencephalogram (EEG) and Manipulators can be driven by identifying human intentions using the same equipment.

When the manipulator moves in a normal environment instead of an artificially processed environment, it enters an inoperable space due to obstacles in the environment or link collision of the manipulator, according to the operator's intention through the master device. This happens. Despite the inability of the manipulator to enter this restriction space, if the operator transmits a signal that forces the manipulator through the master device, the stability of the entire system due to the position error between the master device and the manipulator It cannot be guaranteed.

Therefore, conventionally, the manipulator is prevented from entering the confined space in the form of repulsive force through a force sensor or a position sensor, and information about whether the manipulator has entered the confined space is transmitted to the operator.

The calculation of the confined space by the force sensor is good in terms of accuracy, but since the price of the force sensor is expensive, it is inefficient to attach a sufficient number of force sensors to the manipulator to detect all possible collisions in the surrounding environment. . However, if only a small number of force sensors are attached and fail to detect collisions that need to be detected, stability is compromised in the environment where the manipulator is applied. For example, in a remote control system, if a manipulator such as a robot collides with an obstacle around it, but the operator is not provided with a repulsive force through the master device, the operator continues to transmit a manipulation signal through the master device, and the manipulator is Since it does not move, the position error becomes large, and the stability of the whole system is not guaranteed.

In the case of calculating a limited space using a conventional position sensor, a position sensor, an angle sensor, an encoder, a hall sensor, and the like are attached to the manipulator to continuously grasp the angle and position information of the joint of the manipulator, and the operator Compared with the information about the target position to enter into, the master device transmits the presence of the limited space in the form of repulsive force. However, this method allows the operator to determine the existence of the constraint space due to the repulsive force generated in the master device, but has a problem in that the articulated manipulator cannot calculate the direction of the constraint space.

An object of the present invention, using the position sensor attached to the manipulator, calculates the presence of the limited space and the direction information of the limited space with respect to the space in which the movement of the manipulator is limited, and transmits it to the operator in the form of a repulsive force that is a vector. Rather, it is to optimize the joint placement of the manipulator in consideration of the confined space.

Constrained space calculation method using a position sensor of the articulated manipulator according to an embodiment of the present invention for achieving the above object calculates a space in which the movement is limited by the position sensor of the articulated manipulator, the movement of the manipulator CLAIMS 1. A method for controlling, comprising: generating a target position of the manipulator in a master device to drive the manipulator; After calculating the joint angle of the manipulator through the position sensor, by converting the joint angle into the attitude vector of the manipulator end, and calculating the attitude error compared to the target attitude vector of the manipulator, the driven manipulator has Calculating a space in which movement is limited due to insufficient degrees of freedom; After calculating the joint angle of the manipulator through the position sensor, the joint angle is calculated by comparing the joint angle with the target joint angle of the manipulator, and summed with the calculated attitude error so that the driven manipulator is external. Calculating a space in which movement is limited due to an obstacle or a link collision of the manipulator; And optimizing the joint arrangement of the manipulator using the calculated constraint space information.

The method for calculating a limited space using a position sensor of the articulated manipulator includes: converting a space in which movement is limited due to insufficient degrees of freedom of the driven manipulator to a first repulsive force (F _RG ); Converting, by the driven manipulator, a space in which movement is limited due to an external obstacle or a link collision of the manipulator to a second repulsive force F _RE ; And adding the first repulsive force F _RG and the second repulsive force F _RE to the master device.

The first repulsive force F _RG may be calculated according to Equation 1 below.

[Equation 1]

F _RG = R _G e _sx = -K _G (IJ _s J _s ^# ) e _sx

Where R _G is a first mapping matrix that maps the temporary limited space (IRS) due to the lack of freedom of the manipulator to the repulsive force, e _sx is the attitude error at the end of the manipulator, and K _G is used to adjust the magnitude of the repulsive force F _RG Scalar force gain, I is the identity matrix, Jacobian J _s = ∂x _s / ∂q _s Is, x _s Is the attitude vector at the end of the manipulator, q _s is the joint angle vector of the manipulator, and J _s ^# is the pseudo-inverse of J _s .

The second repulsive force F _RE may be calculated according to Equation 2 below.

[Equation 2]

F _RE = R _E e _sx = -K _E {(IJ _{s θ} J ^# _sθ ) J _s J ^# _s } e _sx

Here, R _E is a second mapping matrix that maps the temporary limited space (IRS) due to the external constraint of the manipulator to the repulsive force, e _sx is the attitude error at the end of the manipulator, and K _E is to adjust the size of the repulsive force F _RE . Is the scalar force gain, I is the identity matrix, and Jacobian J _s = ∂x _s / ∂q _s Is, x _s Is the vector orientation of the manipulator terminal, and the joint angle vector q _s is a manipulator, J ^# _s is a pseudo inverse matrix of the J _{s, θ s} is J J _s is the inherited Jacobian, and J ^# _sθ is the J _{s θ} It is a pseudo inverse.

A plurality of force sensors are attached to the manipulator, and by detecting an external obstacle in the force sensor, converting the space where the movement of the manipulator is limited to a third repulsive force (F _EF ), and the third repulsive force (F _EF) ) May be added to the first repulsive force F _RG and the second repulsive force F _RE , and may be transmitted to the master device.

)를 적분기를 통해 적분하여 획득하며, 상기 목표 조인트 속도(

)의 산출은 제한공간의 정보를 이용하여 매니퓰레이터의 관절 배치를 최적화하기 위하여 하기 수학식 5에 따라 이루어질 수 있다. The target joint angle of the manipulator is the target joint speed of the manipulator (

) Is obtained by integrating through an integrator, and the target joint velocity (

) Can be calculated according to Equation 5 below to optimize the joint placement of the manipulator using the information of the constraint space.

[Equation 5]

은 단위 행렬이며,

이며,

,

이며, e_sq는 조인트 오차이며, q_sd는 목표 조인트 각도이며, q_s는 조인트 각도이며, k₁ 및 k₂는 외력에 대한 매니퓰레이터의 감도(sensitivity) 및 안정성(stability)을 결정하는 상수들이다.here,

Is an identity matrix,

Is,

,

Where e _sq is the joint error, q _sd is the target joint angle, q _s is the joint angle, and k ₁ and k ₂ are constants that determine the sensitivity and stability of the manipulator to external forces.

The method of calculating the confined space using the position sensor of the articulated manipulator is applied to a haptic device, teleoperation, human-robot interaction, prosthetic, etc. When there is an obstacle or link collision of the manipulator itself in the unstructured environment of the system, the restriction space or constraint space with the position sensor alone, or an immovable space, for example, the manipulator By calculating the space that can no longer be moved and transmitting it to the operator in the form of repulsive force and optimizing the joint arrangement of the manipulator using the information of the limited space, there is an effect that can prevent malfunction or failure due to excessive manipulation of the manipulator. .

In addition, the method of calculating the confined space using the position sensor of the articulated manipulator of the present invention has an effect that can be applied without additional manipulation even if the shape and the degrees of freedom of the master device and the manipulator in the remote place are different.

In addition, the method of calculating the confined space using the position sensor of the articulated manipulator has an effect of ensuring stable human-robot interaction of the manipulator. For example, when the present invention is applied to a prosthesis to calculate a space in which the prosthetic movement is limited, the prosthetic can be prevented from colliding with an obstacle or another person regardless of the intention of the operator, so that safe operation without a force sensor can be prevented. It becomes possible.

In addition, in the method of calculating the limited space using the position sensor of the articulated manipulator of the present invention, when the limited space is calculated and converted into the repulsive force, the repulsive force is composed of vectors, so that not only the existence of the limited space but also information on the direction of the limited space There is also an effect that can be provided.

1A to 1C are views for explaining the concept of the temporary confined space IRS and the temporary moving space IMS of the articulated manipulator.
2 is a block diagram illustrating a method of calculating a confined space using a position sensor of an articulated manipulator according to an exemplary embodiment of the present invention.
FIG. 3 is a view for explaining a process of converting information about a limited space calculated by the method of FIG. 2 into repulsive force.

Hereinafter, with reference to the accompanying drawings will be described in detail a method of calculating the limited space using the position sensor of the articulated manipulator according to the preferred embodiments of the present invention.

1A to 1C are views for explaining the concept of the temporary confined space IRS and the temporary moving space IMS of the articulated manipulator.

While moving in free space, there is no limit to the movement of the manipulator, and therefore the operator feels no repulsive force at all. However, if the movement of the manipulator is limited, the operator needs to sense the repulsive force so that the operator does not move the remote control device to the limited space.

Therefore, the purpose of the two-way remote control system is to create a confined space equal to the confined space on the manipulator side, rather than matching the distal position of the manipulator with the distal position of the manipulator, such as the method used in the position error feedback method. .

1A-1C, an illustration of a temporary confined space of a manipulator in a two-dimensional x-y Cartesian space is shown.

In FIG. 1A, the obstruction 20 limits the distal movement of the manipulator 10 in the x-axis direction. Therefore, the manipulator 10 can move only in the y-axis direction instantaneously, which is called an instantaneous motion space (IMS). Algebraic complement of IMS is defined as Temporary Constrained Space (IRS). Thus, in FIG. 1A the x-axis direction is in the IRS.

In FIG. 1B, the positive y-axis movement of the first joint 30 is limited by the obstacle 50 so that the movement space is controlled by the second joint 40 in the x-axis direction and y of the first joint 30. Extends in the axial negative direction. Thus, the temporarily generated IRS is in the positive y-axis direction.

1C shows the IRS generated due to the lack of freedom of manipulator 60 relative to the dimensions of a given task space. In this case, even if the task space is a two-dimensional x-y space, the manipulator 60 has only one degree of freedom, and thus the movement space of the manipulator 60 cannot extend over the entire task space. The IRS in FIG. 1C is in the direction of contact with the link 62, that is, in the y-axis direction.

1A and 1B, the IRS is generated by external constraints such as obstacles 20 and 50 and link 32 collisions. In FIG. 1C, the IRS is generated with insufficient degrees of freedom compared to a given task space. Therefore, the IRS may be defined as follows.

IMS: the convex cone of all possible velocity vectors that the manipulator can achieve kinematically

IRS: Algebraic Complement of IMS

IRS _G : IRS generated by the lack of freedom of the manipulator

IRS _E : IRS generated by external restrictions

IMS and IRS can be defined mathematically as follows when the joint is restricted in both directions.

IMS = R (J _{s θ} )

IRS _G = R (J _s ) ^⊥

IRS _E = R (J _s ) ∩ R (J _{s θ} ) ^⊥

Here, R (·) represents a space range of (·), and (·) ^⊥ represents an orthogonal complementary space. Also, Jacobian J _s = ∂x _s / ∂q _s Where x _s Represents the pose vector at the end of the manipulator, q _s Denotes the joint angle vector of the manipulator.

J _{θ s} is defined as a genetic Jacobian (inherited Jacobian) of the J _s, columns of J _{s θ} corresponding to limited joint are replaced with zero column vector.

To detect constrained joints, a predetermined threshold ε is introduced to determine if the joint angle error is due to movement limitation by obstacles or due to free movement control error. In other words, if the joint angle error is greater than the threshold ε, then the column of J _{s θ} is replaced with a zero vector. The threshold ε must be larger than the control error in the free moving space without the expected confined space.

R (J _s ) ^을 represents a limited space that is not reachable because of the lack of freedom of the manipulator, i.e., IRS _G , even without external limitations. If the movement of the manipulator is limited, IMS becomes R (J _{s θ} ) and IRS _E Becomes R (J _s ) ∩ R (J _{s θ} ) ^⊥ .

Hereinafter, the calculation method of the limited space will be described.

FIG. 2 is a block diagram illustrating a method for calculating a limited space using a position sensor of an articulated manipulator according to an embodiment of the present invention, and FIG. 3 illustrates information about a limited space calculated by the method of FIG. It is a figure for demonstrating the process of converting.

Referring to FIG. 2, the operator 110 inputs a manipulation force for moving the manipulator 140 to a target position into the master device 120. The master device 120 remotely transmits the information about the target position to the manipulator controller 130, and the manipulator 140 is driven according to the control signal of the manipulator controller 130.

The driving manipulator 140 causes action / reaction with the surrounding environment according to the position of the joint and the link, and when the manipulator 140 enters the restricted space, movement is limited.

Subsequently, the position sensor attached to the manipulator 140 detects the relative position of the joint and transmits it to the limited space calculator 160. The limited space calculator 160 integrates information about the attitude of the manipulator 140 and the joint angle of the manipulator 140 to calculate a space in which the movement of the manipulator 140 is restricted. The information about the constraint space calculated by the constraint space calculator 160 may be converted into, for example, a form of force reflection. At this time, the manipulator controller 130 generates a control signal in a direction to optimize the joint arrangement of the manipulator in consideration of the calculated constraint space.

Thereafter, the limited space calculator 160 transmits the converted repulsive force to the master device 120 or the operator 110, and the operator 110 senses the repulsive force, so that the manipulation 140 enters the restricted space. You can check whether or not and the direction of the confined space.

Meanwhile, the information about the limited space calculated by the limited space calculator 160 may be converted and used according to various algorithms in addition to the aforementioned repulsive force. For example, when information about a confined space is applied to the prosthesis, an algorithm for stopping the prosthetic movement may be used if the prosthesis collides with an external obstacle or an external object approaches and contacts the prosthesis.

The process of converting the information about the limited space into the form of repulsive force by the limited space calculating unit 160 will be described in detail later with reference to FIG. 3.

The present invention relates to a novel position-sensor-based repulsive method that represents an accurate IRS, and introduces RSP (restriction space projection) matrices to produce an IRS in the form of repulsive force.

As described above, the IRS _G may be calculated from Jacobian, ie, J _S of the manipulator 140. IRS _E can be calculated from J _{s θ} using the measured joint error shape and J _S.

When the target position is in the IRS, the movement limitation of the manipulator 140 causes a control error. In the RSP method of the present invention, the task error is projected to the IRS to produce a repulsive force. Two RSP matrices, such as R _G and R _E, are formed to yield IRS _G and IRS _E.

From the definition of IRS _G , the first mapping matrix R _G that maps the IRS due to the lack of freedom of the manipulator 140 to the repulsive force is defined as follows.

R _G : e _sx → F _RG

R _G = -K _G (IJ _S J _S ^# )

Thus, the repulsive force F _RG due to the lack of freedom of the manipulator 140 Is equal to the following equation.

[Equation 1]

F _RG = R _G e _sx = -K _G (IJ _s J _s ^# ) e _sx

Where (·) ^# is a pseudo-inverse of (·), and e _sx (= x _d -x _s ) is the attitude error at the end of the manipulator. Where x _d Is the target pose vector, x _s Is the attitude vector at the end of the manipulator. K _G is a scalar force gain for adjusting the magnitude of the repulsive force F _RG , and I is an identity matrix.

Similarly to the above, from the definition of IRS _E , the second mapping matrix R _E which maps the IRS with repulsive force due to external constraints of the manipulator 140 can be defined as follows.

R _E : e _sx → F _RE

R _E = -K _E (IJ _{s θ} J ^# _sθ ) J _s J ^# _s

Therefore, the repulsive force F _RE due to the external limitation of the manipulator 140 is expressed by the following equation.

[Equation 2]

F _RE = R _E e _sx = -K _E {(IJ _{s θ} J ^# _sθ ) J _s J ^# _s } e _sx

Where K _E is a scalar force gain.

In addition, J _S in the said Formulas 1 and 2 can be calculated | required by the following formula.

&Quot; (3) "

J _{s θ} = J _s D

이며, here,

Is,

이다.

to be.

Therefore, the total repulsive force F _R can be calculated as follows.

&Quot; (4) "

_R = _RG + F _RE F F R = _G + R _E e e _{sx sx}

=-(K _G (IJ _s J _s ^# ) + K _E (IJ _{s θ} J ^# _sθ ) J _s J ^# _s } e _sx

Although the proposed RSP method yields the direction of the confined space, the size cannot be estimated. Therefore, the operator should change K _E and K _G to suit the application considering the weight between F _RG and F _RE . Since F _RG and F _RE exist in orthogonal complementary spaces, the force gains K _E and K _G can be adjusted independently.

가 전달되면, 매니퓰레이터(140)의 공간 범위, 즉, R(I-R_G)로 투사된다. 다음으로, 목표 조인트 속도가 투사된 목표 명령

로부터 산출되며, 매니퓰레이터(140)의 목표 조인트 각도 q_sd가 역기구학(inverse kinematics, IK)을 통해 산출된다. 매니퓰레이터 제어기(130), 즉, K_s는 제한공간을 고려하여 매니퓰레이터(140)의 관절 배치를 최적화하면서 목표 위치를 추종하도록 한다. 동시에, 장애물 충돌로 인해 외부 토크 τ_ext가 매니퓰레이터(140)의 조인트로 분배된다. In FIG. 3, a target command from the master device 120

Is transmitted, it is projected into the spatial range of manipulator 140, ie R (IR _G ). Next, the target command projected to the target joint speed

The target joint angle q _sd of the manipulator 140 is calculated through inverse kinematics (IK). Manipulator controller 130, that is, K _s to follow the target position while optimizing the joint arrangement of the manipulator 140 in consideration of the confined space. At the same time, the external torque τ _ext is distributed to the joint of the manipulator 140 due to the obstacle collision.

P _S represents the dynamics of the manipulator 140. The attitude error e _sx is calculated from the target attitude vector x _d and the attitude vector x _{s at the} manipulator end. The variable q _s is the joint angle of the manipulator 140. FK represents the forward kinematic mapping from the joint angle vector q _s to the pose vector x _s of the manipulator end. R _G can be calculated from Jacobian J _s .

Next, the _RG = R F _G e _sx. From J _s and joint error e _sq , J _{s θ} can be calculated. F is _RE = R _E e _sx. F _EF is the force signal detected by the force sensor.

가 결정되면, 역기구학(IK)을 통해 출력되는 목표 조인트 속도

를 하기와 같이 산출할 수 있다.

Is determined, the target joint velocity output through inverse kinematics (IK)

It can be calculated as follows.

[Equation 5]

은 단위 행렬이며,

이다. 또한,

,

이며, k₁ 및 k₂는 외력에 대한 매니퓰레이터의 감도(sensitivity)와 안정성(stability)을 결정하는 상수들이다. 또한, I_n-J_s ^#J_s 는 멱등 행렬(indempotent matrix)이라는 것을 이용하여, p는 감소함수 임을 증명할 수 있고, 이는 매니퓰레이터가 상기의 역기구학(IK)을 이용하면, 제한공간을 고려하여 관절 배치를 최적화 또는 관절의 제어 오차를 최소화하는 방향으로 목표 조인트 속도를 산출한다는 것을 의미한다.here,

Is an identity matrix,

to be. Also,

,

K ₁ and k ₂ are constants that determine the sensitivity and stability of the manipulator to external forces. Also, I _n -J _s ^# J _s Is an idempotent matrix, and p can be proved to be a decreasing function. When the manipulator uses the inverse kinematics (IK), it is possible to optimize joint placement or control error of joints by considering constraint space. This means calculating the target joint velocity in the direction of minimization.

Inverse kinematics (IK) changes the target joint angle placement to reduce joint angle error to avoid joint movement limitations.

Since the IRS is transmitted to the master device 120 in the form of repulsive force, when there is a force sensor attached to the manipulator to detect a collision, the force sensor signal can be used together with the calculated repulsive force. If there is a non-zero force signal from the force sensor (F _EF ≠ 0), then F _EF replaces F _{RE so} that the repulsive force F _R = F _RG + F _{EF ,} or F _R = F _RG + F _RE + F _EF If the force sensor avoids collision (F _EF = 0) but there is a calculated limit space, then F _R = F _RG + F _RE .

While the present invention has been particularly shown and described with reference to exemplary embodiments thereof, it is to be understood that the invention is not limited to the disclosed embodiments, but, on the contrary, And additions should be considered as falling within the scope of the following claims.

10, 60, 140: manipulator
20, 50: obstacle
30: first joint
32, 62: link
40: second joint
110: operator
120: master device
130: manipulator controller
140: manipulator
150: surroundings
160: limited space calculation unit

Claims (6)

As a method for controlling the movement of the manipulator by calculating a space in which movement is limited by the position sensor of the articulated manipulator,
Generating a target position of the manipulator in a master device to drive the manipulator;
After calculating the joint angle of the manipulator through the position sensor, by converting the joint angle into the attitude vector of the manipulator end, and calculating the attitude error compared to the target attitude vector of the manipulator, the driven manipulator has Calculating a space in which movement is limited due to insufficient degrees of freedom;
After calculating the joint angle of the manipulator through the position sensor, the joint angle is calculated by comparing the joint angle with the target joint angle of the manipulator, and summed with the calculated attitude error so that the driven manipulator is external. Calculating a space in which movement is limited due to an obstacle or a link collision of the manipulator; And
And optimizing the joint arrangement of the manipulator using the calculated information on the constraint space. The method of claim 1,
Converting a space in which movement is limited due to insufficient degrees of freedom of the driven manipulator to a first repulsive force F _RG ;
Converting, by the driven manipulator, a space in which movement is limited due to an external obstacle or a link collision of the manipulator to a second repulsive force F _RE ; And
And adding the first repulsive force (F _RG ) and the second repulsive force (F _RE ) to the master device and transmitting the sum of the first repulsive force (F _RG ) to the master device. The method of claim 2, wherein the first repulsive force F _RG is calculated according to Equation 1 below.
[Equation 1]
F _RG = R _G e _sx = -K _G (IJ _s J _s ^# ) e _sx
Where R _G is the first mapping matrix that maps the temporary limited space (IRS) due to the lack of freedom of the manipulator with repulsive force,
e _sx is the attitude error at the end of the manipulator,
K _G is the scalar force gain for scaling the repulsive force F _RG ,
I is an identity matrix,
Jacobian J _s = ∂x _s / ∂q _s Is,
x _s Is the attitude vector at the end of the manipulator,
q _s Is the joint angle vector of the manipulator,
J _s ^# is the pseudo-inverse of J _s . The method of claim 2, wherein the second repulsive force F _RE is calculated according to Equation 2 below.
[Equation 2]
F _RE = R _E e _sx = -K _E {(IJ _{s θ} J ^# _sθ ) J _s J ^# _s } e _sx
Here, R _E is a second mapping matrix for mapping the temporary limited space (IRS) by the external constraint of the manipulator with a repulsive force,
e _sx is the attitude error at the end of the manipulator,
K _E is the scalar force gain for scaling the repulsive force F _RE ,
I is an identity matrix,
Jacobian J _s = ∂x _s / ∂q _s Is,
x _s Is the attitude vector at the end of the manipulator,
q _s Is the joint angle vector of the manipulator,
J ^# _s is the pseudo inverse of J _s ,
J _{s θ} is J _'s inherited Jacobian,
J ^# _sθ is the pseudo inverse of J _{s θ} . The method of claim 2, wherein the manipulator is attached to a plurality of force sensors,
Sensing an external obstacle in the force sensor, converting a space where movement of the manipulator is restricted to a third repulsive force F _EF ;
And adding the third repulsive force (F _EF ) to the first repulsive force (F _RG ) and the second repulsive force (F _RE ), and transmitting the result to the master device. Constrained space calculation method using The method of claim 1, wherein the target joint angle of the manipulator is a target joint speed of the manipulator (

) Is obtained by integrating through an integrator, and the target joint velocity (

) Is calculated according to Equation 5 in order to optimize the joint arrangement of the manipulator using the information of the constraint space using the position sensor of the articulated manipulator:
&Quot; (5) "

here,

Is an identity matrix,

Is,

,

Is,
e _sq is the joint error,
q _sd is the target joint angle,
q _s is the joint angle,
k ₁ and k ₂ are constants that determine the sensitivity and stability of the manipulator to external forces.

Images