CN105252532A - Method of cooperative flexible attitude control for motion capture robot - Google Patents

Abstract

The invention provides a method of cooperative flexible attitude control for a motion capture robot. The method comprises the following steps: capturing corresponding target joint displacement data of a target human body at different moments in real time by use of a body sensing apparatus Kinect; sending the target joint displacement data to a PC; drawing a human body skeleton frame by the PC in real time; dynamically displaying the human body skeleton frame on a screen by the PC and providing an error feedback; obtaining joint angle data; and controlling a steering engine of a humanoid robot NAO so that the humanoid robot to track the motions of the target human body in real time. According to the method, the displacement joint data of the target human body is captured by use of the Kinect so that various gestures of the human body can be identified, and therefore, real-time imitation of the human body motions by the robot is realized. The method is high in human body motion identification speed, high in accuracy, smooth in robot tracking motions, and capable of reflecting the gestures of the target human body in real time, and has good robustness under different environmental complex conditions.

Description

Motion capture robot works in coordination with the method for flexible gesture stability

Technical field

The present invention relates to a kind of method that motion capture robot works in coordination with flexible gesture stability, belong to field of human-computer interaction and artificial intelligence field.

Background technology

Man-machine interaction (HCI, Human-ComputerInteraction) be one about design, evaluate and realize interactive computer system for people to use, and study the subject of correlated phenomena occurred therefrom.Human-computer interaction technology mainly studies the information exchange between people and computer, be embodied in people to computer and computer to information interaction two parts of people, the former refers to that people can by equipment such as keyboard, mouse, action bars, position tracker, data glove, with the action of hand, pin, sound, attitude and health, sight line even brain wave etc. to computer transmission of information; The latter is that computer provides information by the output equipments such as printer, plotter, display, helmet-mounted display, audio amplifier or display device to people.

Kinect be Microsoft on the E3 great Zhan on June 2nd, 2009, the formal XBOX360 body sense periphery peripheral hardware announced.Kinect has thoroughly overturned the single operation of game, and the theory of human-computer interaction is shown more thoroughly.It is a kind of 3D body sense video camera, and it has imported the functions such as instant motion capture, image identification, microphone input, speech recognition, community interactive simultaneously.Body sense, i.e. somatesthesia are the general names of sense of touch, pressure sensation, the sense of heat, the pain sensation and proprioception (sensation about muscle and joint position and motion, body posture and motion and facial expression).The Kinect of LightCoding and Source calibration technology is adopted to utilize RF transmitter to send laser light, by each speckle in infrared C MOS camera record space, in conjunction with original speckle pattern, the image with the 3D degree of depth is calculated by wafer, and then be transformed into skeleton tracing system, make Kinect can be applied to many fields.

NAO is the anthropomorphic robot able to programme of the height 58 centimetres developed by AldebaranRobotics company.Its health has 25 frees degree, and main element is motor and electric actuator.Its motion module, based on generalized inverse kinematics, can process cartesian coordinate system, joint control, balance, redundancy and priority of task.NAO head embedded Intel ATOM1.6GHz processor, run LINUX kernel, have NAOqi operating system, be a software platform of increasing income, available C++ or Python are programmed.Based on above feature, robot NAO can realize motion tracking the most realistically, is important component part of the present invention.

Summary of the invention

For the deficiencies in the prior art, the present invention relates to a kind of method that motion capture robot works in coordination with flexible gesture stability.The object of the invention is to for man-machine interaction proposes a kind of new model.

Technical scheme of the present invention is as follows:

Motion capture robot works in coordination with a method for flexible gesture stability, comprises step as follows:

(1) body sense equipment Kinect is utilized to catch not the corresponding target joint displacement data of target body in the same time in real time; Target body stands in body sense equipment Kinect horizon range, is to ensure best identified effect, and target body should be in 1.2-3.5m before the camera lens of body sense equipment Kinect, within horizontal view angle ± 57 °;

(2) the target joint displacement data that body sense equipment Kinect captures is sent to PC end;

(3) PC termination receives all target joint displacement datas, and draws out skeleton framework in real time according to it;

(4) PC holds by skeleton framework Dynamic Announce on screen, and provides the feedback that reports an error;

(5) PC end processes all target joint displacement datas received: comprise relative threshold distance and compare filtering calculating, space vector calculating, obtain the gesture stability data of anthropomorphic robot NAO, i.e. joint angles data; The present invention, due to reasons such as environmental factor and sensor self shakes, causes there is interfering data in the initial data that obtains, is therefore necessary that carrying out relative threshold distance to initial data compares filtering and calculate, robot motion is followed the trail of more accurately, reliable;

