CN106112951A - Multiple degrees of freedom master-slave mode remote operating method for controlling robot - Google Patents

Abstract

The invention discloses a kind of multiple degrees of freedom master-slave mode remote operating method for controlling robot, including following operating procedure: step A, on-the-spot from mechanical hand, according to the method for positive kinematics, with the angle in current each joint for parameter determination appearance matrix；Step B, judges to capture impact point (X, Y, Z) or the direction of X, Y, Z according to actual target image or video；Step C, the artificial route track setting meter gripper of manipulator；Step D, sets interpolated point, the step-length in the most each mobile section；Step E, according to motion Supreme Being's inverse arithmetic, calculating machine hands arrives next interpolated point to be needed the kinematic parameter in each joint and performs；Step F, carries out feeding back and repeating step A with artificial judgment, makes to arrive target location from mechanical arm.The present invention use people vision localization function can the 6DOF bonding machine mechanical arm of remote operating, replace artificial simple joint and control, the mental burden alleviating operator improves more greatly work efficiency.

Description

Multiple degrees of freedom master-slave mode remote operating method for controlling robot

Technical field

The invention belongs to robot control field, be specifically related to a kind of multiple degrees of freedom master-slave mode remote operating manipulator control side Method.

Background technology

Along with development in science and technology, and robot replaces artificial field and increases sharply, and all kinds of robots grind The problem that system has become countries in the world and army pays close attention to jointly.Owing to the working environment situation of robot is failed to understand, a lot of occasion people Class cannot arrive or can only pass through video acquisition job information, during needing the Randomness of position of operation relatively big and capturing with The possibility occurred outside Shi Youyi, if people is near operation field, unexpected the most at any time it may happen that.Therefore, machine under this special environment Device people uses master-slave mode operation to be remotely controlled operation, reduces intractability, to improving science and technology overall technology level and working performance Significant.

The most existing master-slave mode teleoperation robot includes " Leonardo da Vinci's operating robot ", " remote operating dangerous materials dig Pick transfer robot ", " moonfall robot ", " curious number " mars exploration robot etc..Remote operating controls to be to closing in robot An important part, the core that remote operating controls is inverse motion algorithm, and its quality degree direct influence the man-machine friendship of robot Interaction performance, the degree of reliability and security performance etc..

For the special circumstances of robot manipulating task environment, if the control system of robot is unreliable, it will introduce new Unsafe factor, not only could not solve problem, can inspire the upgrading of contradictory problems on the contrary.At multivariant Robot actions Time, it is common practice to using monarthric Remote mode, the location of arm end paw is captured and causes pole by this Big difficulty.The operation of the necessary very familiar explosive-removal robot of operator, judges arm end position by photographic head at a distance Put, and operate each joint fast and accurately, make the paw of arm end just to suspicious object and capture.Should Kind of mode is relatively big to the mental burden of operator, the most tired and operating efficiency is relatively low.

Summary of the invention

In order to solve above-mentioned technical problem, the present invention is to provide a kind of multiple degrees of freedom master-slave mode remote operating manipulator control side Method, it is intended to carry and be capable of mechanical hand multi-joint coordinated signals.

The technical scheme realizing the object of the invention is a kind of multiple degrees of freedom master-slave mode remote operating method for controlling robot, including Following operating procedure:

Step A, on-the-spot from mechanical hand, according to the method for positive kinematics, with the angle in current each joint for parameter determination appearance Matrix；

Step B, judges to capture impact point (X, Y, Z) or the direction of X, Y, Z according to actual target image or video, not Consider targeted attitude；

Step C, the artificial route track setting meter gripper of manipulator；

Step D, sets interpolated point, the step-length in the most each mobile section；

Step E, according to motion Supreme Being's inverse arithmetic, calculating machine hands arrives next interpolated point needs the motion ginseng in each joint Counting and perform, the most each joint rotation angle and movement velocity realize；

Step F, carries out feeding back and repeating step A with artificial judgment, makes to arrive target location from mechanical arm.

In described cloth step A, the method for described positive kinematics is the method for the positive kinematics using flute card coordinate system, is By the iteration of space conversion matrices that is each and that save, calculate from the current location of mechanical arm tail end paw.

In step C, the route track set is as straight path or dog-leg path.

The step-length set in step D as set end next one interpolated point distance, the single that namely paw moves away from From, it is (Δ X, Δ Y, Δ Z) apart from current position step-length.

Step E Computational Methods is, θ₁=A tan 2 (p_y,p_x)；

Make k₁=p_xc₁+p_ys₁；

