CN105137973A - Method for robot to intelligently avoid human under man-machine cooperation scene - Google Patents

Abstract

The invention discloses a method for a robot to intelligently avoid human under a man-machine cooperation scene, comprising steps of constructing a robot column model of a simulation scene through robot D-H parameters, (2) obtaining RGB images of the real scene and skeleton data of an operator through Kinect, and constructing the column model of the simulation scene operator, 3) corresponding calibrations of the robot and the simulator in the real scene and the simulation scene , and 4) performing real-time detection collision of the robot model and the operator model. By using the method disclosed by the invention, the operator can freely move in the space where the robot operates, but the operator can avoid the obstacle according to the operator information obtained by the Kinect.

Description

Intelligent robot under a kind of man-machine collaboration scene hides people's class methods

Technical field

The present invention relates to the technical field of robot motion, the intelligent robot referred in particular under a kind of man-machine collaboration scene hides people's class methods.

Background technology

Intelligent robot intersects and fusion with other field, creates many scenes needing human and computer people to cooperate.Intelligent robot participates in the operating environment of people, and under the prerequisite ensureing work efficiency, the labour of all right effective liberate people, in particular cases can also replace people to complete high risk task.Under the background that human and computer people closely cooperates, how the safety of underwriter is vital problem.This section of invention proposes the modeling of robot under a kind of man-machine collaboration scene and keeps away the method for barrier, the method builds robot cylinder model according to robot D-H parameter, use Kinect sensor acquisition operations person bone site, build the cylinder model of operator with this, and identify with making intelligent robot according to the collision detection result of robot and operator and hide human body.

When building Robot Virtual scene, common way utilizes the modeling tools such as 3dsMax to build the virtual machine human model completely corresponding with the robot of real scene, but this method does not have a versatility, and modeling cost is relatively high, therefore builds robot model by the D-H parameter that robot is intrinsic and be necessary.

In man-machine collaboration scene, robot, according to the position of human body and action message, makes and dodging, and avoids colliding.But traditional robot collision avoidance research is mainly to the collision prevention of object, different from environmental objects, people possesses self architectural feature, and body state and motion also have unpredictability, and therefore robot needs to identify in real time human body.

Before Kinect occurs, the human body information acquisition technique based on many vision camera is relatively popular, but the method needs fluoroscopy images, and relative cost is also higher.In the many vision camera human body recognition technology based on mark, human body will wear specific clothes or equipment, is in limited state in active procedure, limits the application of this technology.Extract acquisition technique based on unmarked human body information then to need the priori such as human body image, three-dimensional information and objective function to mate, matching process needs enough sample sizes, otherwise identification accuracy can be caused lower, is also comparatively difficult to ensure card real-time.

Summary of the invention

The object of the invention is to overcome the deficiencies in the prior art, the intelligent robot under a kind of man-machine collaboration scene is provided to hide people's class methods, operator can free movement in robot manipulating task space, the information of the operator that robot obtains according to Kinect carry out initiatively keep away barrier.

For achieving the above object, technical scheme provided by the present invention is: the intelligent robot under a kind of man-machine collaboration scene hides people's class methods, comprises the following steps:

1) the robot cylinder model of virtual scene is built by robot D-H parameter, as follows:

Suppose that robot is made up of with arbitrary form a series of connecting rod and joint, D-H Parametric Representation be relation between joint of robot and connecting rod; In order to carry out modeling to robot, adopt point to carry out modeling to joint, right cylinder carries out modeling to connecting rod; Relate to the positive kinematics of robot, by the initial angle in each joint of robot, solve the transformation relation of the relative basis coordinates system of coordinate system in each joint; Define a right-handed coordinate system to each joint, the homogeneous transformation between the coordinate system in a joint and the coordinate system in next joint is called A matrix, A ₁represent that first joint is relative at the position of basis coordinates system and attitude, so A ₂then be expressed as second joint relative to the position in first joint and attitude, and second joint can be made up of following matrix product relative in the position of basis coordinates system and attitude:

T ₂＝A ₁A ₂(1)

