CN102350700A - Method for controlling robot based on visual sense - Google Patents

Abstract

The invention provides a method for controlling a robot based on visual sense. The method comprises the following steps of: (1) acquiring a gesture image of a human hand by using a camera; (2) extracting characteristic points of the human hand from the gesture image; (3) performing three-dimensional reconstruction on the characteristic points to obtain a position relation of the characteristic points of the human hand in a three-dimensional space; (4) converting coordinate points corresponding to the characteristic points of the human hand to be under a base coordinate of the robot; (5) performing inverse-solution calculation by using the position relation of the human hand under a base coordinate system of the robot to obtain a joint angle of the robot; and (6) driving the robot to run by using the calculated joint angle. The method has the advantages that: 1) the control is intuitive, and the holding gesture of the robot directly corresponds to the gesture of the human hand; 2) the control is flexible without contacting an onerous exchange tool; 3) an operator can be assisted to operate more accurately and safely by imitating the prior art; 4) the recovery is allowed to be interrupted or the operator is allowed to be replaced in midway; and 5) the operator does not need to walk in a wide range so that the operating pressure of the operator is reduced.

Description

A kind of robot control method based on vision

Technical field

The invention belongs to the robot field of human-computer interaction, particularly a kind of robot control method based on vision.

Background technology

Expansion day by day along with the robot application field; Particularly robot gets into daily life; People and robot equity mutual (peer-to-peer interaction) research more and more are much accounted of; In equity is mutual; People and robot more are partnerships, rather than simple people and instrument relation.

The teleoperation robot system comes with some shortcomings with full autonomous machine robot system, makes the man-robot cooperative system have very big potentiality.In this type systematic, people and the member of robot cooperate each other according to self-ability and accomplish goal task jointly, and the advantage of this team pattern is the next goal task of effectively accomplishing of the intelligence of integrated people and robot.

The research of man-robot interactive subject relates to many different field, for example different interactive modes, cognitive model and evaluation method etc.In interactive mode research, the interactive mode that meets the interpersonal communication custom is the focus of current research, however gesture be much accounted of from interpersonal communication model intuitively as a kind of, wherein to follow the tracks of be the basis of gesture identification to gesture.Because gesture the ability diversity, ambiguity and the time that have and the characteristics such as otherness on the space, the gesture interaction of current practicality is often according to application characteristic design certain semantic storehouse, to guarantee the accuracy and the validity of gesture interaction in addition.

The mechanical arm operation needs the instruction of a series of complicacies, and the control requirement of mechanical arm can not be satisfied in simple certain semantic storehouse.The difficult point of robotic arm manipulation is the control of attitude.Because mechanical arm imitates the structure of staff to design, the hand control of choosing so mechanical arm is a method very intuitively.

Summary of the invention

The shortcoming that the objective of the invention is to overcome above-mentioned prior art provides a kind of flexible natural a kind of robot control method based on vision with not enough.For reaching above-mentioned purpose, the present invention adopts following technical scheme:

A kind of robot control method based on vision is characterized in that, comprises the steps:

S1, obtain the staff images of gestures through video camera;

The characteristic point of staff in S2, the extraction images of gestures;

S3, characteristic point is carried out three-dimensional reconstruction, obtain the staff characteristic point and concern in three-dimensional position;

S4, the corresponding coordinate points of staff characteristic point is transformed under the robot basis coordinates;

S5, utilize that the position orientation relation of staff under robot basis coordinates system is counter separates calculating, obtain the joint angles of robot;

The joint angles drive machines people motion that S6, utilization calculate.

2, the robot control method based on vision according to claim 1 is characterized in that, said step S1 comprises: according to the binocular positioning principle, two cameras are installed above staff, are caught human hand movement image in real time.