${\mathrm{\θ}}_2=\arctan \frac{k_1-a_1}{p_z}-\arctan \frac{k_2/{\mathrm{\ρ}}_2}{\±\sqrt{1-{(k_2/{\mathrm{\ρ}}_2)}^2}};$

Order

${\mathrm{\θ}}_3=\arctan \frac{a_3}{d_4}-\arctan \frac{k_3/{\mathrm{\ρ}}_3}{\±\sqrt{1-{(k_3/{\mathrm{\ρ}}_3)}^2}};$

${\mathrm{\θ}}_4=\arctan (-\frac{s_4}{c_4});$

${\mathrm{\θ}}_5=\arctan \left(\frac{s_5}{c_5}\right);{\mathrm{\θ}}_6=\arccos \left(\frac{c_3{n_y}^2-s_3{n_x}^2}{sin\left({\mathrm{\θ}}_5\right)}\right)$

Wherein θ₁For first articulation angle of mechanical arm, θ₂For first articulation angle of mechanical arm, θ₃For machinery First articulation angle of arm, θ₄For the 4th articulation angle of mechanical arm, θ₅For the 5th articulation angle of mechanical arm Degree, θ₆For the 6th articulation angle of mechanical arm；c_i=cos θ_i, s_i=sin θ_i, a_i=arctan θ_i(i=1,2, .....6)；

Speed during interpolation, is got by joint variable quantity and interpolation cycle ratio.

The present invention has a positive effect: the present invention uses the vision localization function of people can the 6DOF welding of remote operating Mechanical arm, replaces artificial simple joint and controls, use inverse arithmetic, solve operator and carry out from manipulator control away from scene A difficult problem, the mental burden alleviating operator improves more greatly work efficiency.

Accompanying drawing explanation

In order to make present disclosure be more likely to be clearly understood, below according to specific embodiment and combine accompanying drawing, right The present invention is described in further detail, wherein:

Fig. 1 is the method flow schematic diagram of the present invention；

Fig. 2 is Multi-freedom-degreemanipulator manipulator moving step length spatial position change geometric diagram figure in the present invention.

Detailed description of the invention

(embodiment 1)

Fig. 1 and Fig. 2 shows a kind of detailed description of the invention of the present invention, and wherein Fig. 1 is the method flow signal of the present invention Figure；Fig. 2 is Multi-freedom-degreemanipulator manipulator moving step length spatial position change geometric diagram figure in the present invention.

See Fig. 1 and Fig. 2, a kind of multiple degrees of freedom master-slave mode remote operating method for controlling robot, including following operating procedure:

Step A, on-the-spot from mechanical hand, according to the method for positive kinematics, with the angle in current each joint for parameter determination appearance Matrix；

Step B, judges to capture impact point (X, Y, Z) or the direction of X, Y, Z according to actual target image or video, not Consider targeted attitude；

Step C, the artificial route track setting meter gripper of manipulator；

Step D, sets interpolated point, the step-length in the most each mobile section；

Step E, according to motion Supreme Being's inverse arithmetic, calculating machine hands arrives next interpolated point needs the motion ginseng in each joint Counting and perform, the most each joint rotation angle and movement velocity realize；

Step F, carries out feeding back and repeating step A with artificial judgment, makes to arrive target location from mechanical arm.

In described cloth step A, the method for described positive kinematics is the method for the positive kinematics using flute card coordinate system, is By the iteration of space conversion matrices that is each and that save, calculate from the current location of mechanical arm tail end paw.

In step C, the route track set is as straight path or dog-leg path.

The step-length set in step D as set end next one interpolated point distance, the single that namely paw moves away from From, it is (Δ X, Δ Y, Δ Z) apart from current position step-length.

Step E Computational Methods is, θ₁=A tan²(p_y,p_x)；

Make k₁=p_xc₁+p_ys₁；

${\mathrm{\θ}}_2=\arctan \frac{k_1-a_1}{p_z}-\arctan \frac{k_2/{\mathrm{\ρ}}_2}{\±\sqrt{1-{(k_2/{\mathrm{\ρ}}_2)}^2}};$

Order

${\mathrm{\θ}}_3=\arctan \frac{a_3}{d_4}-\arctan \frac{k_3/{\mathrm{\ρ}}_3}{\±\sqrt{1-{(k_3/{\mathrm{\ρ}}_3)}^2}};$

${\mathrm{\θ}}_4=\arctan (-\frac{s_4}{c_4});$

${\mathrm{\θ}}_5=\arctan \left(\frac{s_5}{c_5}\right);{\mathrm{\θ}}_6=\arccos \left(\frac{c_3{n_y}^2-s_3{n_x}^2}{sin\left({\mathrm{\θ}}_5\right)}\right)$

