CN103112007A - Human-machine interaction method based on mixing sensor - Google Patents

Abstract

The invention provides a human-machine interaction method based on a mixing sensor. The human-machine interaction method based on the mixing sensor comprises steps as below: obtaining and processing red green blue (RGB) images and depth images during an operation task of a robot through hand movement of an operator in real time, and obtaining the position of the hand of the operator; using an inertia measurement device to measure gesture of the hand of the operator and evaluating the gesture data together with a quaternion algorithm (FQA) and Kalman filtering (KFs) to obtain stable gesture of the hand of the operator; driving a robot according to the gesture of the hand of the operator by the mentioned steps. The human-machine interaction method based on the mixing sensor allows an operator to control the robot through three-dimensional hand movements, allows the operator to use the action of hands and arms to control the robot without limits of a physical device, and enables the operator to simply concentrate on the task at hand without dividing the task into limited commands.

Description

Man-machine interaction method based on hybrid sensor

Technical field

The invention belongs to the robot motion field, particularly a kind of man-machine interaction method based on hybrid sensor.

Background technology

When the mankind are not suitable for appearing at robot site, it is very necessary utilizing the operator to handle tele-robotic in the destructuring dynamic environment.For reaching the object run task, the machinery that distant operation is used and other contact interfaces need factitious mankind's action, and they tend to hinder human action.Existing based on vision man-machine exchanged form otherwise only use the minority free degree of staff, or need the operator to do Large Amplitude Motion, very effort operates.This piece invention has proposed a kind of discontiguous teleoperation of robot method based on multisensor, and it allows the operator to come the complete operation task by the staff three-dimensional motion control six degree of freedom virtual robot of nature.Man-machine interface based on multisensor helps the operator end user manually to make control and understands robot motion and surrounding environment.

The device that the normally used distant formula man-machine interface of operating machine has comprised that multiple operator wears, such as the skeleton-type mechanical device, tool glove, electromagnetism or inertia motion tracking sensor, perhaps electromyographic activity sensor.Yet the device that these are worn, sensor also have their line may hinder the operator to move flexibly.The interface of other contacts is such as imitative robot mechanical arm, rotating disk, and rocking bar, computer mouse and computer graphical interface etc. all need the factitious action of operator or need the operator to learn how to operate.Used the control mode of gesture identification and speech recognition combination based on the interaction technique of vision, it allows the operator more naturally to provide order intuitively and there is no restriction on physical unit.Yet under this control, the action of controlled robot all is comprised of some simple orders, such as position and the orientation of determining end effector, crawl also has a series of less tasks as above to move to move down, rotation suspends, and continues the pattern that also changes and operate.When the complexity of the robotic manipulator that requires action was higher, this technology all can be difficult to operation.The hand motion of single camera is followed the tracks of and has been used in the simulation of teleoperation of robot and distant operation.Yet action command only is confined in the subset of the real end effector free degree.In addition, the gesture that those and specific operation task have nothing to do is used to change the operator scheme of robot, so these have all caused the interchange of operation task natural.Ideally, a kind ofly natural need not be limited instruction with the Task-decomposing of complexity and do not have the method for contact physical unit restriction required by people.

Summary of the invention

This invention has proposed a kind of man-machine interaction method based on hybrid sensor, allows the operator to come control by the hand motion of three-dimensional.This method has been used contactless based on the man-machine interface based on hybrid sensor, and it can feed back in addition by operator's action control robot the mutual situation of robot motion and robot and surrounding environment object, and concrete technical scheme is as follows.

Based on the man-machine interaction method of hybrid sensor, it comprises the steps:

S1, Real-time Obtaining are also processed the operator and are carried out RGB image and depth image in robot manipulation's task process by hand exercise, obtain operator's hand position;

S2, use inertia measurement instrument are measured operator's hand attitude, and in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), attitude data are estimated, thereby obtain stable operator's hand attitude;

S3, the operator's hand pose driven machine people who obtains according to above-mentioned steps.

Further, described step S1 comprises the following steps:

When the operator carries out robot manipulation's task by hand exercise, catch RGB image and depth image by the Kinect that is fixed on the operator front, RGB image and the depth image processed continuously in this process are realized hand tracking and location; Kinect has three kinds of automatic focusing camera heads: optimize the infrared camera of depth detection and the vision spectrum camera that is used for visual identity of a standard for two;

Extract the human skeleton artis from depth image; 15 artis of this that extracts as shown in Figure 1, from top to bottom and from left to right sequence is: (1) head; (2) shoulder center; (3) right shoulder; (4) right hand elbow; (5) right finesse; (6) left shoulder; (7) left hand elbow; (8) left finesse; (9) hip joint center; (10) right hip; (11) right knee; (12) right crus of diaphragm; (13) left hip; (14) left knee; (15) left foot; The coordinate of these 15 artis is called as the Kinect coordinate; As shown in Figure 2, the position of operator's right hand may be defined as right finesse P ₅The position, but so directly utilize P ₅The positional information impact that comes control may cause the position of robot to be subjected to operator's health to move or tremble; In order to address this problem, with shoulder joint P ₃As a reference point and build reference point

