CN105904458A - Non-integrate teleoperation constraint control method based on complex operation tasks - Google Patents

Abstract

The invention discloses a non-integrate teleoperation constraint control method based on complex operation tasks. The complex operation tasks are decomposed by calculating the constraint matrix CVF, and a constraint controller is designed finally. Compared with a general mechanical arm control method, the non-integrate teleoperation constraint control method has two beneficial effects on the aspects of the task level and control method for complex space control tasks, specifically, the control tasks which a mechanical arm needs to complete in the space include approaching the target, tracing the path, avoiding obstacles and the like, and since proper movement spinor collections are selected according to the different operation tasks so as to establish the constraint matrix, the method has a better capacity of adapting to the complex tasks compared with prior methods; and besides, by means of the non-integrate teleoperation constraint control method, operation is more flexible, an operator can control the mechanical arm to move only in the horizontal motion direction or rotation direction, no extra constraint is needed, and accordingly the stress of the operator is relieved.

Description

Incomplete teleoperation constraint control method based on complex operation task

[ technical field ] A method for producing a semiconductor device

The invention belongs to the technical field of space teleoperation, and relates to an incomplete teleoperation constraint control method based on a complex operation task.

[ background of the invention ]

In the field of robots, it has been proved that a virtual fixture improves task execution time and overall accuracy, but the fixture is also non-sensitive auxiliary, for example, in a space teleoperation process, the position and the posture of the end of a mechanical arm need to be controlled simultaneously by the virtual fixture, and accurate control of the posture of the end is often lost under the condition that optimal position control is ensured. In order to achieve a good operation effect in space teleoperation, proper assistance needs to be added to the tail end of the mechanical arm, so that the mechanical arm can flexibly coordinate the position and the posture according to a task environment and an operation task.

In an unstructured environment, the motion of the robot is constrained geometrically and dynamically, and the translational and rotational degrees of freedom are limited. During the process of space teleoperation tasks, the tail end of the mechanical arm can finish operation tasks generally only under the condition of constraint of the translation direction, however, for some complex space operation tasks, the constraint limitation of the translation direction is difficult to ensure the high efficiency and the safety of the operation, so that the auxiliary effect is required to be added in the rotation direction of the tail end of the mechanical arm, an operator can dynamically change the auxiliary effect of a virtual clamp in the translation and rotation directions according to the actual operation environment, and the teleoperation tasks are better finished.

[ summary of the invention ]

The invention aims to overcome the defects of the prior art and provide a non-complete teleoperation restriction control method based on a complex operation task.

In order to achieve the purpose, the invention adopts the following technical scheme to realize the purpose:

an incomplete teleoperation constraint control method based on complex operation tasks comprises the following steps:

1) computing a constraint matrix C_VF；

Defining a constraint matrix C_VFRepresents the corresponding constraint capability, and C_VFIs a semi-positive definite symmetric matrix of 6 × 6, and maps the space acting force into C_VFA matrix element of (a); the operator' S control of the movement of the end of the arm in the optimal and non-optimal directions constitutes a space-geometric constraint, and both are represented by unit moments in se (3), giving a set of moments S:thenThe sum of the semi-positive definite matrices decomposed into m rank-1:

$\begin{array}{c}{C}_{VF}^b=c_1{S}_1{S}_1^T+c_2{S}_2{S}_2^T+...+c_m{S}_m{S}_m^T\\ =C_1+C_2+...+C_m,c_i>0\end{array}---\left(1\right)$

wherein the direct proportional coefficient c_iEach constraint matrix C is represented_iThe strong constraint enables an operator to control the tail end of the mechanical arm to move along the optimal direction, and the weak constraint enables the tail end of the mechanical arm to deviate along the non-optimal direction;

operator control forceDetermines the displacement change of the mechanical arm tail end in the rotating and translating directionsChange X^b＝(φ^b,q^b) Is provided with

$\left(\begin{array}{c}\mathrm{\δ}{\mathrm{\φ}}^b\\ {\mathrm{\δq}}^b\end{array}\right)=C_{VF}^b\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(2\right)$