(6) call the JointControl function in the NAOqi operating system of anthropomorphic robot NAO, according to the joint angles data sent, the steering wheel of anthropomorphic robot NAO is controlled, make the action of anthropomorphic robot NAO real-time tracking target body.

Preferred according to the present invention, described step (4) also comprises, and described PC holds Dynamic Announce skeleton framework: if keeping strokes of the skeleton framework of PC end display and target body, then joint angles data are sent to anthropomorphic robot NAO; If the skeleton framework of PC end display does not mate with the action of target body, then reinitialize program, receive reliable gesture stability data with guarantor's anthropomorphic robot NAO.

Preferred according to the present invention, in described step (5), described relative threshold distance compares filtering calculating and comprises:

By observing the space coordinates change of displacement artis, calculate same joint in set time section (such as: 0.1s-0.5s) vectorial in the fluctuation of time started and end time composition, observe this fluctuation vector field homoemorphism and the fluctuation in space coordinates all directions, by setting fluctuation threshold value screening joint undulating value.Can find out that the shake of artis recognizing site is mainly along the fast jitter of change in coordinate axis direction, and when there is shake, fluctuation vector field homoemorphism significantly can increase.Therefore, the larger artis of reply fluctuation does respective handling, keeps laststate constant, use different threshold values to compare for different joints to the artis of fluctuation within a narrow range, ensure that each filtered result is all optimal solution, to ensure the continuity that robot pose converts.

Preferred according to the present invention, in described step (5), described space vector calculates and comprises:

The space coordinates that Kinect uses are different from common space coordinates, and its x-axis is identical with Traditional Space coordinate system with the zero point of y-axis, but its z-axis co-ordinate zero point is Kinect sensor, and positive direction is the dead ahead that Kinect points to.Kinect space coordinates are as shown in Figure 6:

By in Kinect space coordinates, draw any two coordinate points P1 do not overlapped (x1, y1, z1) in Kinect coordinate system, P2 (x2, y2, z2), the vectorial P1P2 to its composition:

If there is another P3 and this point not on the straight line at P1P2 place, then there is following relational expression:

According to above-mentioned character, human synovial angle calculation is reduced to the calculating to space vector angle, the computational methods of upper limb joint angle will be described respectively below:

1) joint angles: LeftElbowRoll

As shown in Figure 7: calculate one group of vector that LeftElbowRoll angle only need construct this place, space angle both sides,

And obtain according to the joint angles computing formula above mentioned:

$LeftElbowRoll=\∠ABC=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-3)$

2) joint angles: LeftElbowYaw

As shown in Figure 8:

The angle in LeftElbowYaw joint is the angle that ancon produces when upper arm rotates, and this angle is the angle of ABC and BCD two intersecting plane under normal circumstances, i.e. the angle of figure midplane S1 and S2; Show that the computational methods of angle LeftElbowRoll are as follows according to space plane angle calcu-lation formula.

First the formula of the normal vector clearly calculating two non-colinear vector place planes is needed:

$\stackrel{\→}{m}=\stackrel{\→}{a}\×\stackrel{\→}{b}=\det \left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|---(3-4)$

Therefore the normal vector of S1, S2 plane is expressed as:

$\begin{array}{c}\stackrel{\→}{M_1}=\stackrel{\→}{BA}\×\stackrel{\→}{BC}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{BA}}_x& {\mathrm{BA}}_y& {\mathrm{BA}}_z\\ {\mathrm{BC}}_x& {\mathrm{BC}}_y& {\mathrm{BC}}_z\end{array}\right|\\ =({\mathrm{BA}}_y{\mathrm{BC}}_z-{\mathrm{BA}}_z{\mathrm{BC}}_y)\stackrel{\→}{i}+({\mathrm{BA}}_z{\mathrm{BC}}_x-{\mathrm{BA}}_x{\mathrm{BC}}_z)\stackrel{\→}{j}\\ +({\mathrm{BA}}_x{\mathrm{BC}}_y-{\mathrm{BA}}_y{\mathrm{BC}}_x)\stackrel{\→}{k}\end{array}---(3-5)$

$\begin{array}{c}\stackrel{\→}{M_2}=\stackrel{\→}{CB}\×\stackrel{\→}{CD}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{CB}}_x& {\mathrm{CB}}_y& {\mathrm{CB}}_z\\ {\mathrm{CD}}_x& {\mathrm{CD}}_y& {\mathrm{CD}}_z\end{array}\right|\\ =({\mathrm{CB}}_y{\mathrm{CD}}_z-{\mathrm{CB}}_z{\mathrm{CD}}_y)\stackrel{\→}{i}+({\mathrm{CB}}_z{\mathrm{CD}}_x-{\mathrm{CB}}_x{\mathrm{CD}}_z)\stackrel{\→}{j}\\ +({\mathrm{CB}}_x{\mathrm{CD}}_y-{\mathrm{CB}}_y{\mathrm{CD}}_x)\stackrel{\→}{k}\end{array}$