Right hand framework;

With respect to reference point

P ₅Position P ' ₅For:

P′ ₅=P ₅-P ₃ (1)

The definition right hand with respect to the moving range of right shoulder is $W_{\mathrm{RH}}^1<x<W_{\mathrm{RH}}^2,$ $H_{\mathrm{RH}}^1<y<H_{\mathrm{RH}}^2,$ $L_{\mathrm{RH}}^1<z<L_{\mathrm{RH}}^2,$ And the moving range of robot is $W_{\mathrm{RR}}^1<x<W_{\mathrm{RR}}^2,$ $H_{\mathrm{RR}}^1<y<H_{\mathrm{RR}}^2,$ $L_{\mathrm{RR}}^1<z<L_{\mathrm{RR}}^2,$ The mapping point of robot moving range is:

$\left\{\begin{array}{c}{R}_x^R=\frac{({\mathrm{<figure-callout\; id="8"\; label="P"\; filenames="130206100657.png"\; state="\{\{state\}\}">P</figure-callout>}}_{8\_x}^{\&prime;}-W_{\mathrm{RH}}^1)}{(W_{\mathrm{RH}}^2-W_{\mathrm{RH}}^1)}\&CenterDot;(W_{\mathrm{RR}}^2-W_{\mathrm{RR}}^1)+W_{\mathrm{RR}}^1\\ R_y^R=\frac{({\mathrm{<figure-callout\; id="8"\; label="P"\; filenames="130206100657.png"\; state="\{\{state\}\}">P</figure-callout>}}_{8\_y}^{\&prime;}-H_{\mathrm{RH}}^1)}{(H_{\mathrm{RH}}^2-H_{\mathrm{RH}}^1)}\&CenterDot;H_{\mathrm{RR}}^2-H_{\mathrm{RR}}^1+H_{\mathrm{RR}}^1\\ R_z^R=\frac{({\mathrm{<figure-callout\; id="8"\; label="P"\; filenames="130206100657.png"\; state="\{\{state\}\}">P</figure-callout>}}_{8\_z}^{\&prime;}-L_{\mathrm{RH}}^1)}{(L_{\mathrm{RH}}^2-L_{\mathrm{RH}}^1)}\&CenterDot;(L_{\mathrm{RR}}^2-L_{\mathrm{RR}}^1)+L_{\mathrm{RR}}^1\end{array}\right.---\left(2\right)$

Described step S2 comprises the following steps:

When the operator carried out robot manipulation's task by hand exercise, the right hand was held an inertia measurement instrument, and this inertia measurement instrument can be determined rolling, pitching and the deflection attitude of operator's right hand; The inertia measurement instrument is by a triaxial accelerometer, and two two axis gyroscope instrument and three axle magnetometers form;

Define three coordinate systems: operator's body coordinate system x _by _bz _b, inertia measurement instrument coordinate system x _sy _sz _sWith terrestrial coordinate system x _ey _ez _eInertia measurement instrument coordinate system x _sy _sz _sThe accelerometer that corresponding three quadratures are installed and the axle of magnetometer; Because the inertia measurement instrument is arranged on the robot end rigidly, suppose operator's body coordinate system x _by _bz _bWith inertia measurement instrument coordinate system x _sy _sz _sIn conjunction with; Earth fixed coordinate system x _ey _ez _eFollow north-Dong-under (NED) pact, wherein x _eEnergized north, y _ePoint to east and z _eUnder sensing; The inertia measurement instrument can be measured rolling, pitching and the deflection attitude of himself; Definition is around x _eRotation φ represent to roll, around y _eThe rotation θ of axle represents pitching and around z _eThe rotation ψ of axle represents deflection; According to Euler's rotation theorem, from Eulerian angles φ, θ and ψ being converted to hypercomplex number q:

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \sin (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)-\cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\mathrm{cso}(\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)-\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)\end{array}\right]---\left(3\right)$

And being constrained to of four Euler Parameter:

$q_0^2+q_1^2+q_2^2+q_3^2=1---\left(4\right)$

Here q ₀Scalar part and (q ₁, q ₂, q ₃) be the vector part; So be tied to the attitude cosine matrix of earth fixed coordinate system from inertia measurement instrument coordinate Can be represented as:

$M_s^e=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2-q_3^2\end{array}\right]---\left(5\right);$

Because gyroscope and magnetometer have white noise and random walk, in order to obtain stable data, utilize Kalman filtering to estimate the state of inertia measurement instrument from one group of noisy and incomplete measurement; Inertia measurement instrument state x from moment k-1 _k-1Estimate the inertia measurement instrument state x of k constantly _kTransfer equation be:

x _k=A _k·x _k-1+B·u _k-1+w _k-1 (6)

z _k=H·x _k+v _k

Here x _kBe the state vector of n * 1, represent the inertia measurement instrument state of k constantly; A is the state transition matrix of n * n; B is system's input matrix of n * p; u _k-1Be the k-1 moment certainty input vector of p * 1, p is the number of certainty input; w _k-1It is the k-1 n in the moment * 1 process noise vector; z _kThat vector is measured in the k m in the moment * 1; H is the observation matrix of m * n and v _kIt is the measurement noise vector of m * 1; N=7 herein, m=6; With the quaternion differential equation of time correlation be:

$\left[\begin{array}{c}{\&PartialD;q}_0/\&PartialD;t\\ {\&PartialD;q}_1/\&PartialD;t\\ {\&PartialD;q}_2/\&PartialD;t\\ {\&PartialD;q}_3/\&PartialD;t\end{array}\right]=\left[\begin{array}{cccc}{q}_0& {-q}_1& {-q}_2& {-q}_3\\ q_1& q_0& {-q}_3& q_2\\ q_2& q_3& q_0& {-q}_1\\ q_3& {-q}_2& q_1& q_0\end{array}\right]\&CenterDot;\begin{array}{c}\left[\begin{array}{c}0\\ v_x/2\\ v_y/2\\ v_z/2\end{array}\right]---\left(7\right)\end{array}$

V wherein _x, v _y, v _zThat the inertia measurement instrument is at axle x _s, y _s, z _sAngular speed; Due to state x _kComprise hypercomplex number state and angular speed, so x _kFollowing form is arranged:

x _k=[q _0,kq _1,kq _2,kq _3,kv _x,kv _y,kv _z,k] (8)

Here q ₀, _k, q ₁, _k, q ₂, _k, q ₃, _k, v _x, _k, v _y, _k, v _z, _kBe k hypercomplex number state and angular speed constantly, can get from equation (7), the state transfer equation is:

$A_k=\left[\begin{array}{ccccccc}1& 0& 0& 0& -q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 1& 0& 0& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 1& 0& q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 0& 1& -q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(9\right)$

Here Δ t is sample time, owing to there is no control inputs, allows B=0 ^{N * p}, Bu _k-1=0 ^{N * 1}Estimate the hypercomplex number state with angular speed, obtain the process noise vector and be

w _k=[0000 w _x w _y w _z] ^T (10)

Here w _x, w _y, w _zIt is the process noise of angular speed; Owing to coming measured angular speed with gyroscope, observation matrix H is

H=[0 ^3×4I ^3×3] (11)

In order to satisfy (4), definition

K certainty hypercomplex number q constantly _kCan be standardized as:

q _k=[q _0,k/D q _1,k/D q _2,k/D q _3,k/D] (12)

Arrive formula (12) in conjunction with formula (6), can be from the inertia measurement instrument state x of moment k-1 _k-1Estimate the inertia measurement instrument state x of k constantly _k

Described step S3 comprises the following steps:

Utilize anti-solution of end pose of robot, then each joint is by anti-angular movement of separating, when the hand of the complete virtual robot of whole joint motions is grabbed the position that end just arrives appointment.There is no the situation of barrier around this situation is suitable for, the virtual robot arm motion can not bump with the surrounding environment barrier.

And in the Denavit-Hartenberg representation, A _iThe homogeneous coordinate transformation matrix of expression from coordinate system i-1 to coordinate system i usually has:

$A_i=\left[\begin{array}{cccc}{\cos \mathrm{\&theta;}}_i& {-\sin }_i{\cos \mathrm{\&alpha;}}_i& {\sin \mathrm{\&theta;}}_i{\sin \mathrm{\&alpha;}}_i& l_i{\cos \mathrm{\&theta;}}_i\\ {\sin \mathrm{\&theta;}}_i& {\cos \mathrm{\&theta;}}_i{\cos \mathrm{\&alpha;}}_i& {-\cos \mathrm{\&theta;}}_i{\sin \mathrm{\&alpha;}}_i& l_i{\sin \mathrm{\&theta;}}_i\\ 0& {\sin \mathrm{\&alpha;}}_i& {\cos \mathrm{\&alpha;}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]---\left(13\right)$

For a robot with six joints, definition support coordinate is coordinate system 1, and last coordinate is coordinate system 6, the homogeneous transformation matrix T from the support frame of axes to last frame of axes ₆Be defined as:

$T_6=A_1{A}_2{...A}_6=\left[\begin{array}{cccc}{n}_6^0& s_6^0& a_6^0& p_6^0\\ 0& 0& 0& 1\end{array}\right]---\left(14\right)$

In formula

Be the normal vector of paw,

Be sliding vector,

For near vector,

Be position vector.

Utilize (14), (5) have:

T ₆=M (15)

Obtain six joint motions angle values by finding the solution (15): (θ ₁, θ ₂... θ ₆).

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

The present invention proposes a kind of man-machine interaction method based on hybrid sensor, allow the operator to come control by the hand motion of three-dimensional.This interface based on hybrid sensor allows the operator to use the action control robot of hand and arm and there is no the restriction of physical unit, only need to also need not all be decomposed into those limited instructions to the task that energy concentrates at hand to task.

Description of drawings

Fig. 1 is the skeleton joint point diagram;

Fig. 2 is right hand joint point diagram;

Fig. 3 is based on the man-machine interaction flow chart of hybrid sensor.

The specific embodiment

The present invention is described in further detail below in conjunction with embodiment, but embodiments of the present invention are not limited to this.

Embodiment:

The present invention is based on hybrid sensor carries out man-machine interaction method and comprises the steps:

S1, Real-time Obtaining are also processed the operator and are carried out RGB image and depth image in robot manipulation's task process by hand exercise, obtain operator's hand position;

S2, use inertia measurement instrument are measured operator's hand attitude, and in conjunction with Quaternion Algorithm (FQA) and Kalman filtering (KFs), attitude data are estimated, thereby obtain stable operator's hand attitude;

S3, the operator's hand pose driven machine people who obtains according to above-mentioned steps.

Described step S1 comprises the following steps:

When the operator carries out robot manipulation's task by hand exercise, catch RGB image and depth image by the Kinect that is fixed on the operator front, RGB image and the depth image processed continuously in this process are realized hand tracking and location; Kinect has three kinds of automatic focusing camera heads: optimize the infrared camera of depth detection and the vision spectrum camera that is used for visual identity of a standard for two;

Extract the human skeleton artis from depth image; 15 artis of this that extracts as shown in Figure 1, from top to bottom and from left to right sequence is: (1) head; (2) shoulder center; (3) right shoulder; (4) right hand elbow; (5) right finesse; (6) left shoulder; (7) left hand elbow; (8) left finesse; (9) hip joint center; (10) right hip; (11) right knee; (12) right crus of diaphragm; (13) left hip; (14) left knee; (15) left foot; The coordinate of these 15 artis is called as the Kinect coordinate; As shown in Figure 2, the position of operator's right hand may be defined as right finesse P ₅The position, but so directly utilize P ₅The positional information impact that comes control may cause the position of robot to be subjected to operator's health to move or tremble; In order to address this problem, with shoulder joint P ₃As a reference point and build reference point

Right hand framework;

With respect to reference point

P ₅Position P ' ₅For:

P ₅=P ₅-P ₃ (1)

The definition right hand with respect to the moving range of right shoulder is $w_{\mathrm{Rh}}^1<x<w_{\mathrm{RH}}^2,$ $H_{\mathrm{RH}}^1<y<H_{\mathrm{RH}}^2,$ $L_{\mathrm{RH}}^1<z<L_{\mathrm{RH}}^2,$ And the moving range of robot is $W_{\mathrm{RR}}^1<x<W_{\mathrm{RR}}^2,$ $H_{\mathrm{RR}}^1<y<H_{\mathrm{RR}}^2,$ $L_{\mathrm{RR}}^1<z<L_{\mathrm{RR}}^2,$ The mapping point of robot moving range is:

$\left\{\begin{array}{c}{R}_x^R=\frac{({\mathrm{<figure-callout\; id="8"\; label="P"\; filenames="130206100657.png"\; state="\{\{state\}\}">P</figure-callout>}}_{8\_x}^{\&prime;}-W_{\mathrm{RH}}^1)}{(W_{\mathrm{RH}}^2-W_{\mathrm{RH}}^1)}\&CenterDot;(W_{\mathrm{RR}}^2-W_{\mathrm{RR}}^1)+W_{\mathrm{RR}}^1\\ R_y^R=\frac{({\mathrm{<figure-callout\; id="8"\; label="P"\; filenames="130206100657.png"\; state="\{\{state\}\}">P</figure-callout>}}_{8\_y}^{\&prime;}-H_{\mathrm{RH}}^1)}{(H_{\mathrm{RH}}^2-H_{\mathrm{RH}}^1)}\&CenterDot;H_{\mathrm{RR}}^2-H_{\mathrm{RR}}^1+H_{\mathrm{RR}}^1\\ R_z^R=\frac{({\mathrm{<figure-callout\; id="8"\; label="P"\; filenames="130206100657.png"\; state="\{\{state\}\}">P</figure-callout>}}_{8\_z}^{\&prime;}-L_{\mathrm{RH}}^1)}{(L_{\mathrm{RH}}^2-L_{\mathrm{RH}}^1)}\&CenterDot;(L_{\mathrm{RR}}^2-L_{\mathrm{RR}}^1)+L_{\mathrm{RR}}^1\end{array}\right.---\left(2\right)$

Described step S2 comprises the following steps:

When the operator carried out robot manipulation's task by hand exercise, the right hand was held an inertia measurement instrument, and this inertia measurement instrument can be determined rolling, pitching and the deflection attitude of operator's right hand; The inertia measurement instrument is by a triaxial accelerometer, and two two axis gyroscope instrument and three axle magnetometers form;

Define three coordinate systems: operator's body coordinate system x _by _bz _b, inertia measurement instrument coordinate system x _sy _sz _sWith terrestrial coordinate system x _ey _ez _eInertia measurement instrument coordinate system x _sy _sz _sThe accelerometer that corresponding three quadratures are installed and the axle of magnetometer; Because the inertia measurement instrument is arranged on the robot end rigidly, suppose operator's body coordinate system x _by _bz _bWith inertia measurement instrument coordinate system x _sy _sz _sIn conjunction with; Earth fixed coordinate system x _ey _ez _eFollow north-Dong-under (NED) pact, wherein x _eEnergized north, y _ePoint to east and z _eUnder sensing; The inertia measurement instrument can be measured rolling, pitching and the deflection attitude of himself; Definition is around x _eRotation φ represent to roll, around y _eThe rotation θ of axle represents pitching and around z _eThe rotation ψ of axle represents deflection; According to Euler's rotation theorem, from Eulerian angles φ, θ and ψ being converted to hypercomplex number q:

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \sin (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)-\cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\mathrm{cso}(\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)-\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)\end{array}\right]---\left(3\right)$

And being constrained to of four Euler Parameter:

$q_0^2+q_1^2+q_2^2+q_3^2=1---\left(4\right)$

Here q ₀Scalar part and (q ₁, q ₂, q ₃) be the vector part; So be tied to the attitude cosine matrix of earth fixed coordinate system from inertia measurement instrument coordinate

Can be represented as:

$M_s^e=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2-q_3^2\end{array}\right]---\left(5\right);$