By that analogy, the n-th joint relative to the formula in the position of basis coordinates system and attitude is:

$T_n=A_1{A}_2...A_n=\left(\begin{array}{cc}{q}_n^{3\×3}& p_n^{3\×1}\\ 0& 1\end{array}\right)---\left(2\right)$

In formula, represent the attitude in the n-th joint, represent the position of the n-th joint relative to basis coordinates system; Wherein A _ican according to D-H Parametric Representation:

$A_i=\left(\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\cos \left({\mathrm{\θ}}_i\right)\\ \sin \left({\mathrm{\θ}}_i\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& -\cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\sin \left({\mathrm{\θ}}_i\right)\\ 0& \sin \left({\mathrm{\α}}_i\right)& \cos \left({\mathrm{\α}}_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

θ _i, d _i, a _i, α _ifor the D-H parameter of joint of robot i;

Be the coordinate system of model with robot basis coordinates when building the robot cylinder model of virtual scene, solve the position of each joint relative to basis coordinates system connecting rod between adjacent segment adopts right cylinder to carry out modeling, and the cylindrical upper bottom surface center of circle is the position of two joint point, and cylinder radius adjusts according to actual conditions, builds the robot cylinder model of 6DOF;

2) by the RGB image of Kinect Real-time Obtaining real scene and the skeleton data of operator, and operator's cylinder model of virtual scene is built, as follows:

Operator, when entering into the operation interval of robot, by the RGB image of Kinect Real-time Obtaining real scene be fixed on before operator and the skeleton data of operator, realizes the tracking to operator and location; Kinect has three kinds of cameras: a kind of RGB colour imagery shot for gathering coloured image and two kinds of infrared cameras for sampling depth image;

For the acquisition of real scene RGB image, Kinect is put certain position in the environment, open coloured image NUI_INITIALIZE_FLAG_USES_COLOR and carry out initialization Kinect, by the coloured image frame data obtained, draw out with OpenCV;

For the acquisition of skeleton data, open skeleton data NUI_INITIALIZE_FLAG_USES_SKELETON and carry out initialization Kinect, when people is in standing state, Kinect can get the position of 20 articulation points of people to represent the skeleton of people; Extract the cylinder model that 15 articulation points build operator in virtual scene, these 15 articulation points sort from top to bottom and from left to right and are: 1. head; ②Jian center; 3. right shoulder; 4. right hand elbow; 5. the right hand; 6. left shoulder; 7. left hand elbow; 8. left hand; 9. hip joint center; 10. right hip; right knee; right crus of diaphragm; left hip; left knee; left foot; The position of these articulation points is all the position relative to Kinect coordinate system;

Being the coordinate system of model with Kinect coordinate when building operator's cylinder model, by the process of Kinect to depth image, obtaining the position of 15 articulation points of operator adopt right cylinder to carry out modeling to articulation point adjacent in human skeleton, the cylindrical upper bottom surface center of circle is the position of two joint point, and cylinder radius adjusts according to actual conditions;

3) in real scene and virtual scene, the demarcation of robot and operator is corresponding, as follows:

In above-mentioned two steps, be the coordinate system of model with robot basis coordinates to the modeling of robot in virtual scene, be the coordinate system of model with Kinect coordinate to the modeling of operator in virtual scene, in order to virtual scene and real scene are carried out correspondence, choose a coordinate system in real scene, be called world coordinate system E1, basis coordinates system of robot is called coordinate system E2, and Kinect coordinate system is called coordinate system E3;

Relation between robot basis coordinates system E2 and world coordinate system E1 rotation matrix R and translation matrix T represent, if the coordinate of certain articulation point P of robot under basis coordinates system of robot is coordinate under world coordinate system is so the pass between them is:

$\left(\begin{array}{c}{X}_p^{E1}\\ Y_p^{E1}\\ Z_p^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E2}}_{E1}& {T_{E2}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)=M_2\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)---\left(4\right)$