Namely the angle in LeftElbowYaw joint equals M1, M2 two angle of normal vector:

$LeftElbowYaw=arccos\frac{\stackrel{\→}{M1}\·\stackrel{\→}{M2}}{\left|\stackrel{\→}{M_1}\right|\left|\stackrel{\→}{M_2}\right|}---(3-7)$

3) joint angles: LeftShoulder.Roll

As shown in Figure 9:

$LeftShoulderRoll=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-8)$

4) joint angles: LeftShoulderPitch

As shown in Figure 10:

Angle between the angle in the LeftShoulderPitch joint plane that to be the plane that forms of upper arm and both shoulders axis form with both shoulders axis and backbone point, this angle is the angle of ABC and BCD two intersecting plane under normal circumstances, according to space plane angle calcu-lation formula, the computational methods of analogy LeftElbowYaw joint angle, can show that the computational methods of angle LeftShoulderPitch are as follows:

The normal vector of plane ABC:

The normal vector of plane BCD:

Joint angle LeftShoulderPitch calculates:

5) hand opening and closing angle: LeftHand

Because KINECT cannot to be accurate to the state of finger for the reading of hand information, therefore for hand opening and closing angle calculation, estimated the angle of hand opening and closing by the distance calculated between Lefthand and LeftWrist, computational methods are as follows:

As shown in figure 11:

$LeftHaud=\left|\stackrel{\→}{AB}\right|=\sqrt{{(A_x-B_x)}^2+{(A_y-B_y)}^2+{(A_z-B_z)}^2}---(3-12)$

So far, the articulate angle of left arm all calculates complete, in like manner can calculate right arm joint angles;

6) joint angles: HeadPitch

In head two joints, and bow and come back relevant angle name HeadPitch

As shown in figure 12:

The angle of joint of head HeadPitch chooses in shoulder the angle between the normal vector of the plane that vector and two is takeed on and backbone forms pointing to head, and specific formula for calculation is as follows:

7) joint angle: HeadYaw

As shown in figure 13:

The important joint angle HeadYaw of head another one is responsible for realizing head left-right rotation, due to the data message of face orientation cannot be provided in KINECT bone recognition result, therefore HeadYaw angle cannot directly calculate, but can find out by observing bone drafting animation, the locus of head is obviously arranged in the front of shoulder, and (namely front is when KINECT, certain distance is there is in Head point and Shouldercenter point in z-axis), therefore will be converted into the calculating of plane included angle herein about this joint angle:

The normal vector of plane ABC

Plane BCD normal vector

Joint angles

In the motion process of robot, joint of lower extremity activity will directly have influence on the stationarity of robot entirety, in order to simplify control difficulty, relative position method is adopted for lower limb controlling herein, by calculating the position of lower limb end in relative coordinate system in conjunction with the height of mass center of human body, realize the control to robot motion;

As shown in figure 14:

By the Hipcenter of bone recognition result point upright projection to ground, as new coordinate origin, get left and right ankle point Rightankle and Leftankle in new coordinate system coordinate as the control data of robot.

B, C 2 coordinate in O coordinate system is as follows,

LeftFoot＝(A _z-B _z,A _x-B _x)(3-17)

RightFoot＝(A _z-C _z,A _x-C _x)(3-18)

In order to the absolute distance error caused due to different people's height differences, herein that coordinate values is wide divided by human body hip, wherein the wide computing formula of hip is as follows:

$Hipwidth=\left|\stackrel{\→}{BC}\right|=\sqrt{{(B_x-C_x)}^2+{(B_y-C_y)}^2+{(B_z-C_z)}^2}---(3-19)$

Therefore the position of lower limb end in new coordinate system is as follows:

$LeftFoot=(\frac{A_z-B_z}{Hipwidth},\frac{A_x-B_x}{Hipwidth})---(3-20)$

$RightFoot=(\frac{A_z-C_z}{Hipwidth},\frac{A_x-C_x}{Hipwidth})---(3-21).$

Feature of the present invention is:

1, the present invention adopts advanced body sense equipment and forward position Robotics, devises a kind of new interactive mode system.

2, the present invention realizes controlling anthropomorphic robot and imitates human action in real time, has very high reliability, very strong flexibility, has good robustness at varying environment complex condition.

3, the present invention adopts relative threshold Distance Filter method, reduces environmental factor and the impact caused shaken by sensor self, effectively improves the reliability of data, the stability of system cloud gray model.