Wherein,representing operator control forces, X, acting on the end of the arm^bA minute displacement representing the rotation and translation directions; the complete constraint under the assistance of the virtual fixture is equal to the condition that the constraint matrix C is a full-rank matrix, and the corresponding constraint matrix C is a non-full-rank matrix if the incomplete constraint is not complete;

2) decomposing a complex operation task;

the complex operation task in the teleoperation can be decomposed into a combination of a plurality of tasks; selecting an optimal motion vector set aiming at several different tasks;

3) designing a constraint controller;

the space motion speed of the mechanical arm is V^b＝(ω^b,v^b) Wherein ω is^b、v^bRespectively representing the rotation angular velocity and the tangential velocity of the mechanical arm in the inertial space; the controller is designed as follows:

$\left[\begin{array}{c}{\mathrm{\ω}}^b\\ v^b\end{array}\right]=K_c{C}_{VF}(g,g_d)\left[\begin{array}{c}{m}^b\\ f^b\end{array}\right]---\left(9\right)$

wherein, constraint matrix C_VF(g,g_d) Representing the constraint capability of the robot arm, and the operator control force is defined as F^b＝(m^b,f^b) Proportionality coefficient K_c＝diag(c_rI_3×3,c_pI_3×3) Control operationThe author constrains the magnitude of the force.

2. The incomplete teleoperation constraint control method based on the complex operation task as claimed in claim 1, wherein in the step 2), a specific method for selecting an optimal motion rotation set is as follows:

2-1) target approach

Let the reference track of the mechanical arm in the configuration space be g_dWith the current bit shape of g, the error from the reference track is expressed as

$e_1=g_d^{-1}g---\left(3\right)$

The corresponding motion vector set takes the logarithmic expression of the error, which is

$S=log\left(g_d^{-1}g\right)=(\widehat{\mathrm{\ψ}},q)---\left(4\right)$

Corresponding constraint matrix C_VFThe cS is formed by a motion vector set, and the magnitude of c determines the strength of the constraint acting force;

2-2) trajectory tracking

Let the reference track of the mechanical arm movement be g_r(λ, t) axis of the robot arm motion isr is any point on the motion axis, λ is precession parameter in translation direction, | s | | | 1, and the corresponding tangential velocity is

$V_t(\mathrm{\λ},t)=\frac{\∂g_r}{\∂\mathrm{\λ}}{g}_r^{-1}---\left(5\right)$

Set S of unit rotation quantities by recording tangential velocity₁，S₂In logarithmic form of error:

$S_2=log\left(g_r^{-1}g\right)---\left(6\right)$

then the constraint matrix is

C_VF＝c₁S₁+c₂S₂(7)

Wherein the direct proportional coefficient c₁Determines the movement speed of the mechanical arm constrained by the virtual clamp, and c₁The greater the corresponding speed of movement, c₂The size of the reference trajectory determines the ability of the mechanical arm to track the reference trajectory;

2-3) plane motion

