The mechanical arm method is controlled in a kind of sense of body based on Kinect
Technical field
What the present invention relates to is a kind of robot control method, specifically body sense robot control method.
Background technology
Kinect 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 interaction simultaneously.Microsoft has released Kinect for Windows SDK Beta in June, 2011.It is a kind of novel man-machine interactive system, is also a kind of new figure's detecting sensor, and its application is very wide, controls as virtual mirror, 3D modeling, virtual musical instrument, virtual entertainment and machinery etc.Study also seldom based on the robot control method of Kinect at present, research and utilization Kinect realizes that the flexible control of robot has very wide application prospect.
Japan is by its advanced Robotics at present, and scientist applies the Kinect sensor robot is controlled and tests in real time, obtains certain effect, has proved with the Kinect sensor and has realized that the control method of robot is feasible.The U.S. has been applied to Kinect on the patrol robot of military battlefield, and Kinect detecting real-time robot the place ahead three-dimensional environment information is with the operation of decision-making robot, the three-dimensional map in simultaneously can reconstruct robot running.The control research that more domestic companies or university are applied to robot to Kinect now also seldom, more is not applied in the middle of actual production.The present invention passes through the further investigation to Kinect, and controls experiment on the small scale robot platform, is expected to simultaneously this technology is applied to go on patrol the controls such as safety protection robot, medical auxiliary robot, mechanical arm.
Existing robot control method is all by programme-control, can't work asynchronously with the people.
Summary of the invention
The object of the present invention is to provide a kind of sense of body based on Kinect that works asynchronously with the people of intelligence to control the mechanical arm method.
The object of the present invention is achieved like this:
The mechanical arm method is controlled in a kind of sense of body based on Kinect of the present invention, it is characterized in that:
(1) obtain the three-dimensional coordinate in 5 joints of right side upper limbs of human body by the Kinect sensor, the three-dimensional coordinate in 5 joints of right side upper limbs comprises right stern joint coordinates, right shoulder joint coordinate, right elbow joint coordinate, right wrist joint coordinate and right hand joint coordinates;
(2) the two exponent filtering algorithms of 5 joint coordinates data of the right side upper limbs utilization that obtains are carried out smoothly;
(3) 5 joint coordinates of right side upper limbs after utilizing smoothly build vector in the three-dimensional coordinate system of Kinect, obtain the angle at upper limbs place, right side by the compute vector angle, the angle at upper limbs place, described right side comprises right shoulder joint place angle, right elbow joint place angle, right wrist joint place angle;
(4) angle information is merged, forms a packet, add data packet head and verification and, send to robot by wireless serial, carry out mechanical arm and control.
The present invention can also comprise:
1, described pair of exponent filtering algorithm is:
T represents the time, { x
tThe expression original data sequence, { s
tThe constantly two exponential smoothing results of expression t, { b
tThe expression t optimal estimation of data sequence trend constantly, F
t+mExpression x is in t+m optimal estimation constantly, and m is predictive factor, and m〉0, two exponential smoothing filtering algorithm formula are as follows:
s
1=x
0
b
1=x
1-x
0
s
t=αx
t+(1-α)(s
t-1+b
t-1),t>1
b
t=β(s
t-s
t-1)+(1-β)b
t-1,t>1
F
t+m=s
t+mb
t
α represents the data smoothing factor, and 0<α<1, and β represents the trend smoothing factor, and 0<β<1, definition F
1=s
0+ b
0, x can estimate according to above-mentioned pair of exponential smoothing filtering algorithm formula in all values constantly.
2, the process that right elbow joint coordinate is level and smooth is:
(1) initiation parameter, smoothly export s to data smoothing factor α, trend smoothing factor β, predictive factor m, right elbow joint coordinate
n, right elbow joint coordinate trend optimal estimation b
n, right elbow joint coordinate final result optimal estimation F
n+1, the current right elbow joint coordinate v that obtains from Kinect
n, counting variable n gives respectively initial value, n is integer variable, often obtains once the coordinate of right elbow joint, n adds 1;
(2) obtain right elbow joint coordinate v from Kinect
0, enter iteration n=0 for the first time, trend prediction b
0Assignment is 0, smoothly exports s
0Assignment is for working as front right elbow joint coordinate v
0, right elbow joint coordinate is finally exported F
1=s
0+ b
0
(3) obtain right elbow joint coordinate v from Kinect
1, enter iteration n=1 for the second time, smoothly export s
1Assignment is for working as front right elbow joint coordinate v
1With right elbow joint v last time
0Mean value, namely
Trend prediction b
1=(s
1-s
0) β, right elbow joint coordinate is finally exported F
2=s
1+ mb
1
(4) obtain right elbow joint coordinate v from Kinect
2, enter iteration n=2 for the third time, smoothly export s
2According to formula s
n=α v
n+ (1-α) (s
n-1+ b
n-1) calculate trend prediction b
2According to formula b
n=β (s
n-s
n-1)+(1-β) b
n-1Calculate, right elbow joint coordinate is finally exported according to formula F
n+1=s
n+ mb
nCalculate;
(5), according to the iterative manner of step (4), increase iterations, calculate right elbow joint coordinate output,, until n overflows zero clearing, get back to step (2) and restart iteration, along with the motion of people's right arm, realization is constantly level and smooth to right elbow joint coordinate;
The smoothing process of all the other 4 joint coordinates of right side upper limbs is identical with the smoothing process of right elbow joint coordinate.
3, upper limb joint angle computation method in right side is:
(1) set up Kinect coordinate system XYZ, obtain to represent respectively the three dimensional space coordinate H (x of right stern artis H, right shoulder joint node S, right elbow joint point E, right wrist joint point W and right hand artis T under the Kinect coordinate system
h, y
h, z
h), S (x
s, y
s, z
s), E (x
e, y
e, z
e), W (x
w, y
w, z
w), T (x
t, y
t, z
t);
Definition global variable A, B, C represent respectively right shoulder joint place angle, right elbow joint place angle, right wrist joint place angle, all are initialized as 0; (2) calculate respectively vector
(3) calculate respectively ∠ A, ∠ B, ∠ C, the result of calculation correspondence is kept in global variable A, B, C.
4, the main controller basic module initializes, and comprises clock, PWM, serial communication and interruption; Wait for that the arm angle information packet that host computer sends arrives, after arriving, packet notifies main controller in the mode of interrupting, main controller read after whole packet calculation check and, if and the verification of host computer transmission and identical, this packet is effective, and packet is resolved, and reads the angle value in each joint of mechanical arm, otherwise this packet is invalid, and the angle value in each joint of former mechanical arm remains unchanged; Each joint angles value of mechanical arm that reads is converted into corresponding pwm signal, thereby controls manipulator motion.
The control method of the mechanical arm that the control method of the mechanical arm that 5, the left side upper limbs of human body, left side lower limb and right side lower limb are corresponding and the right side upper limbs of human body are corresponding is identical.
Advantage of the present invention is:
1, accurately identify by the action to human body, can complete to mechanical arm, mobile robot flexibly, accurately control.Make the more friendly alternately of people and robot, improve the intelligent of robot.
2, adopt modular programming, facilitate the transplanting of program on different machines people platform.
3, this technology can be used for tele-robotic and control, Long-distance Control explosive-removal robot for example, thus reduce unnecessary casualties.
4, whole robot controls interface and adopts the XAML programming, makes interface and behavior be separated, and conveniently expansion and integrated later, and developer can be brought into play speciality separately, and more complicated robot control is finally completed in synchronous exploitation.
Description of drawings
Fig. 1 is two exponent filtering algorithm flow charts of embodiment 1;
Fig. 2 is the Kinect coordinate system schematic diagram of embodiment 1;
Fig. 3 is the right arm geometric representation of embodiment 1;
Fig. 4 is the geometric representation of the mechanical arm of embodiment 1;
Fig. 5 a is the mechanical arm control principle block diagram a of embodiment 1, and Fig. 5 b is the mechanical arm control principle block diagram b of embodiment 1.
The specific embodiment
For example the present invention is described in more detail below in conjunction with accompanying drawing:
Embodiment 1:
In conjunction with Fig. 1~5, the general steps of present embodiment is:
(1) obtain 20 skeleton point three-dimensional coordinates of human body by the Kinect sensor, the joint coordinates that experiment is mainly used comprises right stern joint coordinates, right shoulder joint coordinate, right elbow joint coordinate, right wrist joint coordinate and right hand joint coordinates.
(2) the two exponent filtering algorithms of the right arm joint coordinates data utilization that obtains are carried out smoothly, reduce the shake in right arm joint motions process.
(3) utilize filtered right arm joint coordinates to build vector in the three-dimensional coordinate system of Kinect, just know all angles of right arm joint by the compute vector angle, comprise right shoulder joint place angle, right elbow joint place angle, right wrist joint place angle.Result of calculation is preserved;
(4) angle information is merged, forms a packet, add data packet head and verification and, send to robot by wireless serial, be used for mechanical arm and control.
Idiographic flow is as follows: the two exponent filtering algorithm principle of (1) joint coordinates and flow process
The present invention adopts two exponential smoothing filtering algorithms.T represents the time, { x
tThe expression original data sequence, { s
tThe constantly two exponential smoothing results of expression t, { b
tThe expression t optimal estimation of data sequence trend constantly, F
t+mExpression x is in t+m optimal estimation constantly, and m is predictive factor, and m〉0, the concrete formula of two exponential smoothing filtering algorithms is as follows:
s
1=x
0 (1)
b
1=x
1-x
0 (2)
s
t=αx
t+(1-α)(s
t-1+b
t-1),t>1 (3)
b
t=β(s
t-s
t-1)+(1-β)b
t-1,t>1 (4)
F
t+m=s
t+ mb
t(5) wherein α represents the data smoothing factor, and 0<α<1, and β represents the trend smoothing factor, and 0<β<1.Definition F
1=s
0+ b
0, x can estimate according to formula in all values constantly like this.With reference to Fig. 1, smoothly as example, other joint coordinates are smoothly similar take right arm elbow joint coordinate, and concrete level and smooth performing step is as follows:
Step 1: initialize a plurality of parameters.
The data smoothing factor-alpha is initialized as 0.5, and trend smoothing factor β is initialized as 0.25, and predictive factor m is initialized as 0.5, s
nRepresent that right elbow joint coordinate smoothly exports, (0,0,0), b begin to turn to
nRepresent the optimal estimation of right elbow joint coordinate trend, be initialized as (0,0,0), F
n+1Represent the optimal estimation of right elbow joint coordinate final result, be initialized as (0,0,0), v
nRepresent the current right elbow joint coordinate that obtains from Kinect, be initialized as (0,0,0), n represents counting variable, often obtains once the coordinate of right elbow joint, and n adds 1, is defined as integer variable, initializes n=0.
Step 2:
Obtain right elbow joint coordinate v from Kinect
0, enter iteration n=0 for the first time, trend prediction b
0Assignment is 0, smoothly exports s
0Assignment is for working as front right elbow joint coordinate v
0, right elbow joint coordinate is finally exported F
1=s
0+ b
0, n adds 1.
Step 3:
Obtain right elbow joint coordinate v from Kinect
1, enter iteration n=1 for the second time, smoothly export s
1Assignment is for working as front right elbow joint coordinate v
1With right elbow joint v last time
0Mean value, namely
Trend prediction b
1=(s
1-s
0) β, right elbow joint coordinate is finally exported F
2=s
1+ mb
1, n adds 1.
Step 4:
Obtain right elbow joint coordinate v from Kinect
2, enter iteration n=2 for the third time, smoothly export s
2According to formula s
n=α v
n+ (1-α) (s
n-1+ b
n-1) calculate, trend prediction b2 is according to formula b
n=β (s
n-s
n-1)+(1-β) b
n-1Calculate, right elbow joint coordinate is finally exported according to formula F
n+1=s
n+ mb
nCalculate, n adds 1.
Step 5: from Kinect, obtain next right elbow joint coordinate v
nRepeating step 4, iteration count variable n constantly adds 1, because n is integer variable, finally make n overflow zero clearing so constantly add 1, get back to again step 2 iteration again, therefore, along with the motion of people's right arm in Kinect the place ahead, just can realize to right elbow joint coordinate constantly not smoothly.
(2) arm joint angle computation method
The Kinect coordinate system is with reference to Fig. 2, and the right arm geometric representation is with reference to Fig. 3.Obtain to represent respectively the three dimensional space coordinate (x, y, z) of right stern artis H, right shoulder joint node S, right elbow joint point E, right wrist joint point W and right hand T under the Kinect coordinate system, at first process through two exponential smoothings, then build vector,, according to three-dimensional vector angle computing formula, establish
Vectorial
With
Included angle cosine be
Its angle
Calculate finally each joint angles of arm.The angle at a, the right shoulder joint of right shoulder joint place angle calculation place is the angle A in Fig. 2, centered by right shoulder joint S, and vector
Right shoulder joint place angle
。
B, right elbow joint place angle calculation
The angle at right elbow joint place is the angle B in Fig. 2, centered by right elbow joint E, and vector
Right elbow joint place angle
。
C, right wrist joint place angle calculation
The angle at right wrist joint place is the angle C in Fig. 2, centered by right wrist joint W, and vector
Right wrist joint place angle
。
Concrete steps:
Step 1:
Definition global variable A, B, C represent respectively right shoulder joint place angle, right elbow joint place angle, right wrist joint place angle, all are initialized as 0.
Step 2:
Right stern joint coordinates H (x after obtaining smoothly
h, y
h, z
h), right shoulder joint coordinate S (x
s, y
s, z
s), right elbow joint coordinate E (x
e, y
e, z
e), right wrist joint coordinate W (x
w, y
w, z
w) and right-handed scale (R.H.scale) T (x
t, y
t, z
t).Calculate respectively vector according to above-mentioned vector calculation formula
Step 3:
Computing formula according to above-mentioned right shoulder joint place angle A calculates ∠ A, in like manner calculates the ∠ B of right elbow joint place, the ∠ C of right wrist joint place, and the result of calculation correspondence is kept in global variable A, B, C.Angle information is formed a packet, add data packet head and verification and, send to robot by wireless serial, return to step 2.
(3) manipulator motion is controlled
The geometric representation of mechanical arm is with reference to figure 4, and its main controller adopts the MC9S12XS128 microcontroller.The motion in each joint of mechanical arm adopts Servo-controller to control, and No. 1 steering wheel represents that right shoulder joint, No. 2 steering wheels represent that right elbow joint, No. 3 steering wheels represent right wrist joint, and when arm was pasting body, in mechanical arm and figure, 0 ° of reference line overlapped.It is as follows that manipulator motion is controlled concrete steps:
Step 1:
The MC9S12XS128 basic module initializes, and comprises clock, PWM, serial communication and interruption.
Step 2:
Wait for that the arm angle information packet that host computer sends arrives, after arriving, packet notifies main controller in the mode of interrupting, main controller read after whole packet calculation check and, if and the verification of host computer transmission and identical, this packet is effective, and packet is resolved, and reads the angle value in each joint of mechanical arm, otherwise this packet is invalid, and the angle value in each joint of former mechanical arm remains unchanged.
Step 3:
Each joint angles value of mechanical arm that reads is converted into corresponding pwm signal, controls manipulator motion, return to step 2.
Constantly repeat according to above-mentioned steps, just can realize the tracking of mechanical arm to arm action, the theory diagram that whole mechanical arm is controlled is with reference to figure 5.In experiment, mechanical arm is tracing arm motion reposefully, there is no jitter phenomenon, has proved that above-mentioned filtering algorithm and arm angle computation method are correct and effective.