In formula, ^e2r _e1for 3X3 matrix, represent the attitudes vibration matrix of basis coordinates system of robot relative to world coordinate system; ^e2t _e1for 3X1 matrix, represent the change in location matrix of basis coordinates system of robot relative to world coordinate system; M ₂for 4X4 matrix, represent the pose transformation matrices of basis coordinates system of robot relative to world coordinate system;

In like manner, the relation between Kinect coordinate system E3 and world coordinate system E1 also represents by rotation matrix R and translation matrix T, if the coordinate of certain articulation point P ' of operator under Kinect coordinate system is coordinate under world coordinate system is pass then between them is:

$\left(\begin{array}{c}{X}_{p^{\′}}^{E1}\\ Y_{p^{\′}}^{E1}\\ Z_{p^{\′}}^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E3}}_{E1}& {T_{E3}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)=M_3\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)---\left(5\right)$

After basis coordinates system of robot and Kinect coordinate system are all transformed into world coordinate system, be next exactly the mapping relations determining real scene and virtual scene and the error weighed between them; If certain 1 P in virtual scene _vcoordinate be then this P _rcoordinate in real scene RGB image is then there is one and map f, make to put P arbitrarily in virtual scene _vand the some P in corresponding real scene _r, there is following relation:

P _r＝f(P _v)+ε(6)

In formula, ε is the error that virtual scene and real scene are corresponding;

Kinect is obtained to bone three-dimensional data and the coloured image 2-D data of operator, KinectSDK provides mutual conversion between the two, namely provides the mapping f of virtual scene operator and real scene operator _person; NuiTransformSkeletonToDepthImage provides the mapping relationship f of three-dimensional skeleton data to two-dimensional depth image ₁, NuiImageGetColorPixelCoordinatesFromDepthPixel provides the mapping f of two-dimensional depth image to Two-dimensional Color Image ₂, the mapping relations of three-dimensional skeleton data to Two-dimensional Color Image can be learnt thus:

f _person＝f ₂·f ₁(7)

So far, the operator of operator's real scene of virtual scene achieves correspondence, for robot, because basis coordinates system of robot and Kinect coordinate system are transformed into world coordinate system all, then robot and the virtual scene residing for operator are the same with the coordinate system of real scene, then the mapping relationship f of virtual scene robot and real scene robot _robotwith the mapping relationship f of virtual scene operator and real scene operator _personthe same:

f _robot＝f _person(8)

Then for whole virtual scene and real scene, the formula of the mapping relationship f between them is:

f＝f _robot＝f _person＝f ₂·f ₁(9)

In conjunction with formula (6) and formula (9) above, the error ε of virtual scene and real scene can be weighed:

ε＝P _r-f ₂·f ₁(P _v)(10)

4) real time collision detection of robot model and operator's model is as follows:

When operator enters in the operating environment of robot, the information of operator has unpredictability for robot, directly can have influence on efficiency and the precision of collision detection to the modeling strategy of operator and robot; Adopt more regular geometric configuration right cylinder to carry out modeling to robot and operator per capita to operator and machine, the collision detection concrete scheme based on right cylinder bounding box is as follows:

If A, B be a cylinder on to go to the bottom the round heart, C, D be another cylinder on to go to the bottom the round heart, space line l _aB, l _cDbe the axis of two cylinders, the main situation considering two straight line antarafacials, asks for two different surface beeline l _aB, l _cDcommon vertical line and intersection point P, Q, if P or Q be not in line segment AB or line segment CD, then carry out Boundary Detection, the end points chosen near other straight line substitutes P or Q point;

If P, Q are all in line segment AB and line segment CD, then two cylinders can only be that side is intersected, and now only need to judge the length of line segment PQ and the radius sum of two cylinders;

If there is a point to drop in line segment in P, Q, a point drops on end points, then two cylinders can only side and end face intersect, and now finds the bus near end face on side, judges that whether bus is crossing with end face;

If P, Q are all on the end points of line segment, then two cylinders can only be that end face intersects, and now only need to judge spatially whether two discs intersect.

Compared with prior art, tool has the following advantages and beneficial effect in the present invention:

The inventive method adopts more regular geometry shape right cylinder to carry out modeling to robot and operator, has good versatility and applicability, and right cylinder modeling simultaneously can improve efficiency and the precision of collision detection to a certain extent.Make operator can free movement in robot manipulating task space, the information of the operator that robot can obtain according to Kinect carry out initiatively keep away barrier, meet operation requirements.

Accompanying drawing explanation

Fig. 1 is the system construction drawing of the inventive method.

Fig. 2 is 6DOF robot cylinder model figure.

Fig. 3 is operator's cylinder model figure.

Embodiment

Below in conjunction with specific embodiment, the invention will be further described.

Intelligent robot under man-machine collaboration scene described in the present embodiment hides people's class methods, make operator can free movement in robot manipulating task space, robot according to Kinect obtain the information of operator carry out initiatively keep away barrier, its system construction drawing is as shown in Figure 1.The key of the inventive method is the collision checking method building the virtual machine human model corresponding with true man-machine collaboration scene and operator's model and robot model and operator's model, and it comprises the following steps:

1) the robot cylinder model of virtual scene is built by robot D-H parameter, as follows:

Suppose that robot is made up of with arbitrary form a series of connecting rod and joint, D-H Parametric Representation be relation between joint of robot and connecting rod.In order to carry out modeling to robot, we adopt and a little carry out modeling to joint, and right cylinder carries out modeling to connecting rod.Relate to the positive kinematics of robot in the present invention, by the initial angle in each joint of robot, solve the transformation relation of the relative basis coordinates system of coordinate system in each joint.Such as: define a right-handed coordinate system to each joint, the homogeneous transformation between the coordinate system in a joint and the coordinate system in next joint is called A matrix.A ₁represent that first joint is relative at the position of basis coordinates system and attitude, so A ₂then be expressed as second joint relative to the position in first joint and attitude, and second joint can be made up of following matrix product relative in the position of basis coordinates system and attitude:

T ₂＝A ₁A ₂(1)

By that analogy, the n-th joint relative to the formula in the position of basis coordinates system and attitude is:

$T_n=A_1{A}_2...A_n=\left(\begin{array}{cc}{q}_n^{3\×3}& p_n^{3\×1}\\ 0& 1\end{array}\right)---\left(2\right)$

In formula, represent the attitude in the n-th joint, represent the position of the n-th joint relative to basis coordinates system; Wherein A _ican according to D-H Parametric Representation:

$A_i=\left(\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\cos \left({\mathrm{\θ}}_i\right)\\ \sin \left({\mathrm{\θ}}_i\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& -\cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\sin \left({\mathrm{\θ}}_i\right)\\ 0& \sin \left({\mathrm{\α}}_i\right)& \cos \left({\mathrm{\α}}_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

θ _i, d _i, a _i, α _ifor the D-H parameter of joint of robot i;

Be the coordinate system of model with robot basis coordinates when the present invention builds the robot cylinder model of virtual scene, solve the position of each joint relative to basis coordinates system connecting rod between adjacent segment adopts right cylinder to carry out modeling, and the cylindrical upper bottom surface center of circle is the position of two joint point, and cylinder radius adjusts according to actual conditions, and the robot cylinder model of the 6DOF of structure as shown in Figure 2.

2) by the RGB image of Kinect Real-time Obtaining real scene and the skeleton data of operator, and operator's cylinder model of virtual scene is built, as follows:

Operator, when entering into the operation interval of robot, by the RGB image of Kinect Real-time Obtaining real scene be fixed on before operator and the skeleton data of operator, realizes the tracking to operator and location.Kinect has three kinds of cameras: a kind of RGB colour imagery shot for gathering coloured image and two kinds of infrared cameras for sampling depth image.

For the acquisition of real scene RGB image, Kinect is put certain position in the environment, open coloured image NUI_INITIALIZE_FLAG_USES_COLOR and carry out initialization Kinect, by the coloured image frame data obtained, draw out with OpenCV.

For the acquisition of skeleton data, open skeleton data NUI_INITIALIZE_FLAG_USES_SKELETON and carry out initialization Kinect, when people is in standing state, Kinect can get the position of 20 articulation points of people to represent the skeleton of people.In the present invention, extract the cylinder model that 15 articulation points build operator in virtual scene, these 15 articulation points sort from top to bottom and from left to right and are: 1. head; ②Jian center; 3. right shoulder; 4. right hand elbow; 5. the right hand; 6. left shoulder; 7. left hand elbow; 8. left hand; 9. hip joint center; 10. right hip; right knee; right crus of diaphragm; left hip; left knee; left foot; The position of these articulation points is all the position relative to Kinect coordinate system.

The present invention is the coordinate system of model with Kinect coordinate when building operator's cylinder model, by the process of Kinect to depth image, obtains the position of 15 articulation points of operator adopt right cylinder to carry out modeling to articulation point adjacent in human skeleton, the cylindrical upper bottom surface center of circle is the position of two joint point, and cylinder radius adjusts according to actual conditions, and operator's cylinder model of structure as shown in Figure 3.

3) in real scene and virtual scene, the demarcation of robot and operator is corresponding, as follows:

In above-mentioned two steps, be the coordinate system of model with robot basis coordinates to the modeling of robot in virtual scene, be the coordinate system of model with Kinect coordinate to the modeling of operator in virtual scene, in order to virtual scene and real scene are carried out correspondence, choose a coordinate system in real scene, be called world coordinate system E1, basis coordinates system of robot is called coordinate system E2, and Kinect coordinate system is called coordinate system E3.

Relation between robot basis coordinates system E2 and world coordinate system E1 can represent by rotation matrix R and translation matrix T, and such as, the coordinate of certain articulation point P of robot under basis coordinates system of robot is coordinate under world coordinate system is so the pass between them is:

$\left(\begin{array}{c}{X}_p^{E1}\\ Y_p^{E1}\\ Z_p^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E2}}_{E1}& {T_{E2}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)=M_2\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)---\left(4\right)$

In formula, ^e2r _e1for 3X3 matrix, represent the attitudes vibration matrix of basis coordinates system of robot relative to world coordinate system; ^e2t _e1for 3X1 matrix, represent the change in location matrix of basis coordinates system of robot relative to world coordinate system; M ₂for 4X4 matrix, represent the pose transformation matrices of basis coordinates system of robot relative to world coordinate system.

In like manner, the relation between Kinect coordinate system E3 and world coordinate system E1 also can represent by rotation matrix R and translation matrix T, and such as, the coordinate of certain articulation point P ' of operator under Kinect coordinate system is coordinate under world coordinate system is pass then between them is:

$\left(\begin{array}{c}{X}_{p^{\′}}^{E1}\\ Y_{p^{\′}}^{E1}\\ Z_{p^{\′}}^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E3}}_{E1}& {T_{E3}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)=M_3\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)---\left(5\right)$

After basis coordinates system of robot and Kinect coordinate system are all transformed into world coordinate system, be next exactly the mapping relations determining real scene and virtual scene and the error weighed between them.Such as, if certain 1 P in virtual scene _vcoordinate be then this P _rcoordinate in real scene RGB image is then there is one and map f, make to put P arbitrarily in virtual scene _vand the some P in corresponding real scene _r, there is following relation:

P _r＝f(P _v)+ε(6)

In formula, ε is the error that virtual scene and real scene are corresponding.

Kinect is obtained to bone three-dimensional data and the coloured image 2-D data of operator, KinectSDK provides mutual conversion between the two, namely provides the mapping f of virtual scene operator and real scene operator _person; NuiTransformSkeletonToDepthImage provides the mapping relationship f of three-dimensional skeleton data to two-dimensional depth image ₁, NuiImageGetColorPixelCoordinatesFromDepthPixel provides the mapping f of two-dimensional depth image to Two-dimensional Color Image ₂, the mapping relations of three-dimensional skeleton data to Two-dimensional Color Image can be learnt thus:

f _person＝f ₂·f ₁(7)