Because gyroscope and magnetometer have white noise and random walk, in order to obtain stable data, utilize Kalman filtering to estimate the state of inertia measurement instrument from one group of noisy and incomplete measurement; Inertia measurement instrument state x from moment k-1 _k-1Estimate the inertia measurement instrument state x of k constantly _kTransfer equation be:

x _k=A _k·x _k-1+B·u _k-1+w _k-1 (6)

z _k=H·x _k+v _k

Here x _kBe the state vector of n * 1, represent the inertia measurement instrument state of k constantly; A is the state transition matrix of n * n; B is system's input matrix of n * p; u _k-1Be the k-1 moment certainty input vector of p * 1, p is the number of certainty input; w _k-1It is the k-1 n in the moment * 1 process noise vector; z _kThat vector is measured in the k m in the moment * 1; H is the observation matrix of m * n and v _kIt is the measurement noise vector of m * 1; N=7 herein, m=6; With the quaternion differential equation of time correlation be:

$\left[\begin{array}{c}{\&PartialD;q}_0/\&PartialD;t\\ {\&PartialD;q}_1/\&PartialD;t\\ {\&PartialD;q}_2/\&PartialD;t\\ {\&PartialD;q}_3/\&PartialD;t\end{array}\right]=\left[\begin{array}{cccc}{q}_0& {-q}_1& {-q}_2& {-q}_3\\ q_1& q_0& {-q}_3& q_2\\ q_2& q_3& q_0& {-q}_1\\ q_3& {-q}_2& q_1& q_0\end{array}\right]\&CenterDot;\begin{array}{c}\left[\begin{array}{c}0\\ v_x/2\\ v_y/2\\ v_z/2\end{array}\right]---\left(7\right)\end{array}$

V wherein _x, v _y, v _zThat the inertia measurement instrument is at axle x _s, y _s, z _sAngular speed; Due to state x _kComprise hypercomplex number state and angular speed, so x _kFollowing form is arranged:

x _k=[q _0,kq _1，kq _2,kq _3，kv _x,kv _y，kv _z,k] (8)

Here q _{0, k}, q _{1, k}, q _{2, k}, q _{3, k}, v _{X, k}, v _{Y, k}, v _z,kBe k hypercomplex number state and angular speed constantly, can get from equation (7), the state transfer equation is:

$A_k=\left[\begin{array}{ccccccc}1& 0& 0& 0& -q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 1& 0& 0& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 1& 0& q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 0& 1& -q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(9\right)$

Here Δ t is sample time, owing to there is no control inputs, allows B=0 ^{N * p}, Bu _k-1=0 ^{N * 1}Estimate the hypercomplex number state with angular speed, obtain the process noise vector and be

w _k=[0000w _x w _y w _z] ^T (10)

Here w _x, w _y, w _zIt is the process noise of angular speed; Owing to coming measured angular speed with gyroscope, observation matrix H is

H=[0 ^3×4 I ^3×3] (11)

In order to satisfy (4), definition

K certainty hypercomplex number q constantly _kCan be standardized as:

q _k=[q _0，k/D q _1,k/D q _2,k/D q _3,k/D] (12)

Arrive formula (12) in conjunction with formula (6), can be from the inertia measurement instrument state x of moment k-1 _k-1Estimate the inertia measurement instrument state x of k constantly _k

Described step S3 comprises the following steps:

Utilize anti-solution of end pose of robot, then each joint is by anti-angular movement of separating, when the hand of the complete virtual robot of whole joint motions is grabbed the position that end just arrives appointment.There is no the situation of barrier around this situation is suitable for, the virtual robot arm motion can not bump with the surrounding environment barrier.

And in the Denavit-Hartenberg representation, A _iThe homogeneous coordinate transformation matrix of expression from coordinate system i-1 to coordinate system i usually has:

$A_i=\left[\begin{array}{cccc}{\cos \mathrm{\&theta;}}_i& {-\sin }_i{\cos \mathrm{\&alpha;}}_i& {\sin \mathrm{\&theta;}}_i{\sin \mathrm{\&alpha;}}_i& l_i{\cos \mathrm{\&theta;}}_i\\ {\sin \mathrm{\&theta;}}_i& {\cos \mathrm{\&theta;}}_i{\cos \mathrm{\&alpha;}}_i& {-\cos \mathrm{\&theta;}}_i{\sin \mathrm{\&alpha;}}_i& l_i{\sin \mathrm{\&theta;}}_i\\ 0& {\sin \mathrm{\&alpha;}}_i& {\cos \mathrm{\&alpha;}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]---\left(13\right)$

For a robot with six joints, definition support coordinate is coordinate system 1, and last coordinate is coordinate system 6, the homogeneous transformation matrix T from the support frame of axes to last frame of axes ₆Be defined as:

$T_6=A_1{A}_2...A_6=\left[\begin{array}{cccc}{n}_6^0& s_6^0& a_6^0& p_6^0\\ 0& 0& 0& 1\end{array}\right]---\left(14\right)$

In formula Be the normal vector of paw, Be sliding vector,

For near vector,

Be position vector.

Utilize (14), (5) have:

T ₆=M (15)

Obtain six joint motions angle values by finding the solution (15): (θ ₁, θ ₂..., θ ₆).

Above-described embodiment is the better embodiment of the present invention; but embodiments of the present invention are not restricted to the described embodiments; other any do not deviate from change, the modification done under Spirit Essence of the present invention and principle, substitutes, combination, simplify; all should be the substitute mode of equivalence, within being included in protection scope of the present invention.

Claims (4)

1. based on the man-machine interaction method of hybrid sensor, it is characterized in that, comprise the following steps:

S1, Real-time Obtaining are also processed the operator and are carried out RGB image and depth image in robot manipulation's task process by hand exercise, obtain operator's hand position;

S2, use inertia measurement instrument are measured operator's hand attitude, and in conjunction with Quaternion Algorithm and Kalman filtering, attitude data are estimated, thereby obtain stable operator's hand attitude;

S3, the operator's hand pose driven machine people who obtains according to above-mentioned steps.

2. the man-machine interaction method based on hybrid sensor according to claim 1, is characterized in that, described step S1 comprises:

When the operator carries out robot manipulation's task by hand exercise, catch RGB image and depth image by the Kinect that is fixed on the operator front, RGB image and the depth image processed continuously in this process are realized hand tracking and location; Kinect has three kinds of automatic focusing camera heads: optimize the infrared camera of depth detection and the vision spectrum camera that is used for visual identity of a standard for two;

Extract the human skeleton artis from depth image; 15 artis of this that extracts sort from top to bottom and from left to right: (1) head; (2) shoulder center; (3) right shoulder; (4) right hand elbow; (5) right finesse; (6) left shoulder; (7) left hand elbow; (8) left finesse; (9) hip joint center; (10) right hip; (11) right knee; (12) right crus of diaphragm; (13) left hip; (14) left knee; (15) left foot; The coordinate of these 15 artis is called as the Kinect coordinate; The position of operator's right hand is defined as right finesse P ₅The position, with shoulder joint P ₃As a reference point and build reference point

Right hand framework;

With respect to reference point

P ₅Position P ' ₅For:

P′ ₅=P ₅-P ₃ (1)

The setting right hand with respect to the moving range of right shoulder is $W_{\mathrm{RH}}^1<x<W_{\mathrm{RH}}^2,$ $H_{\mathrm{RH}}^1<y<H_{\mathrm{RH}}^2,$ $L_{\mathrm{RH}}^1<z<L_{\mathrm{RH}}^2,$ And the moving range of robot is $W_{\mathrm{RR}}^1<x<W_{\mathrm{RR}}^2,$ $H_{\mathrm{RR}}^1<y<H_{\mathrm{RR}}^2,$ $L_{\mathrm{RR}}^1<z<L_{\mathrm{RR}}^2,$ The mapping point of robot moving range is:

$\left\{\begin{array}{c}{R}_x^R=\frac{(P_{8\_x}^{\&prime;}-W_{\mathrm{RH}}^1)}{(W_{\mathrm{RH}}^2-W_{\mathrm{RH}}^1)}\&CenterDot;(W_{\mathrm{RR}}^2-W_{\mathrm{RR}}^1)+W_{\mathrm{RR}}^1\\ R_y^R=\frac{(P_{8\_y}^{\&prime;}-H_{\mathrm{RH}}^1)}{(H_{\mathrm{RH}}^2-H_{\mathrm{RH}}^1)}\&CenterDot;H_{\mathrm{RR}}^2-H_{\mathrm{RR}}^1+H_{\mathrm{RR}}^1\\ R_z^R=\frac{(P_{8\_z}^{\&prime;}-L_{\mathrm{RH}}^1)}{(L_{\mathrm{RH}}^2-L_{\mathrm{RH}}^1)}\&CenterDot;(L_{\mathrm{RR}}^2-L_{\mathrm{RR}}^1)+L_{\mathrm{RR}}^1\end{array}\right..---\left(2\right)$

3. the man-machine interaction method based on hybrid sensor according to claim 1, is characterized in that, described step S2 comprises the following steps:

When the operator carried out robot manipulation's task by hand exercise, the right hand was held an inertia measurement instrument, and this inertia measurement instrument can be determined rolling, pitching and the deflection attitude of operator's right hand; The inertia measurement instrument is by a triaxial accelerometer, and two two axis gyroscope instrument and three axle magnetometers form;

Define three coordinate systems: operator's body coordinate system x _by _bz _b, inertia measurement instrument coordinate system x _sy _sz _sWith terrestrial coordinate system x _ey _ez _eInertia measurement instrument coordinate system x _sy _sz _sThe accelerometer that corresponding three quadratures are installed and the axle of magnetometer; Because the inertia measurement instrument is arranged on the robot end rigidly, suppose operator's body coordinate system x _by _bz _bWith inertia measurement instrument coordinate system x _sy _sz _sIn conjunction with; Earth fixed coordinate system x _ey _ez _eFollow north-Dong-under (NED) pact, wherein x _eEnergized north, y _ePoint to east and z _eUnder sensing; The inertia measurement instrument can be measured rolling, pitching and the deflection attitude of himself; Definition is around x _eRotation φ represent to roll, around y _eThe rotation θ of axle represents pitching and around z _eThe rotation ψ of axle represents deflection; According to Euler's rotation theorem, from Eulerian angles φ, θ and ψ being converted to hypercomplex number q:

$q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}\cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \sin (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)-\cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)+\sin (\mathrm{\&phi;}/2)\mathrm{cso}(\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)\\ \cos (\mathrm{\&phi;}/2)\cos (\mathrm{\&theta;}/2)\sin (\mathrm{\&psi;}/2)-\sin (\mathrm{\&phi;}/2)\sin (\mathrm{\&theta;}/2)\cos (\mathrm{\&psi;}/2)\end{array}\right]---\left(3\right)$