4, bone tracing algorithm is applied to human body attitude identification by the present invention, it is the feature of the anthropomorphic robot able to programme having 25 frees degree for NAO, realize controlling anthropomorphic robot and imitate human action in real time, there is very high reliability, at varying environment complex condition, there is good robustness.The present invention is based on the tracking of Kinect bone framework and robot NAO hardware platform.Kinect holds, and first-selection carries out program initialization, and this process comprises signal wiring and drives inspection, instantiation sensor object, obtains degree of depth authority, registered events monitor.Then start the identification of bone framework, this process comprises the acquisition of depth image, is identified human joint points by bone recognizer storehouse depth image and is extracted space coordinates, after filtering, space vector calculate, for robot NAO provides gesture stability data.

Accompanying drawing explanation

Fig. 1 is the flow chart of the method for the invention;

Fig. 2 is if the skeleton framework of PC end display does not mate with the action of target body, then reinitialize the flow chart of program;

Fig. 3 is skeleton framework identification process figure;

Fig. 4 is the schematic diagram of skeleton framework Dynamic Announce at screen;

Fig. 5 is the flow chart that relative threshold distance of the present invention compares filtering algorithm;

Fig. 6 is Kinect space coordinates;

Fig. 7 is the calculating schematic diagram of joint angles: LeftElbowRoll;

Fig. 8 is the calculating schematic diagram of joint angles: LeftElbowYaw;

Fig. 9 is the calculating schematic diagram of joint angles: LeftShoulder.Roll;

Figure 10 is the calculating schematic diagram of joint angles: LeftShoulderPitch;

Figure 11 is hand opening and closing angles: the schematic diagram of LeftHand;

Figure 12 is the calculating schematic diagram of joint angles: HeadPitch;

Figure 13 is the calculating schematic diagram of joint angle: HeadYaw;

Figure 14 is by the Hipcenter of bone recognition result point upright projection to ground, as new coordinate origin, get left and right ankle point Rightankle and Leftankle in new coordinate system coordinate as the calculating schematic diagram of the control data of robot.

Detailed description of the invention

Below in conjunction with embodiment and Figure of description, the present invention is described in detail, but is not limited thereto.

As Figure 1-5.

Embodiment,

Motion capture robot works in coordination with a method for flexible gesture stability, comprises step as follows:

(1) body sense equipment Kinect is utilized to catch not the corresponding target joint displacement data of target body in the same time in real time; Target body stands in body sense equipment Kinect horizon range, is to ensure best identified effect, and target body should be in 1.2-3.5m before the camera lens of body sense equipment Kinect, within horizontal view angle ± 57 °; Described body sense equipment Kinect possesses colour and depth perception answers camera lens, horizontal field of view: 57 degree, vertical visual field: 43 degree, degree of depth inductor scope 1.2m-3.5m.Run the service routine of PC end, see Fig. 2, first initialize body sense equipment Kinect, this process comprises signal wiring and drives inspection, instantiation sensor object, obtains degree of depth authority, registered events etc.;

(2) the target joint displacement data that body sense equipment Kinect captures is sent to PC end; Target body stands in before body sense equipment Kinect depth perception answers camera lens, can change the attitudes such as head, arm, finger, catch all target joint displacement datas by body sense equipment Kinect;

(3) PC termination receives all target joint displacement datas, and draws out skeleton framework in real time according to it; Draw the flow process of skeleton framework as Fig. 3;

(4) PC holds by skeleton framework Dynamic Announce on screen, and provides the feedback that reports an error; As Fig. 4, give four representative service routine running status figure: the part being labeled as 1 is that target body stands in horizon range, program initialization terminates the skeleton framework of the data construct of being caught first by body sense equipment Kinect of rear display, the program that represents starts normal, starts the attitudes vibration of real-time target acquisition human body; The part being labeled as 2 is the skeleton framework that program starts the data construct that the rear a certain moment is caught by body sense equipment Kinect, and this part reflects the attitude of this moment target body; The part being labeled as 3 represent to run due to program report an error, camera lens that target body is close to body sense equipment Kinect, cause the situation of skeleton framework distortion; The part being labeled as 4 is because target body is away from horizon range, causes body sense equipment Kinect to catch.The part being labeled as 3,4 be program to user feedback that reports an error, at this time need to reinitialize program, to ensure that robot receives reliable gesture stability data;

(5) PC end processes all target joint displacement datas received: comprise relative threshold distance and compare filtering calculating, space vector calculating, obtain the gesture stability data of anthropomorphic robot NAO, i.e. joint angles data; The present invention, due to reasons such as environmental factor and sensor self shakes, causes there is interfering data in the initial data that obtains, is therefore necessary that carrying out relative threshold distance to initial data compares filtering and calculate, robot motion is followed the trail of more accurately, reliable;