So far, the operator of operator's real scene of virtual scene achieves correspondence, for robot, because basis coordinates system of robot and Kinect coordinate system are transformed into world coordinate system all, then robot and the virtual scene residing for operator are the same with the coordinate system of real scene, then the mapping relationship f of virtual scene robot and real scene robot _robotwith the mapping relationship f of virtual scene operator and real scene operator _personthe same:

f _robot＝f _person(8)

Then for whole virtual scene and real scene, the formula of the mapping relationship f between them is:

f＝f _robot＝f _person＝f ₂·f ₁(9)

In conjunction with formula (6) and formula (9) above, the error ε of virtual scene and real scene can be weighed:

ε＝P _r-f ₂·f ₁(P _v)(10)

4) real time collision detection of robot model and operator's model is as follows:

When operator enters in the operating environment of robot, the information of operator has unpredictability for robot, directly can have influence on efficiency and the precision of collision detection to the modeling strategy of operator and robot.In the present invention, adopt more regular geometric configuration right cylinder to carry out modeling to robot and operator per capita to operator and machine, the collision detection concrete scheme based on right cylinder bounding box is as follows:

If A, B be a cylinder on to go to the bottom the round heart, C, D be another cylinder on to go to the bottom the round heart, space line l _aB, l _cDbe the axis of two cylinders, the main situation considering two straight line antarafacials, asks for two different surface beeline l _aB, l _cDcommon vertical line and intersection point P, Q, if P or Q be not in line segment AB or line segment CD, then carry out Boundary Detection, the end points chosen near other straight line substitutes P or Q point.

If P, Q are all in line segment AB and line segment CD, then two cylinders can only be that side is intersected, and now only need to judge the length of line segment PQ and the radius sum of two cylinders.

If there is a point to drop in line segment in P, Q, a point drops on end points, then two cylinders can only side and end face intersect, and now finds the bus near end face on side, judges that whether bus is crossing with end face.

If P, Q are all on the end points of line segment, then two cylinders can only be that end face intersects, and now only need to judge spatially whether two discs intersect.

The examples of implementation of the above are only the preferred embodiment of the present invention, not limit practical range of the present invention with this, therefore the change that all shapes according to the present invention, principle are done, all should be encompassed in protection scope of the present invention.

Claims (1)

1. the intelligent robot under man-machine collaboration scene hides people's class methods, it is characterized in that, comprises the following steps:

1) the robot cylinder model of virtual scene is built by robot D-H parameter, as follows:

Suppose that robot is made up of with arbitrary form a series of connecting rod and joint, D-H Parametric Representation be relation between joint of robot and connecting rod; In order to carry out modeling to robot, adopt point to carry out modeling to joint, right cylinder carries out modeling to connecting rod; Relate to the positive kinematics of robot, by the initial angle in each joint of robot, solve the transformation relation of the relative basis coordinates system of coordinate system in each joint; Define a right-handed coordinate system to each joint, the homogeneous transformation between the coordinate system in a joint and the coordinate system in next joint is called A matrix, A ₁represent that first joint is relative at the position of basis coordinates system and attitude, so A ₂then be expressed as second joint relative to the position in first joint and attitude, and second joint can be made up of following matrix product relative in the position of basis coordinates system and attitude:

T ₂＝A ₁A ₂(1)

By that analogy, the n-th joint relative to the formula in the position of basis coordinates system and attitude is:

$T_n=A_1{A}_2...A_n=\left(\begin{array}{cc}{q}_n^{3\×3}& p_n^{3\×1}\\ 0& 1\end{array}\right)---\left(2\right)$

In formula, represent the attitude in the n-th joint, represent the position of the n-th joint relative to basis coordinates system; Wherein A _ican according to D-H Parametric Representation:

$A_i=\left(\begin{array}{cccc}\cos \left({\mathrm{\θ}}_i\right)& -\sin \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& \sin \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\cos \left({\mathrm{\θ}}_i\right)\\ \sin \left({\mathrm{\θ}}_i\right)& \cos \left({\mathrm{\θ}}_i\right)\cos \left({\mathrm{\α}}_i\right)& -\cos \left({\mathrm{\θ}}_i\right)\sin \left({\mathrm{\α}}_i\right)& a_i\sin \left({\mathrm{\θ}}_i\right)\\ 0& \sin \left({\mathrm{\α}}_i\right)& \cos \left({\mathrm{\α}}_i\right)& d_i\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

θ _i, d _i, a _i, α _ifor the D-H parameter of joint of robot i;

Be the coordinate system of model with robot basis coordinates when building the robot cylinder model of virtual scene, solve the position of each joint relative to basis coordinates system connecting rod between adjacent segment adopts right cylinder to carry out modeling, and the cylindrical upper bottom surface center of circle is the position of two joint point, and cylinder radius adjusts according to actual conditions, builds the robot cylinder model of 6DOF;

2) by the RGB image of Kinect Real-time Obtaining real scene and the skeleton data of operator, and operator's cylinder model of virtual scene is built, as follows:

Operator, when entering into the operation interval of robot, by the RGB image of Kinect Real-time Obtaining real scene be fixed on before operator and the skeleton data of operator, realizes the tracking to operator and location; Kinect has three kinds of cameras: a kind of RGB colour imagery shot for gathering coloured image and two kinds of infrared cameras for sampling depth image;

For the acquisition of real scene RGB image, Kinect is put certain position in the environment, open coloured image NUI_INITIALIZE_FLAG_USES_COLOR and carry out initialization Kinect, by the coloured image frame data obtained, draw out with OpenCV;

For the acquisition of skeleton data, open skeleton data NUI_INITIALIZE_FLAG_USES_SKELETON and carry out initialization Kinect, when people is in standing state, Kinect can get the position of 20 articulation points of people to represent the skeleton of people; Extract the cylinder model that 15 articulation points build operator in virtual scene, these 15 articulation points sort from top to bottom and from left to right and are: 1. head; ②Jian center; 3. right shoulder; 4. right hand elbow; 5. the right hand; 6. left shoulder; 7. left hand elbow; 8. left hand; 9. hip joint center; 10. right hip; right knee; right crus of diaphragm; left hip; left knee; left foot; The position of these articulation points is all the position relative to Kinect coordinate system;

Being the coordinate system of model with Kinect coordinate when building operator's cylinder model, by the process of Kinect to depth image, obtaining the position of 15 articulation points of operator adopt right cylinder to carry out modeling to articulation point adjacent in human skeleton, the cylindrical upper bottom surface center of circle is the position of two joint point, and cylinder radius adjusts according to actual conditions;

3) in real scene and virtual scene, the demarcation of robot and operator is corresponding, as follows:

In above-mentioned two steps, be the coordinate system of model with robot basis coordinates to the modeling of robot in virtual scene, be the coordinate system of model with Kinect coordinate to the modeling of operator in virtual scene, in order to virtual scene and real scene are carried out correspondence, choose a coordinate system in real scene, be called world coordinate system E1, basis coordinates system of robot is called coordinate system E2, and Kinect coordinate system is called coordinate system E3;

Relation between robot basis coordinates system E2 and world coordinate system E1 rotation matrix R and translation matrix T represent, if the coordinate of certain articulation point P of robot under basis coordinates system of robot is coordinate under world coordinate system is so the pass between them is:

$\left(\begin{array}{c}{X}_p^{E1}\\ Y_p^{E1}\\ Z_p^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E2}}_{E1}& {T_{E2}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)=M_2\·\left(\begin{array}{c}{X}_p^{E2}\\ Y_p^{E2}\\ Z_p^{E2}\\ 1\end{array}\right)---\left(4\right)$

In formula, ^e2r _e1for 3X3 matrix, represent the attitudes vibration matrix of basis coordinates system of robot relative to world coordinate system; ^e2t _e1for 3X1 matrix, represent the change in location matrix of basis coordinates system of robot relative to world coordinate system; M ₂for 4X4 matrix, represent the pose transformation matrices of basis coordinates system of robot relative to world coordinate system;