And being constrained to of four Euler Parameter:

$q_0^2+q_1^2+q_2^2+q_3^2=1---\left(4\right)$

Here q ₀Scalar part and (q ₁, q ₂, q ₃) be the vector part; So be tied to the attitude cosine matrix of earth fixed coordinate system from inertia measurement instrument coordinate

Can be represented as:

$M_s^e=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_0{q}_2+q_1{q}_3)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_0{q}_1+q_2{q}_3)& q_0^2-q_1^2-q_2^2-q_3^2\end{array}\right]---\left(5\right);$

Because gyroscope and magnetometer have white noise and random walk, in order to obtain stable data, utilize Kalman filtering to estimate the state of inertia measurement instrument from one group of noisy and incomplete measurement; Inertia measurement instrument state x from moment k-1 _k-1Estimate the inertia measurement instrument state x of k constantly _kTransfer equation be:

x _k=A _k·x _k-1+B·u _k-1+w _k-1 (6)

z _k=H·x _k+v _k

Here x _kBe the state vector of n * 1, represent the inertia measurement instrument state of k constantly; A is the state transition matrix of n * n; B is system's input matrix of n * p; u _k-1Be the k-1 moment certainty input vector of p * 1, p is the number of certainty input; w _k-1It is the k-1 n in the moment * 1 process noise vector; z _kThat vector is measured in the k m in the moment * 1; H is the observation matrix of m * n and v _kIt is the measurement noise vector of m * 1; N=7 herein, m=6; With the quaternion differential equation of time correlation be:

$\left[\begin{array}{c}{\&PartialD;q}_0/\&PartialD;t\\ {\&PartialD;q}_1/\&PartialD;t\\ {\&PartialD;q}_2/\&PartialD;t\\ {\&PartialD;q}_3/\&PartialD;t\end{array}\right]=\left[\begin{array}{cccc}{q}_0& {-q}_1& {-q}_2& {-q}_3\\ q_1& q_0& {-q}_3& q_2\\ q_2& q_3& q_0& {-q}_1\\ q_3& {-q}_2& q_1& q_0\end{array}\right]\&CenterDot;\begin{array}{c}\left[\begin{array}{c}0\\ v_x/2\\ v_y/2\\ v_z/2\end{array}\right]---\left(7\right)\end{array}$

V wherein _x, v _y, v _zThat the inertia measurement instrument is at axle x _s, y _s, z _sAngular speed; Due to state x _kComprise hypercomplex number state and angular speed, so x _kFollowing form is arranged:

x _k=[q _0,kq _1，kq _2,kq _3，kv _x,kv _y，kv _z,k] (8)

Q wherein _{0, k}, q _{1, k}, q _{2, k}, q _{3, k}, v _{X, k}, v _{Y, k}, v _z,kBe k hypercomplex number state and angular speed constantly, can get from equation (7), the state transfer equation is:

$A_k=\left[\begin{array}{ccccccc}1& 0& 0& 0& -q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 1& 0& 0& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 1& 0& q_{3,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2& -q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 0& 1& -q_{2,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{1,k}\&CenterDot;\mathrm{\&Delta;t}/2& q_{0,k}\&CenterDot;\mathrm{\&Delta;t}/2\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]---\left(9\right)$

Wherein Δ t is sample time, owing to there is no control inputs, allows B=0 ^{N * p}, Bu _k-1=0 ^{N * 1}Estimate the hypercomplex number state with angular speed, obtain the process noise vector and be

w _k=[0000w _xw _yw _z] ^T (10)

W wherein _x, w _y, w _zIt is the process noise of angular speed; Owing to coming measured angular speed with gyroscope, observation matrix H is

H=[0 ^3×4I ^3×3] (11)

In order to satisfy (4), definition

K certainty hypercomplex number q constantly _kCan be standardized as:

q _k=[q _0,k/D q _1,k/D q _2,k/D q _3,k/D] (12)

Arrive formula (12) in conjunction with formula (6), can be from the inertia measurement instrument state x of moment k-1 _k-1Estimate the inertia measurement instrument state x of k constantly _k

4. the man-machine interaction method based on hybrid sensor according to claim 1, is characterized in that, described step S3 comprises the following steps:

Utilize anti-solution of end pose of robot, then each joint is by anti-angular movement of separating, when the hand of the complete virtual robot of whole joint motions is grabbed the position that end just arrives appointment; There is no the situation of barrier around this situation is suitable for, the virtual robot arm motion can not bump with the surrounding environment barrier;

In the Denavit-Hartenberg representation, A _iThe homogeneous coordinate transformation matrix of expression from coordinate system i-1 to coordinate system i has:

$A_i=\left[\begin{array}{cccc}{\cos \mathrm{\&theta;}}_i& {-\sin }_i{\cos \mathrm{\&alpha;}}_i& {\sin \mathrm{\&theta;}}_i{\sin \mathrm{\&alpha;}}_i& l_i{\cos \mathrm{\&theta;}}_i\\ {\sin \mathrm{\&theta;}}_i& {\cos \mathrm{\&theta;}}_i{\cos \mathrm{\&alpha;}}_i& {-\cos \mathrm{\&theta;}}_i{\sin \mathrm{\&alpha;}}_i& l_i{\sin \mathrm{\&theta;}}_i\\ 0& {\sin \mathrm{\&alpha;}}_i& {\cos \mathrm{\&alpha;}}_i& r_i\\ 0& 0& 0& 1\end{array}\right]---\left(13\right)$

For a robot with six joints, definition support coordinate is coordinate system 1, and last coordinate is coordinate system 6, the homogeneous transformation matrix T from the support frame of axes to last frame of axes ₆Be defined as:

$T_6=A_1{A}_2...A_6=\left[\begin{array}{cccc}{n}_6^0& s_6^0& a_6^0& p_6^0\\ 0& 0& 0& 1\end{array}\right]---\left(14\right)$

In formula Be the normal vector of paw, Be sliding vector,

For near vector,

Be position vector.

Utilize (14), (5) have:

T ₆=M (15)

Obtain six joint motions angle values by finding the solution (15): (θ ₁, θ ₂..., θ ₆).

Images