In the above-mentioned robot control method based on vision; Said step S2 comprises: according to the characteristics of staff characteristic in the staff image; Carry out image through the method for feature point extraction and handle, obtain the pixel region of staff characteristic point, the central point that extracts the staff characteristic area then is as the staff characteristic point.In feature point extraction, need use 24 (R, G, B) color model, in the RGB color model, all colours can be by R, G, three kinds of colors of B are formed, different combinations presents various colors.For the characteristic point in the ability recognition image, utilize the characteristic point of red color mark hand, and yellow gloves are for the ease of colouring, provide the concrete color model of feature point extraction below.

The value of supposing pixel i (i gets positive integer) in the image is (R _i, G _i, B _i), the model of red pixel is among the identification figure so:

$\left\{\begin{array}{c}{R}_i>G_i+{\mathrm{\δ}}_g\\ R_i>B_i+{\mathrm{\δ}}_b\\ R_i>\mathrm{\δ}\end{array}\right.$

δ wherein _g, δ _b, δ is the color threshold values, and the R value must be only gauge point above the value on inequality the right in the remarked pixel, and the R value of above-mentioned inequality indicator sign point is than G, and it is big that the B value is wanted.

In the above-mentioned robot control method based on vision, said step S3 comprises: in step S2, can obtain the position of red-label point in the image of the left and right sides, in order to reconstruct the three-dimensional coordinate of gauge point, adopt binocular to rebuild principle and carry out three-dimensional reconstruction.

The binocular emplacement depth calculates:

$\frac{T+x_r-x_l}{Z-f}=\frac{T}{Z}$

Order: d=x _l-x _r:

$Z=f\frac{T}{d}$

Wherein P is a measurement point, and T is wide line, and f is a focal length, and Z is the distance that measurement point arrives wide line, Q _lBe the photocentre of left video camera, Q _rBe the center of right video camera, P _lBe the projection of measurement point on left video camera, P _rBe the projection of measurement point on right video camera, x ₁For left picture centre arrives left subpoint P _lVector, x _rFor right picture centre arrives right subpoint P _rVector.

In the above-mentioned robot control method based on vision, step S4 comprises: may further comprise the steps:

S41, the change in location through staff control robot;

S42, the attitude through staff control robot change;

S43, staff attitude and robot end's attitude are shone upon.

In the above-mentioned robot control method based on vision, step S41 comprises:

Owing in carrying out the mechanical arm control procedure, need not the operator walk about on a large scale, and the space of mechanical arm is bigger, can cause loss of significance with little space to the mapping of large space, so take the localization method of difference.At first behind the initialization mechanical arm, can obtain holding in hand terminal initial position (x through the normal solution algorithm ^p, y ^p, z ^p); Next in the video camera coverage, stipulate a working space (working space); Staff can only be in this working space motion; ELSE instruction lost efficacy; And then define a director space (Direction Space); Director space and working space form a space, and this space is used to change the position of mechanical arm:

x ^p＝x ^p+Δx*σ

y ^p＝z ^p+Δy*σ

z ^p＝z ^p+Δz*σ

Δ x, Δ y and Δ z are respectively the displacement of staff on three axis of orientations, and σ is an adjustable parameter, and then the control end position can be an immensity, can reach thick control and microcontrolled effect through the value of revising σ.

In the above-mentioned robot control method based on vision, step S42 comprises:

Hold terminal attitude and staff middle finger end in hand, the attitude of 3 compositions of groove point between forefinger end and thumb root and the forefinger root is consistent.

In the above-mentioned robot control method based on vision, step S43 comprises:

Earlier do not consider translation, suppose that the staff coordinate system overlaps with the initial point of console coordinate system.Transformation matrix is the matrix M of a 3*3, and a some A in the staff coordinate system then transforms in the console basis coordinates system and is A ', and A '=MA is arranged.

Wherein:

$M=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$

In the staff location, x axle unit vector P1[1 in the staff coordinate system, 0,0], y axle unit vector P2[0,1,0] and, z axle unit vector P3[0,0,1] under camera coordinate system be: [x ₁, x ₂, x ₃], [y ₁, y ₂, y ₃], [z ₁, z ₂, z ₃], have so:

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

Get by following formula:

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$

Because staff is consistent with respect to the posture changing matrix of console coordinate system with holding terminal posture changing matrix with respect to basis coordinates system in hand; Provided at location model and hold terminal translation relation in hand, be so obtain the transformation matrix of pose at last with respect to basis coordinates system:

$M^{\′}=\left[\begin{array}{cccc}{x}_1& y_1& z_1& p_1\\ x_2& y_2& z_2& p_2\\ x_3& y_3& z_3& p_3\\ 0& 0& 0& 1\end{array}\right]$

[p wherein ₁, p ₂, p ₃] for holding terminal translation matrix in hand with respect to basis coordinates system.

In the above-mentioned robot control method based on vision, step S5 comprises: may further comprise the steps:

In the Denavit-Hartenberg representation, A _iThe homogeneous coordinate transformation battle array (i get positive integer) of expression from coordinate system i-1 to coordinate system i usually has:

$A_i=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& l_i\cos {\mathrm{\θ}}_i\\ {\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& l_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]$

For a robot with n (n >=6) joint, the homogeneous transformation battle array from the support frame of axes to a last frame of axes is defined as:

$T_6=A_1{A}_2...A_n=\left[\begin{array}{cccc}{n}_n^0& s_n^0& a_n^0& p_n^0\\ 0& 0& 0& 1\end{array}\right]$

Where

is the normal vector of the gripper,

is the sliding vector, is close to vector,

is the position vector.

Above utilizing there be two formulas:

T _n＝M

Obtain n joint motions angle value through finding the solution following formula: (θ ₁, θ ₂..., θ _n).

In the above-mentioned robot control method based on vision, step S6 comprises: may further comprise the steps: utilize step S5 to calculate n joint angle angle drive machines people motion, thereby make the robot end reach desired locations.

The present invention has following advantage and technique effect with respect to prior art:

1, control is directly perceived, and robot holds the direct corresponding staff attitude of attitude in hand.

2, control need not to contact with heavy exchange tool flexibly.

3, utilize virtual reality technology assist operator more accurately more safely to operate.

4, allow to interrupt to recover or change the operator midway.

5, the operator need not to walk about on a large scale, reduces operator's operating pressure.

Description of drawings

Fig. 1 is the frame model figure in the embodiment;

Fig. 2 is location model figure;

Fig. 3 a, Fig. 3 b hold the terminal attitude and the attitude illustraton of model of 3 compositions of groove point between staff middle finger end, forefinger end and thumb root and the forefinger root in hand.

The specific embodiment

Below in conjunction with embodiment and accompanying drawing the present invention is described in further detail, but embodiment of the present invention is not limited thereto embodiment, that Fig. 1 provides is frame model figure.

This basis comprises the steps: based on vision control robotic method

S1, obtain the staff images of gestures through video camera;

The characteristic point of staff in S2, the extraction images of gestures;

S3, characteristic point is carried out three-dimensional reconstruction, obtain the staff characteristic point and concern in three-dimensional position;

S4, the corresponding coordinate points of staff characteristic point is transformed under the robot basis coordinates;

S5, utilize that the position orientation relation of staff under robot basis coordinates system is counter separates calculating, obtain the joint angles of robot;

The joint angles drive machines people motion that S6, utilization calculate.

Said step S1 may further comprise the steps:

S11, according to the binocular positioning principle, two cameras are installed above staff, catch human hand movement image in real time.

Said step S2 may further comprise the steps:

S21, vision according to claim 1 control robotic method; It is characterized in that; Said step S2 comprises: according to the characteristics of staff characteristic in the staff image; Carrying out image through the method for feature point extraction handles; Obtain the pixel region of staff characteristic point, the central point that extracts the staff characteristic area then is as the staff characteristic point.In feature point extraction, need use 24 (R, G, B) color model, in the RGB color model, all colours can be by R, G, three kinds of colors of B are formed, different combinations presents various colors.For the characteristic point in the ability recognition image, utilize the characteristic point of red color mark hand, and yellow gloves are for the ease of colouring.

Said step S3 may further comprise the steps:

S31, in the staff recognition system, can obtain the position of red-label point in the image of the left and right sides, in order to reconstruct the three-dimensional coordinate of gauge point, adopt binocular to rebuild principle and carry out three-dimensional reconstruction.

Said step S4 may further comprise the steps:

S41, owing in carrying out the mechanical arm control procedure, need not the operator walk about on a large scale, and the space of mechanical arm is bigger, can cause loss of significance with little space to the mapping of large space, so take the localization method of difference.At first behind the initialization mechanical arm, can obtain holding in hand terminal initial position (x through the normal solution algorithm ^p, y ^p, z ^p); As shown in Figure 2; Next in the video camera coverage, stipulate a working space (working space); Staff can only be in this working space motion; ELSE instruction lost efficacy; And then define a director space (Direction Space), and director space and working space form a space, and this space is used to change the position of mechanical arm:

x ^p＝x ^p+Δx*σ

y ^p＝z ^p+Δy*σ

z ^p＝z ^p+Δz*σ

Δ x, Δ y and Δ z are respectively the displacement of staff on three axis of orientations, and σ is an adjustable parameter, and then the control end position can be an immensity, can reach thick control and microcontrolled effect through the value of revising σ.

S42, hold terminal attitude and staff middle finger end in hand, the attitude of 3 compositions of groove point between forefinger end and thumb root and the forefinger root is consistent, shown in Fig. 3 a and Fig. 3 b.

S43, elder generation do not consider translation, suppose that the staff coordinate system overlaps with the initial point of console coordinate system.Transformation matrix is the matrix M of a 3*3, and a some A in the staff coordinate system then transforms in the console basis coordinates system and is A ', and A '=MA is arranged.

Wherein:

$M=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$

In the staff location, x axle unit vector P1[1 in the staff coordinate system, 0,0], y axle unit vector P2[0,1,0] and, z axle unit vector P3[0,0,1] under camera coordinate system be: [x ₁, x ₂, x ₃], [y ₁, y ₂, y ₃], [z ₁, z ₂, z ₃], have so:

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

Get by following formula:

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$

Because staff is consistent with respect to the posture changing matrix of console coordinate system with holding terminal posture changing matrix with respect to basis coordinates system in hand; Provided at location model and hold terminal translation relation in hand, be so obtain the transformation matrix of pose at last with respect to basis coordinates system:

$M^{\′}=\left[\begin{array}{cccc}{x}_1& y_1& z_1& p_1\\ x_2& y_2& z_2& p_2\\ x_3& y_3& z_3& p_3\\ 0& 0& 0& 1\end{array}\right]$

[p wherein ₁, p ₂, p ₃] for holding terminal translation matrix in hand with respect to basis coordinates system.

Said step S5 may further comprise the steps:

S51, in the Denavit-Hartenberg representation, A _iThe homogeneous coordinate transformation battle array (i get positive integer) of expression from coordinate system i-1 to coordinate system i usually has:

$A_i=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& l_i\cos {\mathrm{\θ}}_i\\ {\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& l_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]$

For a robot with n (n >=6) joint, the homogeneous transformation battle array from the support frame of axes to a last frame of axes is defined as:

$T_6=A_1{A}_2...A_n=\left[\begin{array}{cccc}{n}_n^0& s_n^0& a_n^0& p_n^0\\ 0& 0& 0& 1\end{array}\right]$

Where is the normal vector of the gripper,

is the sliding vector,

is close to vector, is the position vector.

Above utilizing there be two formulas:

T _n＝M

Obtain n joint motions angle value through finding the solution following formula: (θ ₁, θ ₂..., θ _n).

Said step S6 may further comprise the steps:

S61, utilize step S5 to calculate n joint angle angle drive machines people motion, thereby make the robot end reach desired locations.

The foregoing description is a preferred implementation of the present invention; But embodiment of the present invention is not restricted to the described embodiments; Other any do not deviate from change, the modification done under spirit of the present invention and the principle, substitutes, combination, simplify; All should be the substitute mode of equivalence, be included within protection scope of the present invention.

Claims (10)

1. the robot control method based on vision is characterized in that, comprises the steps:

S1, obtain the staff images of gestures through video camera;

The characteristic point of staff in S2, the extraction images of gestures;

S3, characteristic point is carried out three-dimensional reconstruction, obtain the staff characteristic point and concern in three-dimensional position;

S4, the corresponding coordinate points of staff characteristic point is transformed under the robot basis coordinates;

S5, utilize that the position orientation relation of staff under robot basis coordinates system is counter separates calculating, obtain the joint angles of robot;

The joint angles drive machines people motion that S6, utilization calculate.

2. the robot control method based on vision according to claim 1 is characterized in that, said step S1 comprises: according to the binocular positioning principle, two cameras are installed above staff, are caught human hand movement image in real time.

3. the robot control method based on vision according to claim 1; It is characterized in that; Said step S2 comprises the characteristics according to staff characteristic in the staff image; Carrying out image through the method for feature point extraction handles; Obtain the pixel region of staff characteristic point, the central point that extracts the staff characteristic area then is as the staff characteristic point; In feature point extraction, need use 24 RGB color model, in the RGB color model, all colours is by R, G, and three kinds of colors of B are formed, and different combinations presents various colors; For the characteristic point in the ability recognition image, utilize the characteristic point of red color mark hand, and yellow gloves are for the ease of colouring, provide the concrete color model of feature point extraction below:

The value of supposing pixel i in the image is (R _i, G _i, B _i), i gets positive integer, and the model of red pixel is among the identification figure so:

$\left\{\begin{array}{c}{R}_i>G_i+{\mathrm{\δ}}_g\\ R_i>B_i+{\mathrm{\δ}}_b\\ R_i>\mathrm{\δ}\end{array}\right.$

δ wherein _g, δ _b, δ is the color threshold values, and the R value must be only gauge point above the value on inequality the right in the remarked pixel, and the R value of above-mentioned inequality indicator sign point is than G, and it is big that the B value is wanted.

4. the robot control method based on vision according to claim 1; It is characterized in that said step S3 comprises: in step S2, obtain the position of red-label point in the image of the left and right sides, in order to reconstruct the three-dimensional coordinate of gauge point; Adopt binocular to rebuild principle and carry out three-dimensional reconstruction

The binocular emplacement depth calculates:

$\frac{T+x_r-x_l}{Z-f}=\frac{T}{Z}$

Order: d=x _l-x _r:

$Z=f\frac{T}{d}$

Wherein P is a measurement point, and T is wide line, and f is a focal length, and Z is the distance that measurement point arrives wide line, Q _lBe the photocentre of left video camera, Q _rBe the center of right video camera, P _lBe the projection of measurement point on left video camera, P _rBe the projection of measurement point on right video camera, x ₁For left picture centre arrives left subpoint P _lVector, x _rFor right picture centre arrives right subpoint P _rVector.

5. the robot control method based on vision according to claim 1 is characterized in that step S4 comprises: may further comprise the steps:

S41, the change in location through staff control robot;

S42, the attitude through staff control robot change;

S43, staff attitude and robot end's attitude are shone upon.

6. the robot control method based on vision according to claim 5 is characterized in that step S41 comprises:

At first behind the initialization mechanical arm, can obtain holding in hand terminal initial position (x through the normal solution algorithm ^p, y ^p, z ^p), next in the video camera coverage, stipulate a working space, staff can only be in this working space motion; ELSE instruction lost efficacy; And then define a director space, and director space and working space form a space, and this space is used to change the position of mechanical arm:

x ^p＝x ^p+Δx*σ

y ^p＝z ^p+Δy*σ

z ^p＝z ^p+Δz*σ

Δ x, Δ y and Δ z are respectively the displacement of staff on three axis of orientations, and σ is an adjustable parameter, and then the control end position is an immensity, slightly control and microcontroller through the value of revising σ.

7. the robot control method based on vision according to claim 5 is characterized in that step S42 comprises:

The attitude of holding 3 compositions of groove point between terminal attitude and staff middle finger end, forefinger end and thumb root and the forefinger root in hand is consistent.

8. the robot control method based on vision according to claim 5 is characterized in that step S43 comprises:

Suppose that the staff coordinate system overlaps with the initial point of console coordinate system; Transformation matrix is one 3 * 3 a matrix M, and a some A in the staff coordinate system then transforms in the console basis coordinates system and is A ', and A '=MA is arranged,

Wherein:

$M=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]$

In the staff location, x axle unit vector P1[1 in the staff coordinate system, 0,0], y axle unit vector P2[0,1,0] and, z axle unit vector P3[0,0,1] under camera coordinate system be: [x ₁, x ₂, x ₃], [y ₁, y ₂, y ₃], [z ₁, z ₂, z ₃], have so:

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 1\\ 0\end{array}\right]=\left[\begin{array}{c}{y}_1\\ y_2\\ y_3\end{array}\right]$

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]=\left[\begin{array}{c}{z}_1\\ z_2\\ z_3\end{array}\right]$