Suppose the motion velocity vector of the end of the mechanical arm is v₁And v₂Then span { v₁,v₂Means with v₁，v₂For the generated plane, the operator controls the robotic arm to move in the two-dimensional plane, defining a motion vector ξ ═ ω, v, when taken along v₁When moving in the direction, v is 0 when ω is equal to s₁The motion rotation of the direction is collected as S₁＝(0,v₁) Same principle v₂The motion rotation of the direction is collected as S₂＝(0,v₂) The rotation amount of the mechanical arm moving along the axial direction S under the constraint action is set as S₃(s, r × s) where r is any point on s;

thus, the constraint matrix is defined as

C_VF＝c₁S₁+c₂S₂+c₃S₃(8)

Wherein, c₁Is used for determining the mechanical arm edge v₁Capability of directional movement, c₂Is used for determining the mechanical arm edge v₂Capability of directional movement, c₃The size of the mechanical arm determines the capability of the mechanical arm in restraining the axial direction s, wherein the restraining effect of the mechanical arm around the axial direction s is included.

Compared with the prior art, the invention has the following beneficial effects:

aiming at complex space control tasks, compared with a general mechanical arm control method, the method has two beneficial effects of a task level and a control method. Firstly, the control tasks to be completed by the mechanical arm in space comprise target approaching, path tracking, obstacle avoidance and the like, and the method selects a proper motion rotation set to construct a constraint matrix according to different operation tasks, so that the method has the capability of adapting to complex tasks; and secondly, the operation can be more flexible by the incomplete constraint control method, an operator can control the mechanical arm to move only in the translation or rotation direction, and extra constraint is not required, so that the pressure of the operator is reduced.

[ description of the drawings ]

Fig. 1 is a power constrained schematic.

Fig. 2 is a schematic plan view of the constrained motion.

Fig. 3 is a schematic view of a sphere and its tangent plane.

Fig. 4 is a spherical cross-sectional operational view.

[ detailed description ] embodiments

The invention is described in further detail below with reference to the accompanying drawings:

referring to fig. 1-4, the incomplete teleoperation constraint control method based on complex operation tasks is characterized by comprising the following steps:

the method comprises the following steps: computing a constraint matrix C_VF。

An operator controls the mechanical arm to complete complex tasks in a space environment, effective constraint action is required to be carried out on the control force at the tail end of the mechanical arm, the constraint action forms a virtual clamp, and a constraint matrix C is defined_VFRepresents the corresponding constraint capability, and C_VFIs a semi-positive definite symmetric matrix of 6 × 6, and maps the space acting force into C_VFOf the matrix element(s). The operator controls the movement of the tail end of the mechanical arm towards the optimal direction and the non-optimal direction to form space geometric constraint, and the space geometric constraint can be realizedGiven the set of spin values S, expressed as unit spin values in se (3):thenCan be decomposed into the sum of m semi-positive definite matrices of rank-1:

$\begin{array}{c}{C}_{VF}^b=c_1{S}_1{S}_1^T+c_2{S}_2{S}_2^T+...+c_m{S}_m{S}_m^T\\ =C_1+C_2+...+C_m,c_i>0\end{array}---\left(1\right)$

wherein the direct proportional coefficient c_iEach constraint matrix C is represented_iThe strong constraint enables an operator to control the tail end of the mechanical arm to move along the optimal direction, and the weak constraint enables the tail end of the mechanical arm to deviate along the non-optimal direction. Such movement prioritization reduces the operating pressure of the operator, i.e. the operator only needs to control the mechanical arm to move within the constraint action range of the virtual clamp.

Operator control forceDetermines the displacement change X of the mechanical arm end in the rotating and translating directions^b＝(φ^b,q^b) Is provided with

$\left(\begin{array}{c}\mathrm{\δ}{\mathrm{\φ}}^b\\ {\mathrm{\δq}}^b\end{array}\right)=C_{VF}^b\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(2\right)$

Wherein,representing operator control forces, X, acting on the end of the arm^bIndicating a slight displacement in the rotational and translational directions. The complete constraint under the assistance of the virtual fixture is equivalent to the condition that the constraint matrix C is a full-rank matrix, and the incomplete constraint corresponds to the condition that the constraint matrix C is a non-full-rank matrix.

Step two: and (4) decomposing a complex operation task.

Based on the above theoretical knowledge, a complex operation task in teleoperation can be decomposed into a combination of a plurality of simple tasks (such as target approaching, path tracking, obstacle avoidance, etc.). Aiming at several different simple tasks, selecting an optimal motion vector set:

(1) target approach

Let the reference track of the mechanical arm in the configuration space be g_dWith the current bit shape of g, the error from the reference track is expressed as

$e_1=g_d^{-1}g---\left(3\right)$

The corresponding motion vector set takes the logarithmic expression of the error, which is

$S=log\left(g_d^{-1}g\right)=(\widehat{\mathrm{\ψ}},q)---\left(4\right)$

Corresponding constraint matrix C_VFThe cS is formed by a motion vector set, the strength of the constraint acting force is determined by the size of c, an operator only needs to follow the constraint acting force provided by the virtual clamp to complete an operation task, and no additional control force needs to be provided.

(2) Trajectory tracking

Let the reference track of the mechanical arm movement be gr (lambda, t), and the axis of the mechanical arm movement ber is any point on the motion axis, λ is precession parameter in translation direction, | s | | | 1, and the corresponding tangential velocity is

$V_t(\mathrm{\λ},t)=\frac{\∂g_r}{\∂\mathrm{\λ}}{g}_r^{-1}---\left(5\right)$

Set S of unit rotation quantities by recording tangential velocity₁，S₂As in section 4.3.1, defined as the logarithmic form of the error

$S_2=log\left(g_r^{-1}g\right)---\left(6\right)$

Then the constraint matrix is

C_VF＝c₁S₁+c₂S₂(7)

Wherein the direct proportional coefficient c₁Determines the movement speed of the mechanical arm constrained by the virtual clamp, and c₁The greater the corresponding speed of movement, c₂The size of (d) determines the ability of the robotic arm to track the reference trajectory.

(3) Plane motion

Suppose the motion velocity vector of the end of the mechanical arm is v₁And v₂Then span { v₁,v₂Means with v₁，v₂To base the generated plane, the operator controls the robotic arm to move in the two-dimensional plane, as shown in fig. 2, defining a curl ξ (ω, v) as it followsIs showing v₁When moving in the direction, v is 0 when ω is equal to s₁The motion rotation of the direction is collected as S₁＝(0,v₁) Same principle v₂The motion rotation of the direction is collected as S₂＝(0,v₂) The rotation amount of the mechanical arm moving along the axial direction S under the constraint action is set as S₃R is any point on s (s, r × s).

Thus, the constraint matrix can be defined as

C_VF＝c₁S₁+c₂S₂+c₃S₃(8)

Wherein, c₁Is used for determining the mechanical arm edge v₁Capability of directional movement, c₂Is used for determining the mechanical arm edge v₂Capability of directional movement, c₃The size of the mechanical arm determines the capability of the mechanical arm in restraining the axial direction s, wherein the restraining effect of the mechanical arm around the axial direction s is included.

Step three: and (4) constraining the controller design.

When an operator controls a mechanical arm to complete a given task, the mechanical arm needs to be kept within a stable speed threshold value in the rotation direction and the translation direction, and based on the speed, the spatial motion speed of the mechanical arm is set to be V^b＝(ω^b,v^b) Wherein ω is^b、v^bRespectively representing the angular velocity and tangential velocity of the arm in inertial space. The controller is designed as follows:

$\left[\begin{array}{c}{\mathrm{\ω}}^b\\ v^b\end{array}\right]=K_c{C}_{VF}(g,g_d)\left[\begin{array}{c}{m}^b\\ f^b\end{array}\right]---\left(9\right)$

wherein, constraint matrix C_VF(g,g_d) Representing the constraint capability of the robot arm, and the operator control force is defined as F^b＝(m^b,f^b) Proportionality coefficient K_c＝diag(c_rI_3×3,c_pI_3×3) The constraint acting force of an operator is controlled, the mechanical arm moves faster along the optimal direction due to strong constraint, and proper c is selected to prevent the occurrence of accidents such as collision caused by too high speed_r、c_pThe magnitude of the constraint force applied by an operator is controlled, and the failure of a task caused by the loss of control of the mechanical arm is prevented.

The principle of the invention is as follows:

in classical analytical mechanics, constraints can be divided into complete constraints and incomplete constraints. Whether the mechanical arm can realize given movement under the control of an operator in space teleoperation is related to the integrity of constraint control force, the simplification of operation tasks and the like. Under the condition of incomplete constraint, namely partial constraint limitation is carried out on the mechanical arm, the operation freedom degree of the reference track is smaller than the dimension of the configuration space of the mechanical arm, for example, the tail end of the mechanical arm is controlled to move from point A to point B on a two-dimensional curved surface, and a task sequence T is given_r([m_x,m_y,m_z],p_r) Wherein (m)_x,m_y,m_z) Refers to any point coordinate, p, on the task sequence_rRepresenting the constraint power, as shown in FIG. 1, the judgment method is usually to consider the constraint force momentum for the constrained object to achieve a given motionWith a given speed rotationIf the constraint power is zero, the motion is the motion allowed by the constraint.

In order to verify the effective constraint effect of the designed controller on the mechanical arm in the changing operating environment, a dynamic trajectory tracking experiment is designed as follows:

(1) in the experiment, an operator controls the tail end to operate the tool to move along a reference track, and the operation environment of the tail end is designed to be dynamically changed. In the dynamic trajectory tracking experiment, firstly, the dynamic change frequency of an experimental environment (dynamic change spherical surface) is set to be omega (0-45 rad/s), and a reference trajectory is set to be a latitude circle (the radius of the sphere is r) with r being 50mm on the spherical surface₀100mm), as shown in figure 3,