In like manner, the relation between Kinect coordinate system E3 and world coordinate system E1 also represents by rotation matrix R and translation matrix T, if the coordinate of certain articulation point P ' of operator under Kinect coordinate system is coordinate under world coordinate system is pass then between them is:

$\left(\begin{array}{c}{X}_{p^{\′}}^{E1}\\ Y_{p^{\′}}^{E1}\\ Z_{p^{\′}}^{E1}\\ 1\end{array}\right)=\left(\begin{array}{cc}{R_{E3}}_{E1}& {T_{E3}}_{E1}\\ 0& 1\end{array}\right)\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)=M_3\·\left(\begin{array}{c}{X}_{p^{\′}}^{E3}\\ Y_{p^{\′}}^{E3}\\ Z_{p^{\′}}^{E3}\\ 1\end{array}\right)---\left(5\right)$

After basis coordinates system of robot and Kinect coordinate system are all transformed into world coordinate system, be next exactly the mapping relations determining real scene and virtual scene and the error weighed between them; If certain 1 P in virtual scene _vcoordinate be then this P _rcoordinate in real scene RGB image is then there is one and map f, make to put P arbitrarily in virtual scene _vand the some P in corresponding real scene _r, there is following relation:

P _r＝f(P _v)+ε(6)

In formula, ε is the error that virtual scene and real scene are corresponding;

Kinect is obtained to bone three-dimensional data and the coloured image 2-D data of operator, KinectSDK provides mutual conversion between the two, namely provides the mapping f of virtual scene operator and real scene operator _person; NuiTransformSkeletonToDepthImage provides the mapping relationship f of three-dimensional skeleton data to two-dimensional depth image ₁, NuiImageGetColorPixelCoordinatesFromDepthPixel provides the mapping f of two-dimensional depth image to Two-dimensional Color Image ₂, the mapping relations of three-dimensional skeleton data to Two-dimensional Color Image can be learnt thus:

f _person＝f ₂·f ₁(7)

So far, the operator of operator's real scene of virtual scene achieves correspondence, for robot, because basis coordinates system of robot and Kinect coordinate system are transformed into world coordinate system all, then robot and the virtual scene residing for operator are the same with the coordinate system of real scene, then the mapping relationship f of virtual scene robot and real scene robot _robotwith the mapping relationship f of virtual scene operator and real scene operator _personthe same:

f _robot＝f _person(8)

Then for whole virtual scene and real scene, the formula of the mapping relationship f between them is:

f＝f _robot＝f _person＝f ₂·f ₁(9)

In conjunction with formula (6) and formula (9) above, the error ε of virtual scene and real scene can be weighed:

ε＝P _r-f ₂·f ₁(P _v)(10)

4) real time collision detection of robot model and operator's model is as follows:

When operator enters in the operating environment of robot, the information of operator has unpredictability for robot, directly can have influence on efficiency and the precision of collision detection to the modeling strategy of operator and robot; Adopt more regular geometric configuration right cylinder to carry out modeling to robot and operator per capita to operator and machine, the collision detection concrete scheme based on right cylinder bounding box is as follows:

If A, B be a cylinder on to go to the bottom the round heart, C, D be another cylinder on to go to the bottom the round heart, space line l _aB, l _cDbe the axis of two cylinders, the main situation considering two straight line antarafacials, asks for two different surface beeline l _aB, l _cDcommon vertical line and intersection point P, Q, if P or Q be not in line segment AB or line segment CD, then carry out Boundary Detection, the end points chosen near other straight line substitutes P or Q point;

If P, Q are all in line segment AB and line segment CD, then two cylinders can only be that side is intersected, and now only need to judge the length of line segment PQ and the radius sum of two cylinders;

If there is a point to drop in line segment in P, Q, a point drops on end points, then two cylinders can only side and end face intersect, and now finds the bus near end face on side, judges that whether bus is crossing with end face;

If P, Q are all on the end points of line segment, then two cylinders can only be that end face intersects, and now only need to judge spatially whether two discs intersect.