(6) call the JointControl function in the NAOqi operating system of anthropomorphic robot NAO, according to the joint angles data sent, the steering wheel of anthropomorphic robot NAO is controlled, make the action of anthropomorphic robot NAO real-time tracking target body.

Preferred according to the present invention, described step (4) also comprises, and described PC holds Dynamic Announce skeleton framework: if keeping strokes of the skeleton framework of PC end display and target body, then joint angles data are sent to anthropomorphic robot NAO; If the skeleton framework of PC end display does not mate with the action of target body, then reinitialize program, receive reliable gesture stability data with guarantor's anthropomorphic robot NAO.

Preferred according to the present invention, in described step (5), described relative threshold distance compares filtering calculating and comprises:

By observing the space coordinates change of displacement artis, calculate same joint in set time section (such as: 0.1s-0.5s) vectorial in the fluctuation of time started and end time composition, observe this fluctuation vector field homoemorphism and the fluctuation in space coordinates all directions, by setting fluctuation threshold value screening joint undulating value.Can find out that the shake of artis recognizing site is mainly along the fast jitter of change in coordinate axis direction, and when there is shake, fluctuation vector field homoemorphism significantly can increase.Therefore, the larger artis of reply fluctuation does respective handling, keeps laststate constant, use different threshold values to compare for different joints to the artis of fluctuation within a narrow range, ensure that each filtered result is all optimal solution, to ensure the continuity that robot pose converts.

Preferred according to the present invention, in described step (5), described space vector calculates and comprises:

Preferred according to the present invention, in described step (5), described space vector calculates and comprises:

The space coordinates that Kinect uses are different from common space coordinates, and its x-axis is identical with Traditional Space coordinate system with the zero point of y-axis, but its z-axis co-ordinate zero point is Kinect sensor, and positive direction is the dead ahead that Kinect points to.Kinect space coordinates are as shown in Figure 6:

By in Kinect space coordinates, draw any two coordinate points P1 do not overlapped (x1, y1, z1) in Kinect coordinate system, P2 (x2, y2, z2), the vectorial P1P2 to its composition:

If there is another P3 and this point not on the straight line at P1P2 place, then there is following relational expression:

According to above-mentioned character, human synovial angle calculation is reduced to the calculating to space vector angle, the computational methods of upper limb joint angle will be described respectively below:

1) joint angles: LeftElbowRoll

As shown in Figure 7: calculate one group of vector that LeftElbowRoll angle only need construct this place, space angle both sides,

And obtain according to the joint angles computing formula above mentioned:

$LeftElbowRoll=\∠ABC=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-3)$

2) joint angles: LeftElbowYaw

As shown in Figure 8:

The angle in LeftElbowYaw joint is the angle that ancon produces when upper arm rotates, and this angle is the angle of ABC and BCD two intersecting plane under normal circumstances, i.e. the angle of figure midplane S1 and S2; Show that the computational methods of angle LeftElbowRoll are as follows according to space plane angle calcu-lation formula.

First the formula of the normal vector clearly calculating two non-colinear vector place planes is needed:

$\stackrel{\→}{m}=\stackrel{\→}{a}\×\stackrel{\→}{b}=\det \left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|---(3-4)$

Therefore the normal vector of S1, S2 plane is expressed as:

$\begin{array}{c}\stackrel{\→}{M_1}=\stackrel{\→}{BA}\×\stackrel{\→}{BC}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{BA}}_x& {\mathrm{BA}}_y& {\mathrm{BA}}_z\\ {\mathrm{BC}}_x& {\mathrm{BC}}_y& {\mathrm{BC}}_z\end{array}\right|\\ =({\mathrm{BA}}_y{\mathrm{BC}}_z-{\mathrm{BA}}_z{\mathrm{BC}}_y)\stackrel{\→}{i}+({\mathrm{BA}}_z{\mathrm{BC}}_x-{\mathrm{BA}}_x{\mathrm{BC}}_z)\stackrel{\→}{j}\\ +({\mathrm{BA}}_x{\mathrm{BC}}_y-{\mathrm{BA}}_y{\mathrm{BC}}_x)\stackrel{\→}{k}\end{array}---(3-5)$