(2) when the tail end of the mechanical arm moves from a starting point to a target point, the motion momentum set consists of two parts, and the tangential speed is recorded as a unit momentum set S₁，S₂Defined as logarithmic form of errorThen the constraint matrix is

C_VF＝c₁S₁+c₂S₂

Wherein, let c₁＝0.6，c₂0.8, positive proportionality coefficient c₁The size of the arm determines the movement speed of the mechanical arm under the constraint of the virtual clamp, c₂Of the arm determines the reference trajectoryCapability.

(3) In a two-dimensional surface, make the constrained power p_rThe operation state of 0 is to control the movement of the end of the robot arm along a certain axial direction (such as the z-axis), and all rotation around the z-axis has no constraint influence on the movement in the z-direction, so that the end of the robot arm can make continuous movement along the z-axis.

Setting the radius of the sphere to be r ═ r₀+ Δ r sin ω t, where Δ r ═ 20 mm. Coefficient of proportionality K_c＝diag(c_rI_3×3,c_pI_3×3) Controlling the size of the constraint acting force of an operator, and selecting c in the test process_r＝0.1rad/s,c_p100mm/s, the operator controls the end-effector from a starting position g₀Towards a reference trajectory g_dThe motion is shown in figure 4 as a spherical diameter direction section plane operation diagram:

the experimental result shows that in a changing environment, an operator selects a corresponding motion vector set according to different operation tasks to construct a constraint matrix, so that the terminal operation tool can be well constrained to move along the designated direction.

The above-mentioned contents are only for illustrating the technical idea of the present invention, and the protection scope of the present invention is not limited thereby, and any modification made on the basis of the technical idea of the present invention falls within the protection scope of the claims of the present invention.

Claims (2)

1. An incomplete teleoperation constraint control method based on complex operation tasks is characterized by comprising the following steps:

1) computing a constraint matrix C_VF；

Defining a constraint matrix C_VFRepresents the corresponding constraint capability, and C_VFIs a semi-positive definite symmetric matrix of 6 × 6, and maps the space acting force into C_VFA matrix element of (a); the operator controls the movement of the end of the mechanical arm towards the optimal direction and the non-optimal direction to form space geometric constraint, and the space geometric constraint is expressed by unit rotation in se (3) and gives rotationQuantity set S:thenThe sum of the semi-positive definite matrices decomposed into m rank-1:

$\begin{array}{c}{C}_{VF}^b=c_1{S}_1{S}_1^T+c_2{S}_2{S}_2^T+...+c_m{S}_m{S}_m^T\\ =C_1+C_2+...+C_m,c_i>0\end{array}---\left(1\right)$

wherein the direct proportional coefficient c_iEach constraint matrix C is represented_iThe strong constraint enables an operator to control the tail end of the mechanical arm to move along the optimal direction, and the weak constraint enables the tail end of the mechanical arm to deviate along the non-optimal direction;

operator control forceDetermines the displacement change X of the mechanical arm end in the rotating and translating directions^b＝(φ^b,q^b) Is provided with

$\left(\begin{array}{c}{\mathrm{\δ\φ}}^b\\ {\mathrm{\δq}}^b\end{array}\right)=C_{VF}^b\left(\begin{array}{c}{m}^b\\ f^b\end{array}\right)---\left(2\right)$