Wherein θ₁For first articulation angle of mechanical arm, θ₂For first articulation angle of mechanical arm, θ₃For machinery First articulation angle of arm, θ₄For the 4th articulation angle of mechanical arm, θ₅For the 5th articulation angle of mechanical arm Degree, θ₆For the 6th articulation angle of mechanical arm；c_i=cos θ_i, s_i=sin θ_i, a_i=arctan θ_i(i=1,2, .....6)；

Speed during interpolation, is got by joint variable quantity and interpolation cycle ratio.

The present invention use people vision localization function can the 6DOF bonding machine mechanical arm of remote operating, replace artificial simple joint Control, use inverse arithmetic, solve operator and carry out the difficult problem from manipulator control away from scene, alleviate operator Mental burden improve more greatly work efficiency.

Obviously, the above embodiment of the present invention is only for clearly demonstrating example of the present invention, and is not right The restriction of embodiments of the present invention.For those of ordinary skill in the field, the most also may be used To make other changes in different forms.Here without also cannot all of embodiment be given exhaustive.And these Belong to obvious change that the connotation of the present invention extended out or variation still falls within protection scope of the present invention.

Claims (5)

1. a multiple degrees of freedom master-slave mode remote operating method for controlling robot, it is characterised in that: include following operating procedure:

Step A, on-the-spot from mechanical hand, according to the method for positive kinematics, with the angle in current each joint for parameter determination appearance matrix；

Step B, judges to capture impact point (X, Y, Z) or the direction of X, Y, Z according to actual target image or video, it is not intended that Targeted attitude；

Step C, the artificial route track setting meter gripper of manipulator；

Step D, sets interpolated point, the step-length in the most each mobile section；

Step E, according to motion Supreme Being's inverse arithmetic, calculating machine hands arrives next interpolated point needs the kinematic parameter in each joint also Performing, the most each joint rotation angle and movement velocity realize；

Step F, carries out feeding back and repeating step A with artificial judgment, makes to arrive target location from mechanical arm.

Multiple degrees of freedom master-slave mode remote operating method for controlling robot the most according to claim 1, it is characterised in that: described In cloth step A, the method for described positive kinematics is the method for the positive kinematics using flute card coordinate system, is by sky that is each and that save Between the iteration of transformation matrix, calculate from the current location of mechanical arm tail end paw.

Multiple degrees of freedom master-slave mode remote operating method for controlling robot the most according to claim 2, it is characterised in that: in step In C, the route track set is as straight path or dog-leg path.

Multiple degrees of freedom master-slave mode remote operating method for controlling robot the most according to claim 3, it is characterised in that: in step The step-length set in D is as setting end next one interpolated point distance, and the single distance that namely paw moves, apart from current position Put step-length for (Δ X, Δ Y, Δ Z).

Multiple degrees of freedom master-slave mode remote operating method for controlling robot the most according to claim 4, it is characterised in that: step E Computational Methods is, θ₁=Atan 2 (p_y,p_x)；

Make k₁=p_xc₁+p_ys₁；

${\mathrm{\θ}}_2=\arctan \frac{k_1-a_1}{p_z}-\arctan \frac{k_2/{\mathrm{\ρ}}_2}{\±\sqrt{1-{(k_2/{\mathrm{\ρ}}_2)}^2}};$

Order

${\mathrm{\θ}}_3=\arctan \frac{a_3}{d_4}-\arctan \frac{k_3/{\mathrm{\ρ}}_3}{\±\sqrt{1-{(k_3/{\mathrm{\ρ}}_3)}^2}};$

${\mathrm{\θ}}_4=\arctan (-\frac{s_4}{c_4});$

${\mathrm{\θ}}_5=\arctan \left(\frac{s_5}{c_5}\right);{\mathrm{\θ}}_6=\arccos \left(\frac{c_3{n_y}^2-s_3{n_x}^2}{sin\left({\mathrm{\θ}}_5\right)}\right)$

Wherein θ₁For first articulation angle of mechanical arm, θ₂For first articulation angle of mechanical arm, θ₃For mechanical arm One articulation angle, θ₄For the 4th articulation angle of mechanical arm, θ₅For the 5th articulation angle of mechanical arm, θ₆ For the 6th articulation angle of mechanical arm；c_i=cos θ_i, s_i=sin θ_i, a_i=arctan θ_i(i=1,2 ... ..6)；

Speed during interpolation, is got by joint variable quantity and interpolation cycle ratio.