$\begin{array}{c}\stackrel{\→}{M_2}=\stackrel{\→}{CB}\×\stackrel{\→}{CD}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{CB}}_x& {\mathrm{CB}}_y& {\mathrm{CB}}_z\\ {\mathrm{CD}}_x& {\mathrm{CD}}_y& {\mathrm{CD}}_z\end{array}\right|\\ =({\mathrm{CB}}_y{\mathrm{CD}}_z-{\mathrm{CB}}_z{\mathrm{CD}}_y)\stackrel{\→}{i}+({\mathrm{CB}}_z{\mathrm{CD}}_x-{\mathrm{CB}}_x{\mathrm{CD}}_z)\stackrel{\→}{j}\\ +({\mathrm{CB}}_x{\mathrm{CD}}_y-{\mathrm{CB}}_y{\mathrm{CD}}_x)\stackrel{\→}{k}\end{array}$

Namely the angle in LeftElbowYaw joint equals M1, M2 two angle of normal vector:

$LeftElbowYaw=arccos\frac{\stackrel{\→}{M1}\·\stackrel{\→}{M2}}{\left|\stackrel{\→}{M_1}\right|\left|\stackrel{\→}{M_2}\right|}---(3-7)$

3) joint angles: LeftShoulder.Roll

As shown in Figure 9:

$LeftShoulderRoll=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---(3-8)$

4) joint angles: LeftShoulderPitch

As shown in Figure 10:

Angle between the angle in the LeftShoulderPitch joint plane that to be the plane that forms of upper arm and both shoulders axis form with both shoulders axis and backbone point, this angle is the angle of ABC and BCD two intersecting plane under normal circumstances, according to space plane angle calcu-lation formula, the computational methods of analogy LeftElbowYaw joint angle, can show that the computational methods of angle LeftShoulderPitch are as follows:

The normal vector of plane ABC:

The normal vector of plane BCD:

Joint angle LeftShoulderPitch calculates:

5) hand opening and closing angle: LeftHand

Because KINECT cannot to be accurate to the state of finger for the reading of hand information, therefore for hand opening and closing angle calculation, estimated the angle of hand opening and closing by the distance calculated between Lefthand and LeftWrist, computational methods are as follows:

As shown in figure 11:

$LeftHaud=\left|\stackrel{\→}{AB}\right|=\sqrt{{(A_x-B_x)}^2+{(A_y-B_y)}^2+{(A_z-B_z)}^2}---(3-12)$

So far, the articulate angle of left arm all calculates complete, in like manner can calculate right arm joint angles;

6) joint angles: HeadPitch

In head two joints, and bow and come back relevant angle name HeadPitch

As shown in figure 12:

The angle of joint of head HeadPitch chooses in shoulder the angle between the normal vector of the plane that vector and two is takeed on and backbone forms pointing to head, and specific formula for calculation is as follows:

7) joint angle: HeadYaw

As shown in figure 13:

The important joint angle HeadYaw of head another one is responsible for realizing head left-right rotation, due to the data message of face orientation cannot be provided in KINECT bone recognition result, therefore HeadYaw angle cannot directly calculate, but can find out by observing bone drafting animation, the locus of head is obviously arranged in the front of shoulder, and (namely front is when KINECT, certain distance is there is in Head point and Shouldercenter point in z-axis), therefore will be converted into the calculating of plane included angle herein about this joint angle:

The normal vector of plane ABC

Plane BCD normal vector

Joint angles

In the motion process of robot, joint of lower extremity activity will directly have influence on the stationarity of robot entirety, in order to simplify control difficulty, relative position method is adopted for lower limb controlling herein, by calculating the position of lower limb end in relative coordinate system in conjunction with the height of mass center of human body, realize the control to robot motion;

As shown in figure 14:

By the Hipcenter of bone recognition result point upright projection to ground, as new coordinate origin, get left and right ankle point Rightankle and Leftankle in new coordinate system coordinate as the control data of robot.

B, C 2 coordinate in O coordinate system is as follows,

LeftFoot＝(A _z-B _z,A _x-B _x)(3-17)

RightFoot＝(A _z-C _z,A _x-C _x)(3-18)

In order to the absolute distance error caused due to different people's height differences, herein that coordinate values is wide divided by human body hip, wherein the wide computing formula of hip is as follows:

$Hipwidth=\left|\stackrel{\→}{BC}\right|=\sqrt{{(A_x-C_x)}^2+{(B_y-C_y)}^2+{(B_z-C_z)}^2}---(3-19)$

Therefore the position of lower limb end in new coordinate system is as follows:

$LeftFoot=(\frac{A_z-B_z}{Hipwidth},\frac{A_x-B_x}{Hipwidth})---(3-20)$

$RightFoot=(\frac{A_z-C_z}{Hipwidth},\frac{A_x-C_x}{Hipwidth})---(3-21).$

Claims (4)

1. motion capture robot works in coordination with a method for flexible gesture stability, it is characterized in that the method comprising the steps of as follows:

(1) body sense equipment Kinect is utilized to catch not the corresponding target joint displacement data of target body in the same time in real time;

(2) the target joint displacement data that body sense equipment Kinect captures is sent to PC end;

(3) PC termination receives all target joint displacement datas, and draws out skeleton framework in real time according to it;

(4) PC holds by skeleton framework Dynamic Announce on screen, and provides the feedback that reports an error;

(5) PC end processes all target joint displacement datas received: comprise relative threshold distance and compare filtering calculating, space vector calculating, obtain the gesture stability data of anthropomorphic robot NAO, i.e. joint angles data;

(6) call the JointControl function in the NAOqi operating system of anthropomorphic robot NAO, according to the joint angles data sent, the steering wheel of anthropomorphic robot NAO is controlled, make the action of anthropomorphic robot NAO real-time tracking target body.

2. a kind of motion capture robot according to claim 1 works in coordination with the method for flexible gesture stability, it is characterized in that, described step (4) also comprises, described PC holds Dynamic Announce skeleton framework: if keeping strokes of the skeleton framework of PC end display and target body, then joint angles data are sent to anthropomorphic robot NAO; If the skeleton framework of PC end display does not mate with the action of target body, then reinitialize program.

3. a kind of motion capture robot according to claim 1 works in coordination with the method for flexible gesture stability, it is characterized in that, in described step (5), described relative threshold distance compares filtering calculating and comprises:

By observing the space coordinates change of displacement artis, calculate same joint in set time section vectorial in the fluctuation of time started and end time composition, observe this fluctuation vector field homoemorphism and the fluctuation in space coordinates all directions, by setting fluctuation threshold value screening joint undulating value.

4. a kind of motion capture robot according to claim 1 works in coordination with the method for flexible gesture stability, it is characterized in that, in described step (5), described space vector calculates and comprises:

By in Kinect space coordinates, draw any two coordinate points P1 do not overlapped (x1, y1, z1) in Kinect coordinate system, P2 (x2, y2, z2), the vectorial P1P2 to its composition:

If there is another P3 and this point not on the straight line at P1P2 place, then there is following relational expression:

According to above-mentioned character, human synovial angle calculation is reduced to the calculating to space vector angle, the computational methods of upper limb joint angle will be described respectively below:

1) joint angles: LeftElbowRoll

Calculate one group of vector that LeftElbowRoll angle only need construct this place, space angle both sides,

And obtain according to the joint angles computing formula above mentioned:

$\mathrm{leftElbowRoll}=\∠\mathrm{ABC}=\arccos \frac{\stackrel{\→}{\mathrm{BA}}\·\stackrel{\→}{\mathrm{BC}}}{\left|\stackrel{\→}{\mathrm{BA}}\right|\left|\stackrel{\→}{\mathrm{BC}}\right|}---(3-3)$

2) joint angles: LeftElbowYaw

The angle in LeftElbowYaw joint is the angle that ancon produces when upper arm rotates, and this angle is the angle of ABC and BCD two intersecting plane under normal circumstances; Show that the computational methods of angle LeftElbowRoll are as follows according to space plane angle calcu-lation formula:

First the formula of the normal vector clearly calculating two non-colinear vector place planes is needed:

$\stackrel{\→}{m}=\stackrel{\→}{a}\×\stackrel{\→}{b}=\det \left|\begin{array}{ccc}i& j& k\\ a_x& a_y& a_z\\ b_x& b_y& b_z\end{array}\right|---\left(3-4\right)$

Therefore the normal vector of S1, S2 plane is expressed as:

$\begin{array}{c}\stackrel{\→}{M_1}=\stackrel{\→}{BA}\×\stackrel{\→}{BC}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{BA}}_x& {\mathrm{BA}}_y& {\mathrm{BA}}_z\\ {\mathrm{BC}}_x& {\mathrm{BC}}_y& {\mathrm{BC}}_z\end{array}\right|\\ =\left({\mathrm{BA}}_y{\mathrm{BC}}_z-{\mathrm{BA}}_z{\mathrm{BC}}_y\right)\stackrel{\→}{i}+\left({\mathrm{BA}}_z{\mathrm{BC}}_x-{\mathrm{BA}}_x{\mathrm{BC}}_z\right)\stackrel{\→}{j}\\ +\left({\mathrm{BA}}_x{\mathrm{BC}}_y-{\mathrm{BA}}_y{\mathrm{BC}}_x\right)\stackrel{\→}{k}\end{array}---\left(3-5\right)$