Wherein,representing operator control forces, X, acting on the end of the arm^bA minute displacement representing the rotation and translation directions; the complete constraint under the assistance of the virtual fixture is equal to the condition that the constraint matrix C is a full-rank matrix, and the corresponding constraint matrix C is a non-full-rank matrix if the incomplete constraint is not complete;

2) decomposing a complex operation task;

the complex operation task in the teleoperation can be decomposed into a combination of a plurality of tasks; selecting an optimal motion vector set aiming at several different tasks;

3) designing a constraint controller;

the space motion speed of the mechanical arm is V^b＝(ω^b,v^b) Wherein ω is^b、v^bRespectively representing the rotation angular velocity and the tangential velocity of the mechanical arm in the inertial space; the controller is designed as follows:

$\left[\begin{array}{c}{\mathrm{\ω}}^b\\ v^b\end{array}\right]=K_c{C}_{VF}(g,g_d)\left[\begin{array}{c}{m}^b\\ f^b\end{array}\right]---\left(9\right)$

wherein, constraint matrix C_VF(g,g_d) Representing the constraint capability of the robot arm, and the operator control force is defined as F^b＝(m^b,f^b) Proportionality coefficient K_c＝diag(c_rI_3×3,c_pI_3×3) Controlling the amount of operator restraining force.

2. The incomplete teleoperation constraint control method based on the complex operation task as claimed in claim 1, wherein in the step 2), a specific method for selecting an optimal motion rotation set is as follows:

2-1) target approach

Let the reference track of the mechanical arm in the configuration space be g_dWith the current bit shape of g, the error from the reference track is expressed as

$e_1=g_d^{-1}g---\left(3\right)$

The corresponding motion vector set takes the logarithmic expression of the error, which is

$S=\log \left(g_d^{-1}g\right)=(\widehat{\mathrm{\ψ}},q)---\left(4\right)$

Corresponding constraint matrix C_VFThe cS is formed by a motion vector set, and the magnitude of c determines the strength of the constraint acting force;

2-2) trajectory tracking

Let the reference track of the mechanical arm movement be g_r(λ, t) axis of the robot arm motion isr is any point on the motion axis, λ is precession parameter in translation direction, | s | | | 1, and the corresponding tangential velocity is

$V_t(\mathrm{\λ},t)=\frac{\∂g_r}{\∂\mathrm{\λ}}{g}_r^{-1}---\left(5\right)$

Set S of unit rotation quantities by recording tangential velocity₁，S₂In logarithmic form of error:

$S_2=log\left(g_r^{-1}g\right)---\left(6\right)$

then the constraint matrix is

C_VF＝c₁S₁+c₂S₂(7)

Wherein the direct proportional coefficient c₁Determines the movement speed of the mechanical arm constrained by the virtual clamp, and c₁The greater the corresponding speed of movement, c₂The size of the reference trajectory determines the ability of the mechanical arm to track the reference trajectory;

2-3) plane motion

Suppose the motion velocity vector of the end of the mechanical arm is v₁And v₂Then span { v₁,v₂Means with v₁，v₂For the generated plane, the operator controls the robotic arm to move in the two-dimensional plane, defining a motion vector ξ ═ ω, v, when taken along v₁When moving in the direction, v is 0 when ω is equal to s₁The motion rotation of the direction is collected as S₁＝(0,v₁) Same principle v₂The motion rotation of the direction is collected as S₂＝(0,v₂) The rotation amount of the mechanical arm moving along the axial direction S under the constraint action is set as S₃(s, r × s) where r is any point on s;

thus, the constraint matrix is defined as

C_VF＝c₁S₁+c₂S₂+c₃S₃(8)

Wherein, c₁Is used for determining the mechanical arm edge v₁Capability of directional movement, c₂Is used for determining the mechanical arm edge v₂Capability of directional movement, c₃The size of the mechanical arm determines the capability of the mechanical arm in restraining the axial direction s, wherein the restraining effect of the mechanical arm around the axial direction s is included.