Get by following formula:

$\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ x_3& y_3& z_3\end{array}\right]$

Because staff is consistent with respect to the posture changing matrix of console coordinate system with holding terminal posture changing matrix with respect to basis coordinates system in hand; Provided at location model and hold terminal translation relation in hand, be so obtain the transformation matrix of pose at last with respect to basis coordinates system:

$M^{\′}=\left[\begin{array}{cccc}{x}_1& y_1& z_1& p_1\\ x_2& y_2& z_2& p_2\\ x_3& y_3& z_3& p_3\\ 0& 0& 0& 1\end{array}\right]$

[p wherein ₁, p ₂, p ₃] for holding terminal translation matrix in hand with respect to basis coordinates system.

9. the robot control method based on vision according to claim 1 is characterized in that step S5 comprises: may further comprise the steps:

In the Denavit-Hartenberg representation, A _iThe homogeneous coordinate transformation battle array of expression from coordinate system i-1 to coordinate system i, i gets positive integer, has:

$A_i=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_i& -\sin {\mathrm{\θ}}_i\cos {\mathrm{\α}}_i& \sin {\mathrm{\θ}}_i\sin {\mathrm{\α}}_i& l_i\cos {\mathrm{\θ}}_i\\ {\sin \mathrm{\θ}}_i& {\cos \mathrm{\θ}}_i{\cos \mathrm{\α}}_i& -{\cos \mathrm{\θ}}_i{\sin \mathrm{\α}}_i& l_i{\sin \mathrm{\θ}}_i\\ 0& {\sin \mathrm{\α}}_i& {\cos \mathrm{\α}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]$

For a robot with n joint, n >=6, the homogeneous transformation battle array from the support frame of axes to a last frame of axes is defined as:

$T_6=A_1{A}_2...A_n=\left[\begin{array}{cccc}{n}_n^0& s_n^0& a_n^0& p_n^0\\ 0& 0& 0& 1\end{array}\right]$

Where for the gripper normal vector, is the sliding vector,

is close to vector,

is the position vector;

Above utilizing there be two formulas:

T _n＝M

Obtain n joint motions angle value through finding the solution following formula: (θ ₁, θ ₂..., θ _n).

10. the robot control method based on vision according to claim 1 is characterized in that, step S6 comprises that utilizing step S5 to calculate n joint angle angle drive machines people moves, thereby makes the robot end reach desired locations.

Images