$\begin{array}{c}\stackrel{\→}{M_2}=\stackrel{\→}{CB}\×\stackrel{\→}{CD}=\det \left|\begin{array}{ccc}\stackrel{\→}{i}& \stackrel{\→}{j}& \stackrel{\→}{k}\\ {\mathrm{CB}}_x& {\mathrm{CB}}_y& {\mathrm{CB}}_z\\ {\mathrm{CD}}_x& {\mathrm{CD}}_y& {\mathrm{CD}}_z\end{array}\right|\\ =\left({\mathrm{CB}}_y{\mathrm{CD}}_z-{\mathrm{CB}}_z{\mathrm{CD}}_y\right)\stackrel{\→}{i}+\left({\mathrm{CB}}_z{\mathrm{CD}}_x-{\mathrm{CB}}_x{\mathrm{CD}}_z\right)\stackrel{\→}{j}\\ +\left({\mathrm{CB}}_x{\mathrm{CD}}_y-{\mathrm{CB}}_y{\mathrm{CD}}_x\right)\stackrel{\→}{k}\end{array}$

Namely the angle in LeftElbowYaw joint equals M1, M2 two angle of normal vector:

$LeftElbowYaw=arccos\frac{\stackrel{\→}{M1}\·\stackrel{\→}{M2}}{\left|\stackrel{\→}{M_1}\right|\left|\stackrel{\→}{M_2}\right|}---\left(3-7\right)$

3) joint angles: LeftShoulder.Roll

$LeftShoulderRoll=arccos\frac{\stackrel{\→}{BA}\·\stackrel{\→}{BC}}{\left|\stackrel{\→}{BA}\right|\left|\stackrel{\→}{BC}\right|}---\left(3-8\right)$

4) joint angles: LeftShoulderPitch

Angle between the angle in the LeftShoulderPitch joint plane that to be the plane that forms of upper arm and both shoulders axis form with both shoulders axis and backbone point, this angle is the angle of ABC and BCD two intersecting plane under normal circumstances, according to space plane angle calcu-lation formula, the computational methods of analogy LeftElbowYaw joint angle, can show that the computational methods of angle LeftShoulderPitch are as follows:

The normal vector of plane ABC:

The normal vector of plane BCD:

Joint angle LeftShoulderPitch calculates:

5) hand opening and closing angle: LeftHand

Because KINECT cannot to be accurate to the state of finger for the reading of hand information, therefore for hand opening and closing angle calculation, estimated the angle of hand opening and closing by the distance calculated between Lefthand and LeftWrist, computational methods are as follows:

$LeftHaud=\left|\stackrel{\→}{AB}\right|=\sqrt{{(A_x-B_x)}^2+{(A_y-B_y)}^2+{(A_z-B_z)}^2}---\left(3-12\right)$

So far, the articulate angle of left arm all calculates complete, in like manner can calculate right arm joint angles;

6) joint angles: HeadPitch

In head two joints, and bow and come back relevant angle name HeadPitch

The angle of joint of head HeadPitch chooses in shoulder the angle between the normal vector of the plane that vector and two is takeed on and backbone forms pointing to head, and specific formula for calculation is as follows:

7) joint angle: HeadYaw

The important joint angle HeadYaw of head another one is responsible for realizing head left-right rotation, therefore will be converted into the calculating of plane included angle herein about this joint angle:

The normal vector of plane ABC

Plane BCD normal vector

Joint angles

In the motion process of robot, joint of lower extremity activity will directly have influence on the stationarity of robot entirety, in order to simplify control difficulty, relative position method is adopted for lower limb controlling herein, by calculating the position of lower limb end in relative coordinate system in conjunction with the height of mass center of human body, realize the control to robot motion;

By the Hipcenter of bone recognition result point upright projection to ground, as new coordinate origin, get left and right ankle point Rightankle and Leftankle in new coordinate system coordinate as the control data of robot;

B, C 2 coordinate in O coordinate system is as follows,

LeftFoot＝(A _z-B _z,A _x-B _x)(3-17)

RightFoot＝(A _z-C _z,A _x-C _x)(3-18)

In order to the absolute distance error caused due to different people's height differences, herein that coordinate values is wide divided by human body hip, wherein the wide computing formula of hip is as follows:

$Hipwidth=\left|\stackrel{\→}{BC}\right|=\sqrt{{(B_x-C_x)}^2+{(B_y-C_y)}^2+{(B_z-C_z)}^2}---\left(3-19\right)$

Therefore the position of lower limb end in new coordinate system is as follows:

$LeftFoot=\left(\frac{A_z-B_z}{Hipwidth},\frac{A_x-B_x}{Hipwidth}\right)---\left(3-20\right)$

$RightFoot=\left(\frac{A_z-C_z}{Hipwidth},\frac{A_x-C_x}{Hipwidth}\right)---\left(3-21\right).$