CN101574586B - Shuttlecock robot and control method thereof - Google Patents

Abstract

The invention discloses a shuttlecock robot and a control method thereof, which belong to the field of robots. The invention provides a shuttlecock robot capable of moving in a three-dimensional spaceand a control method thereof. The shuttlecock robot comprises an operation track part, a hitting part and a visual part, wherein a longitudinal track is provided with a longitudinal sliding block, an d a transverse track is provided with a transverse sliding block; longitudinal and transverse synchronous belts are connected with a motor respectively; the transverse sliding block is provided with the hitting part, and a shaft of a racket is connected with a shaft of the motor; the visual part comprises two cameras which are perpendicular to each other; and the inside of the motor is provided with a sensor. The control method of the shuttlecock robot comprises the steps of: acquiring an image for binarization processing, extracting coordinates of a shuttlecock image, and converting the coordinates of the shuttlecock image into actual coordinates; establishing a three-dimensional rectangular coordinate system, and evaluating three-dimension coordinates of a shuttlecock; performing Kallmanfiltering on the coordinates; performing trajectory pre-estimation; and detecting information of the racket through the sensor, sending the information of the racket into a signal control system alon g with pre-estimated coordinates, and using a servo system to control the motor to finish the operation along the tracks and the action of the hitting of the robot.

Description

Shuttlecock playing robot and control method

Technical field:

The invention belongs to the robot field, particularly a kind of can be in the shuttlecock playing robot and the control method of three dimensions activity.

Background technology:

First robot truly is born in the U.S. from nineteen fifty-nine, and Robotics has experienced development at full speed, and its developing direction is also intelligent, specialized day by day.In the development process of physical culture robot system, not only want comprehensive artificial intelligence, precision optical machinery, communication and computer technology etc., but also relate to multi-disciplinary contents such as image processing, sensing data fusion, decision-making and countermeasure.The shuttlecock playing robot is as one in numerous branches of physical culture robot, become the project that each scientific research institution, academic unit both at home and abroad give priority to.Yet, because traditional physical culture robot is confined to two-dimensional space mostly for the processing of data and the feedback of moving, make its vision system bad to the anticipation effect of object space, cause the identification and the location of object accurate inadequately.Being embodied in the shuttlecock playing robot, is exactly that the shuttlecock playing robot is bad to the anticipation effect of shuttlecock playing position, causes the identification and the location of shuttlecock playing accurate inadequately.

Summary of the invention:

The present invention is directed to existing physical culture robot for the problem that the processing and the action feedback of data only is confined to two-dimensional space, provide a kind of can be in the shuttlecock playing robot and the control method of three dimensions activity.

To achieve these goals, the present invention adopts following technical scheme, a kind of shuttlecock playing robot, comprise orbit part, batting part and vision part, described orbit partly comprises the base with long rails, long rails is provided with the longitudinal sliding block that can have cross track above moving on the long rails, and the cross track of longitudinal sliding block is provided with the transverse slider that can move on cross track; Between long rails, be respectively arranged with vertically synchronously band, horizontal band synchronously between cross track, vertically synchronously band, laterally synchronously band respectively with vertical control motor of base side, laterally control motor and link to each other; Transverse slider is provided with described batting part, and described driving part branch comprises the base plate that is fixed with racket, and the axle of racket links to each other with the axle of the worm reducer of motor; Described vision partly comprises two the mutually perpendicular cameras that link to each other with the shuttlecock playing robot control system that are arranged on top, shuttlecock court ground; At vertical control motor of rail portion, laterally the motor internal of controlling motor and batting part is provided with the sensor that detects racket angle and positional information.

Described shuttlecock playing ROBOT CONTROL method comprises the steps:

Step 1: by two mutually perpendicular two-dimensional images of two mutually perpendicular camera collections, and the two-dimensional image that collects is carried out binary conversion treatment, obtain two width of cloth bianry images by OPENCV software; The last image coordinate that extracts shuttlecock playing in two width of cloth bianry images, and the image coordinate of the shuttlecock playing that extracts carried out the correction of image radial distortion and projection distortion is converted to actual coordinate with respect to the place with the image coordinate of shuttlecock playing;

Step 2: set up three-dimensional cartesian coordinate system according to two mutually perpendicular bianry images, in three-dimensional cartesian coordinate system, determine the actual coordinate P of shuttlecock playing in horizontal bianry image ₁(x ₁, y ₁, 0) and the actual coordinate P of shuttlecock playing in vertical bianry image ₂(0, y ₂, z ₂); Record the actual coordinate Pc of vertical camera in three-dimensional cartesian coordinate system ₁(x _C1, y _C1, z _C1) and the actual coordinate Pc of horizontal camera in three-dimensional cartesian coordinate system ₂(x _C2, y _C2, z _C2); Connect P ₁With Pc ₁, obtain space line L ₁, connect P ₂With Pc ₂, obtain space line L ₂, space line L ₁, L ₂The mid point Pt (x of common vertical line section _t, y _t, z _t) be the coordinate of shuttlecock playing in three-dimensional cartesian coordinate system;

Step 3: to the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system that tries to achieve in the step 2 _t, y _t, z _t) carry out Kalman filtering, obtain the coordinate Pt ' (x of a filtered shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t);

Step 4: the orbiting motion equation that obtains shuttlecock playing by mechanical analysis; The coordinate Pt ' (x of the filtered shuttlecock playing that obtains in the integrating step three in three-dimensional cartesian coordinate system again _t, y _t, z _t), carry out trace predicating with least square method; Estimate the three-dimensional coordinate P (x that obtains next moment drop point of shuttlecock playing _P, y _P, z _P), and the coordinate input signal control system that will put;

Step 5: by being arranged on the sensor of rail portion and batting motor internal partly, detect the angle information and the positional information of racket, and with voltage signal input signal control system; Next three-dimensional coordinate P (x of drop point constantly of the shuttlecock playing that whistle control system obtains in the integrating step four again _P, y _P, z _P), calculate the amount of exercise of shuttlecock playing robot, and, finish the shuttlecock playing robot along the operation of track and the action of batting by motor servo control system control motor; Promptly reach laterally control driven by motor batting partly at the horizontal rail componental movement, then control the motor of batting part again and carry out shot by vertical control motor.

Described image coordinate is carried out the correction of image radial distortion and projection distortion, the image coordinate of shuttlecock playing is converted to actual coordinate with respect to the place, the specific implementation process is as follows:

(1) two mutually perpendicular cameras being looked the shuttlecock playing place and demarcated respectively along edge in the place, is initial point with the upper left corner, place, measures the actual coordinate of calibration point; And by in image, clicking the image coordinate that calibration point draws calibration point;

(2) utilize the multinomial scaling method of radial distortion, set up actual coordinate (x _w, y _w) and image coordinate (u, v) the cubic polynomial relation between:

$\left\{\begin{array}{c}{x}_w=a_0+a_{1}v+a_2{v}^2+a_3{v}^3+a_{4}u+a_5\mathrm{uv}+a_6{\mathrm{uv}}^2+a_7{u}^2+a_8{u}^{2}v+a_9{u}^3\\ y_w={\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">b</figure-callout>}}_0+b_{1}v+b_2{v}^2+b_3{v}^3+b_{4}u+b_5\mathrm{uv}+b_6{\mathrm{uv}}^2+b_7{u}^2+b_8{u}^{2}v+b_9{u}^3\end{array}\right.---\left(1\right)$

(3), utilize least square solution overdetermined equation group according to the actual coordinate and the image coordinate of the calibration point of measuring in the step (1):

$\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]=0\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]x_{i1}=0\\ ...\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]x_{\mathrm{iq}}=0\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(b_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]=0\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(b_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]x_{i1}=0\\ ...\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(b_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]x_{\mathrm{iq}}=0\end{array}\right.---\left(3\right)$

Wherein, a _i(i=0,1,2 ... 9), b _i(i=0,1,2 ... 9) be parameter, obtain a _i(i=0,1,2 ... 9), b _i(i=0,1,2 ... 9) value; The mapping relations between image coordinate and the actual coordinate have promptly been set up;

(4) according to image coordinate of setting up in the step (3) and the mapping relations between the actual coordinate, and the image coordinate of the shuttlecock playing that in two width of cloth bianry images, extracts in the step 1, obtain the actual coordinate of shuttlecock playing with respect to the shuttlecock playing place.

Coordinate Pt (the x of described shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) concrete computational process as follows:

(1) asks for space line L ₁, L ₂Equation:

Space line L ₁:

$\frac{x-x_1}{x_{c1}-x_1}=\frac{y-y_1}{y_{c1}-y_1}=\frac{z-z_1}{z_{c1}-z_1}---\left(4\right)$

Space line L ₂:

$\frac{x-x_2}{x_{c2}-x_2}=\frac{y-y_2}{y_{c2}-y_2}=\frac{z-z_2}{z_{c2}-z_2}---\left(5\right)$

(2) with square distance be asking for of object function:

$M(x_t,y_t,z_t)=d_1^2(x_t,y_t,z_t)+d_2^2(x_t,y_t,z_t)---\left(6\right)$

Wherein, M (x _t, y _t, z _t) be object function, d ₁, d ₂Be respectively the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) apart from space line L ₁, L ₂Distance;

(3) the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) try to achieve:

To the object function M (x that asks in the step (2) _t, y _t, z _t) ask about x respectively _t, y _t, z _tPartial derivative, simultaneous becomes equation group:

$\left\{\begin{array}{c}\frac{\&PartialD;M(x_t,y_t,z_t)}{{\&PartialD;x}_t}=0\\ \frac{\&PartialD;M(x_t,y_t,z_t)}{{\&PartialD;y}_t}=0\\ \frac{\&PartialD;M(x_t,y_t,z_t)}{{\&PartialD;z}_t}=0\end{array}\right.---\left(7\right)$

Find the solution this equation group, obtain the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) coordinate figure.

The detailed process of described trace predicating is as follows:

Set up air drag model: f=kv (8)

Wherein, k is the intrinsic parameter of shuttlecock playing, and is relevant with its volume and shape under the constant substantially situation of atmospheric density;

In addition, shuttlecock playing also is subjected to gravity,

G＝mg (9)

$\stackrel{\&RightArrow;}{a}=\frac{\stackrel{\&RightArrow;}{f}+\stackrel{\&RightArrow;}{G}}{m}---\left(10\right)$

According to the newtonian motion law, have again

$s=v_{0}t+\frac{1}{2}{\mathrm{at}}^2---\left(11\right)$

v＝v ₀+at (12)

Because shuttlecock playing is to move in three dimensions, existing motion with shuttlecock playing is decomposed into three components of axially going up motion, and comprehensive above equation finally obtains the equation of motion of shuttlecock playing track:

$\left\{\begin{array}{c}x\left(t\right)=v_{x0}t+v_{x0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+x_0\\ y\left(t\right)=v_{y0}t+{\mathrm{<figure-callout\; id="0"\; label="v\; y"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">v</figure-callout>}}_y\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+y_0\\ z\left(t\right)=v_{z0}t+(v_{\mathrm{<figure-callout\; id="0"\; label="z"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">z</figure-callout>}0}+\frac{\mathrm{mg}}{k})\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+z_0\end{array}\right.---\left(13\right)$

With the x direction is example:

$x=v_{x0}t+v_{x0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+x_0,$ It is turned to

$y=a_0(x+\frac{x^2}{-2x-\frac{2m}{k}})+a_1---\left(14\right)$

The coordinate Pt ' (x of the filtered shuttlecock playing that obtains in the integrating step three in three-dimensional cartesian coordinate system _t, y _t, z _t), carry out trace predicating with least square method, get equation group:

$\left\{\begin{array}{c}\underset{i=0}{\overset{k}{\mathrm{\&Sigma;}}}[y_i-a_0(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})-a_1]\&CenterDot;(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})=0\\ \underset{i=0}{\overset{k}{\mathrm{\&Sigma;}}}[y_i-a_0(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})-a_1]=0\end{array}\right.---\left(15\right)$

Separate this equation group, promptly get parameter a ₀, a ₁Value; In like manner, try to achieve the parameter b of y direction ₀, b ₁Value, the parameter c of z direction ₀, c ₁Value, promptly obtained estimating curve;

Make z (the t)=H in the equation (13) ₀(16)

Wherein, H ₀Be the axis of the shuttlecock playing robot racket distance apart from horizontal plane, simultaneous (13) and (16) can solve next three-dimensional coordinate P (x of drop point constantly of shuttlecock playing _P, y _P, z _P).

Beneficial effect of the present invention: shuttlecock playing of the present invention robot utilizes binocular vision system to finish collection to the Three-Dimensional Dynamic image, the image processing algorithm that utilization is optimized, the trace predicating of dynamic object are finished the space orientation to the Three-Dimensional Dynamic target, finish control by high performance motor servo control system again to plant equipment, realize the shuttlecock playing robot to shuttlecock playing continuously, stroke fast, accurately.Shuttlecock playing of the present invention robot is applicable to robot researches such as university and scientific research institutions exploitation mechanism, as the experiment porch of research directions such as machine vision, SERVO CONTROL, artificial intelligence.In addition, shuttlecock playing of the present invention robot can be used as amusement and leisure equipment, enriches people's free life.

Description of drawings:

Fig. 1 is the structural representation of the orbit part of shuttlecock playing of the present invention robot;

Fig. 2 is the structural representation of the batting part of shuttlecock playing of the present invention robot;

Fig. 3 is the flow chart of shuttlecock playing ROBOT CONTROL method of the present invention;

Fig. 4 is the schematic diagram of the position of the foundation of three-dimensional cartesian coordinate system and the coordinate points of shuttlecock playing in three-dimensional cartesian coordinate system.

Wherein, among Fig. 1,1-orbit part, the 2-long rails, 3-vertically is with synchronously, the 4-longitudinal sliding block, the laterally synchronous band of 5-cross track, 6-, 7-transverse slider, 8-are laterally controlled motor, and 9-base, 10-are vertically controlled motor;

Among Fig. 2,11-motor, the 12-part of batting, 13-base plate, 14-worm reducer, the axle of 15-worm reducer, the axle of 16-racket, 17-racket.

The specific embodiment:

A kind of shuttlecock playing robot, comprise orbit part 1, batting part 12 and vision part, as shown in Figure 1, described orbit part 1 comprises the base 9 with long rails 2, long rails 2 is provided with the longitudinal sliding block 4 that can have cross track 5 above motion on the long rails 2, and the cross track 5 of longitudinal sliding block 4 is provided with the transverse slider 7 that can move on cross track 5; Be respectively arranged with at 2 of long rails, 5 of cross tracks and vertically be with 3 synchronously, laterally be with 6 synchronously, vertically synchronously with 3, laterally synchronously with 6 respectively with vertical control motor 10 of base 9 sides, laterally control motor 8 and link to each other; Transverse slider 7 is provided with described batting part 12, and as shown in Figure 2, described batting part 12 comprises the base plate 13 that is fixed with racket 17, and the axle 16 of racket 17 links to each other with the axle 15 of the worm reducer 14 of motor 11; Described vision partly comprises two the mutually perpendicular cameras that link to each other with the shuttlecock playing robot control system that are arranged on top, shuttlecock court ground; At vertical control motor 10 of rail portion 1, laterally motor 11 inside of controlling motor 8 and batting part 12 are provided with the sensor that detects racket 17 angles and positional information.

As shown in Figure 3, described shuttlecock playing ROBOT CONTROL method comprises the steps:

Before the use, at first need system is carried out the initialization setting, comprise and adjust the camera parameter, measure shuttlecock playing size, place size, camera height, set the shuttlecock playing luminance threshold, calculate and preserve place information searching table.The shuttlecock playing robot is to being subjected to the influence of picture brightness in the treatment of picture process, same threshold value can obtain different binaryzation results under the different illumination conditions; Therefore, need adjust threshold value according to each illumination condition, the result who makes image handle can identify shuttlecock playing preferably, and then can analyze the information of shuttlecock playing, as area, center of gravity etc.

Step 1: by two mutually perpendicular two-dimensional images of two mutually perpendicular camera collections, and the two-dimensional image that collects is carried out binary conversion treatment, obtain two width of cloth bianry images by OPENCV software; The last image coordinate that extracts shuttlecock playing in two width of cloth bianry images, and the image coordinate of the shuttlecock playing that extracts carried out the correction of image radial distortion and projection distortion is converted to actual coordinate with respect to the place with the image coordinate of shuttlecock playing.

Owing to there are being these errors inevitably to exist in the middle of the reality: camera and shuttlecock playing place out of plumb, camera make projection distortion inevitably take place not directly over the shuttlecock playing place etc.In the process of setting up place information searching table, it is the most key technology that projection distortion is proofreaied and correct.In addition, the image radial distortion that wide-angle lens brought also must be proofreaied and correct.

Described image coordinate is carried out the correction of image radial distortion and projection distortion, the image coordinate of shuttlecock playing is converted to actual coordinate with respect to the place, the specific implementation process is as follows:

(1) two mutually perpendicular cameras is looked the shuttlecock playing place and demarcated respectively, vertical camera is looked the shuttlecock playing place get 12 calibration points, parallel camera is looked the shuttlecock playing place get 13 calibration points along edge in the place along edge in the place along edge in the place; With the upper left corner, place is initial point, measures the actual coordinate of calibration point; And by in image, clicking the image coordinate that calibration point draws calibration point;

(2) utilize the multinomial scaling method of radial distortion, set up actual coordinate (x _w, y _w) and image coordinate (u, v) the cubic polynomial relation between:

$\left\{\begin{array}{c}{x}_w=a_0+a_{1}v+a_2{v}^2+a_3{v}^3+a_{4}u+a_5\mathrm{uv}+a_6{\mathrm{uv}}^2+a_7{u}^2+a_8{u}^{2}v+a_9{u}^3\\ y_w={\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">b</figure-callout>}}_0+b_{1}v+b_2{v}^2+b_3{v}^3+b_{4}u+b_5\mathrm{uv}+b_6{\mathrm{uv}}^2+b_7{u}^2+b_8{u}^{2}v+b_9{u}^3\end{array}\right.---\left(1\right)$

(3), utilize least square solution overdetermined equation group according to the actual coordinate and the image coordinate of the calibration point of measuring in the step (1):

$\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]=0\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]x_{i1}=0\\ ...\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]x_{\mathrm{iq}}=0\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-({\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">b</figure-callout>}}_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]=0\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-({\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">b</figure-callout>}}_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]x_{i1}=0\\ ...\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-({\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">b</figure-callout>}}_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]x_{\mathrm{iq}}=0\end{array}\right.---\left(3\right)$

Wherein, a _i(i=0,1,2 ... 9), b _i(i=0,1,2 ... 9) be parameter, obtain a _i(i=0,1,2 ... 9), b _i(i=0,1,2 ... 9) value; The mapping relations between image coordinate and the actual coordinate have promptly been set up;

(4) according to image coordinate of setting up in the step (3) and the mapping relations between the actual coordinate, and the image coordinate of the shuttlecock playing that in two width of cloth bianry images, extracts in the step 1, obtain the actual coordinate of shuttlecock playing with respect to the shuttlecock playing place.

Step 2: as shown in Figure 4, set up three-dimensional cartesian coordinate system, in three-dimensional cartesian coordinate system, determine the actual coordinate P of shuttlecock playing in horizontal bianry image according to two mutually perpendicular bianry images ₁(x ₁, y ₁, 0) and the actual coordinate P of shuttlecock playing in vertical bianry image ₂(0, y ₂, z ₂); Record the actual coordinate Pc of vertical camera in three-dimensional cartesian coordinate system ₁(x _C1, y _C1, z _C1) and the actual coordinate Pc of horizontal camera in three-dimensional cartesian coordinate system ₂(x _C2, y _C2, z _C2); Connect P ₁With Pc ₁, obtain space line L ₁, connect P ₂With Pc ₂, obtain space line L ₂, space line L ₁, L ₂The mid point Pt (x of common vertical line section _t, y _t, z _t) be the coordinate of shuttlecock playing in three-dimensional cartesian coordinate system.

Coordinate Pt (the x of described shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) concrete computational process as follows:

(1) asks for space line L ₁, L ₂Equation:

Space line L ₁:

$\frac{x-x_1}{x_{c1}-x_1}=\frac{y-y_1}{y_{c1}-y_1}=\frac{z-z_1}{z_{c1}-z_1}---\left(4\right)$

Space line L ₂:

$\frac{x-x_2}{x_{c2}-x_2}=\frac{y-y_2}{y_{c2}-y_2}=\frac{z-z_2}{z_{c2}-z_2}---\left(5\right)$

(2) with square distance be asking for of object function:

$M(x_t,y_t,z_t)=d_1^2(x_t,y_t,z_t)+d_2^2(x_t,y_t,z_t)---\left(6\right)$

Wherein, M (x _t, y _t, z _t) be object function, d ₁, d ₂Be respectively the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) apart from space line L ₁, L ₂Distance;

(3) the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) try to achieve:

Object function M (the x that asks in the step (2) _t, y _t, z _t) pairing x when being minimum of a value _t, y _t, z _tRepresent such point:

1. he is on the common vertical line of two space line L1, L2; 2. he equates apart from the distance of two space line L1, L2; Be the mid point of space line L1, L2 common vertical line section, also be the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t).To the object function M (x that asks in the step (2) _t, y _t, z _t) ask about x respectively _t, y _t, z _tPartial derivative, simultaneous becomes equation group:

$\left\{\begin{array}{c}\frac{\&PartialD;M(x_t,y_t,z_t)}{{\&PartialD;x}_t}=0\\ \frac{\&PartialD;M(x_t,y_t,z_t)}{{\&PartialD;y}_t}=0\\ \frac{\&PartialD;M(x_t,y_t,z_t)}{{\&PartialD;z}_t}=0\end{array}\right.---\left(7\right)$

Find the solution this equation group, obtain the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) coordinate figure.

Step 3: to the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system that tries to achieve in the step 2 _t, y _t, z _t) carry out Kalman filtering, obtain the coordinate Pt ' (x of a filtered shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t).

Because shuttlecock playing belongs to irregularly shaped, thereby its visual identity is difficult to disturbance and error not occur with the location, and the present invention adopts Kalman filtering that the coordinate of shuttlecock playing in three-dimensional cartesian coordinate system carried out filtering to handle at this problem.

Step 4: the orbiting motion equation that obtains shuttlecock playing by mechanical analysis; The coordinate Pt ' (x of the filtered shuttlecock playing that obtains in the integrating step three in three-dimensional cartesian coordinate system again _t, y _t, z _t), carry out trace predicating with least square method; Estimate the three-dimensional coordinate P (x that obtains next moment drop point of shuttlecock playing _P, y _P, z _P), and the coordinate input signal control system that will put.

The detailed process of described trace predicating is as follows:

Through repeatedly experimental verification, it is comparatively desirable adopting formula (8) to describe the air drag model:

f＝kv (8)

Wherein, k is the intrinsic parameter of shuttlecock playing, and is relevant with its volume and shape under the constant substantially situation of atmospheric density;

In addition, shuttlecock playing also is subjected to gravity,

G＝mg (9)

$\stackrel{\&RightArrow;}{a}=\frac{\stackrel{\&RightArrow;}{f}+\stackrel{\&RightArrow;}{G}}{m}---\left(10\right)$

According to the newtonian motion law, have again

$s=v_{0}t+\frac{1}{2}{\mathrm{at}}^2---\left(11\right)$

v＝v ₀+at (12)

Because shuttlecock playing is to move in three dimensions, existing motion with shuttlecock playing is decomposed into three components of axially going up motion, and comprehensive above equation finally obtains the equation of motion of shuttlecock playing track:

$\left\{\begin{array}{c}x\left(t\right)=v_{x0}t+v_{x0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+x_0\\ y\left(t\right)=v_{y0}t+{\mathrm{<figure-callout\; id="0"\; label="v\; y"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">v</figure-callout>}}_y\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+y_0\\ z\left(t\right)=v_{z0}t+({\mathrm{<figure-callout\; id="0"\; label="v\; z"\; filenames="H200910011919XE00041.png"\; state="\{\{state\}\}">v</figure-callout>}}_z+\frac{\mathrm{mg}}{k})\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+z_0\end{array}\right.---\left(13\right)$

With the x direction is example:

$x=v_{x0}t+v_{x0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+x_0,$ It is turned to

$y=a_0(x+\frac{x^2}{-2x-\frac{2m}{k}})+a_1---\left(14\right)$

The coordinate Pt ' (x of the filtered shuttlecock playing that obtains in the integrating step three in three-dimensional cartesian coordinate system _t, y _t, z _t), carry out trace predicating with least square method, get equation group:

$\left\{\begin{array}{c}\underset{i=0}{\overset{k}{\mathrm{\&Sigma;}}}[y_i-a_0(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})-a_1]\&CenterDot;(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})=0\\ \underset{i=0}{\overset{k}{\mathrm{\&Sigma;}}}[y_i-a_0(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})-a_1]=0\end{array}\right.---\left(15\right)$

Separate this equation group, promptly get parameter a ₀, a ₁Value; In like manner, try to achieve the parameter b of y direction ₀, b ₁Value, the parameter c of z direction ₀, c ₁Value, promptly obtained estimating curve.

Make z (the t)=H in the equation (13) ₀(16)

Wherein, H ₀Be the axis of the shuttlecock playing robot racket distance apart from horizontal plane, simultaneous (13) and (16) can solve next three-dimensional coordinate P (x of drop point constantly of shuttlecock playing _P, y _P, z _P).

Step 5: by being arranged on the sensor of rail portion and batting motor internal partly, detect the angle information and the positional information of racket, and with voltage signal input signal control system; Next three-dimensional coordinate P (x of drop point constantly of the shuttlecock playing that whistle control system obtains in the integrating step four again _P, y _P, z _P), calculate the amount of exercise of shuttlecock playing robot, and, finish the shuttlecock playing robot along the operation of track and the action of batting by motor servo control system control motor; Promptly reach laterally control driven by motor batting partly at the horizontal rail componental movement, then control the motor of batting part again and carry out shot by vertical control motor.

The motor servo control system of shuttlecock robot of the present invention adopts Mitsubishi Electric's servo-control system, because this system has high accuracy, high-speed, can satisfy the requirement that the shuttlecock robot movement velocity is fast, the location is accurate. The controller of this system adopts Q series, more high-performance, miniaturization, thus realize motion control at a high speed, possess the motion control functions such as multi-axis interpolation, speed control, Software Convex Cam location, TRAJECTORY CONTROL.

Claims (5)

1. shuttlecock playing robot, it is characterized in that, comprise orbit part (1), batting part (12) and vision part, described orbit part (1) comprises the have long rails base (9) of (2), long rails (2) is provided with and can goes up the longitudinal sliding block (4) that the top of moving has cross track (5) at long rails (2), and the cross track (5) of longitudinal sliding block (4) is provided with can go up the transverse slider (7) of motion at cross track (5); Between long rails (2), be respectively arranged with vertically synchronously band (3), horizontal band (6) synchronously between cross track (5), vertically synchronously band (3), laterally synchronously band (6) respectively with vertical control motor (10) of base (9) side, laterally control motor (8) and link to each other; Transverse slider (7) is provided with described batting part (12), and described batting part (12) comprises the base plate (13) that is fixed with racket (17), and the axle (16) of racket (17) links to each other with the axle (15) of the worm reducer (14) of motor (11); Described vision partly comprises two the mutually perpendicular cameras that link to each other with the shuttlecock playing robot control system that are arranged on top, shuttlecock court ground; At vertical control motor (10) of described orbit part (1), laterally motor (11) inside of controlling motor (8) and batting part (12) is provided with the sensor that detects racket (17) angle and positional information.

2. be used to control shuttlecock playing ROBOT CONTROL method as claimed in claim 1, it is characterized in that, comprise the steps:

Step 1: by two mutually perpendicular two-dimensional images of two mutually perpendicular camera collections, and the two-dimensional image that collects is carried out binary conversion treatment, obtain two width of cloth bianry images by OPENCV software; The last image coordinate that extracts shuttlecock playing in two width of cloth bianry images, and the image coordinate of the shuttlecock playing that extracts carried out the correction of image radial distortion and projection distortion is converted to actual coordinate with respect to the place with the image coordinate of shuttlecock playing;

Step 2: set up three-dimensional cartesian coordinate system according to two mutually perpendicular bianry images, in three-dimensional cartesian coordinate system, determine the actual coordinate P of shuttlecock playing in horizontal bianry image ₁(x ₁, y ₁, 0) and the actual coordinate P of shuttlecock playing in vertical bianry image ₂(0, y ₂, z ₂); Record the actual coordinate Pc of vertical camera in three-dimensional cartesian coordinate system ₁(x _C1, y _C1, z _C1) and the actual coordinate Pc of horizontal camera in three-dimensional cartesian coordinate system ₂(x _C2, y _C2, z _C2); Connect P ₁With Pc ₁, obtain space line L ₁, connect P ₂With Pc ₂, obtain space line L ₂, space line L ₁, L ₂The mid point Pt (x of common vertical line section _t, y _t, z _t) be the coordinate of shuttlecock playing in three-dimensional cartesian coordinate system;

Step 3: to the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system that tries to achieve in the step 2 _t, y _t, z _t) carry out Kalman filtering, obtain the coordinate Pt ' (x of a filtered shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t);

Step 4: the orbiting motion equation that obtains shuttlecock playing by mechanical analysis; The coordinate Pt ' (x of the filtered shuttlecock playing that obtains in the integrating step three in three-dimensional cartesian coordinate system again _t, y _t, z _t), carry out trace predicating with least square method; Estimate the three-dimensional coordinate P (x that obtains next moment drop point of shuttlecock playing _P, y _P, z _P), and the coordinate input signal control system that will put;

Step 5: by being arranged on the sensor of rail portion and batting motor internal partly, detect the angle information and the positional information of racket, and with voltage signal input signal control system; Next three-dimensional coordinate P (x of drop point constantly of the shuttlecock playing that whistle control system obtains in the integrating step four again _P, y _P, z _P), calculate the amount of exercise of shuttlecock playing robot, and, finish the shuttlecock playing robot along the operation of track and the action of batting by motor servo control system control motor; Promptly reach laterally control driven by motor batting partly at the horizontal rail componental movement, then control the motor of batting part again and carry out shot by vertical control motor.

3. control method according to claim 2 is characterized in that, described image coordinate is carried out the correction of image radial distortion and projection distortion, and the image coordinate of shuttlecock playing is converted to actual coordinate with respect to the place, and the specific implementation process is as follows:

(1) two mutually perpendicular cameras being looked the shuttlecock playing place and demarcated respectively along edge in the place, is initial point with the upper left corner, place, measures the actual coordinate of calibration point; And by in image, clicking the image coordinate that calibration point draws calibration point;

(2) utilize the multinomial scaling method of radial distortion, set up actual coordinate (x _w, y _w) and image coordinate (u, v) the cubic polynomial relation between:

$\left\{\begin{array}{c}{x}_w=a_0+a_{1}v+a_2{v}^2+a_3{v}^3+a_{4}u+a_5\mathrm{uv}+a_6{\mathrm{uv}}^2+a_7{u}^2+a_8{u}^{2}v+a_9{u}^3\\ y_w=b_0+b_{1}v+b_2{v}^2+b_3{v}^3+b_{4}u+b_5\mathrm{uv}+b_6{\mathrm{uv}}^2+b_7{u}^2+b_8{u}^{2}v+b_9{u}^3\end{array}\right.---\left(1\right)$

(3), utilize least square solution overdetermined equation group according to the actual coordinate and the image coordinate of the calibration point of measuring in the step (1):

$\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]=0\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]x_{i1}=0\\ ...\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(a_0+a_1{x}_{i1}+...+a_q{x}_{\mathrm{iq}})]x_{\mathrm{iq}}=0\end{array}\right.---\left(2\right)$

$\left\{\begin{array}{c}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(b_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]=0\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(b_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]x_{i1}=0\\ ...\\ \underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}[y_i-(b_0+b_1{x}_{i1}+...+b_q{x}_{\mathrm{iq}})]x_{\mathrm{iq}}=0\end{array}\right.---\left(3\right)$

Wherein, a _i(i=0,1,2 ... 9), b _i(i=0,1,2 ... 9) be parameter, obtain a _i(i=0,1,2 ... 9), b _i(i=0,1,2 ... 9) value; The mapping relations between image coordinate and the actual coordinate have promptly been set up;

(4) according to image coordinate of setting up in the step (3) and the mapping relations between the actual coordinate, and the image coordinate of the shuttlecock playing that in two width of cloth bianry images, extracts in the step 1, obtain the actual coordinate of shuttlecock playing with respect to the shuttlecock playing place.

4. control method according to claim 2 is characterized in that, the coordinate Pt (x of described shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) concrete computational process as follows:

(1) asks for space line L ₁, L ₂Equation:

Space line L ₁:

$\frac{x-x_1}{x_{c1}-x_1}=\frac{y-y_1}{y_{c1}-y_1}=\frac{z-z_1}{z_{c1}-z_1}---\left(4\right)$

Space line L ₂:

$\frac{x-x_2}{x_{c2}-x_2}=\frac{y-y_2}{y_{c2}-y_2}=\frac{z-z_2}{z_{c2}-z_2}---\left(5\right)$

(2) with square distance be asking for of object function:

$M(x_t,y_t,z_t)=d_1^2(x_t,y_t,z_t)+d_2^2(x_t,y_t,z_t)---\left(6\right)$

Wherein, M (x _t, y _t, z _t) be object function, d ₁, d ₂Be respectively the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) apart from space line L ₁, L ₂Distance;

(3) the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) try to achieve:

To the object function M (x that asks in the step (2) _t, y _t, z _t) ask about x respectively _t, y _t, z _tPartial derivative, simultaneous becomes equation group:

$\left\{\begin{array}{c}\frac{\&PartialD;M(x_t,y_t,z_t)}{\&PartialD;x_t}=0\\ \frac{\&PartialD;M(x_t,y_t,z_t)}{\&PartialD;y_t}=0\\ \frac{\&PartialD;M(x_t,y_t,z_t)}{\&PartialD;z_t}=0\end{array}\right.---\left(7\right)$

Find the solution this equation group, obtain the coordinate Pt (x of shuttlecock playing in three-dimensional cartesian coordinate system _t, y _t, z _t) coordinate figure.

5. control method according to claim 2 is characterized in that, the detailed process of described trace predicating is as follows:

Set up air drag model: f=kv (8)

Wherein, k is the intrinsic parameter of shuttlecock playing, and is relevant with its volume and shape under the constant substantially situation of atmospheric density;

In addition, shuttlecock playing also is subjected to gravity,

G＝mg (9)

$\stackrel{\&RightArrow;}{a}=\frac{\stackrel{\&RightArrow;}{f}+\stackrel{\&RightArrow;}{G}}{m}---\left(10\right)$

According to the newtonian motion law, have again

$s=v_{0}t+\frac{1}{2}{\mathrm{at}}^2---\left(11\right)$

v＝v ₀+at (12)

Because shuttlecock playing is to move in three dimensions, existing motion with shuttlecock playing is decomposed into three components of axially going up motion, and comprehensive above equation finally obtains the equation of motion of shuttlecock playing track:

$\left\{\begin{array}{c}x\left(t\right)=v_{x0}t+v_{x0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+x_0\\ y\left(t\right)=v_{y0}t+v_{y0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+y_0\\ z\left(t\right)=v_{z0}t+(v_{z0}+\frac{\mathrm{mg}}{k})\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+z_0\end{array}\right.---\left(13\right)$

With the x direction is example:

$x=v_{x0}t+v_{x0}\&CenterDot;\frac{t^2}{-2t-\frac{2m}{k}}+x_0,$ It is turned to

$y=a_0(x+\frac{x^2}{-2x-\frac{2m}{k}})+a_1---\left(14\right)$

The coordinate Pt ' (x of the filtered shuttlecock playing that obtains in the integrating step three in three-dimensional cartesian coordinate system _t, y _t, z _t), carry out trace predicating with least square method, get equation group:

$\left\{\begin{array}{c}\underset{i=0}{\overset{k}{\mathrm{\&Sigma;}}}[y_i-a_0(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})-a_1]\&CenterDot;(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})=0\\ \underset{i=0}{\overset{k}{\mathrm{\&Sigma;}}}[y_i-a_0(x_i+\frac{{x_i}^2}{-2x_i-\frac{2m}{k}})-a_1]=0\end{array}\right.---\left(15\right)$

Separate this equation group, promptly get parameter a ₀, a ₁Value; In like manner, try to achieve the parameter b of y direction ₀, b ₁Value, the parameter c of z direction ₀, c ₁Value, promptly obtained estimating curve;

Make z (the t)=H in the equation (13) ₀(16)

Wherein, H ₀Be the axis of the shuttlecock playing robot racket distance apart from horizontal plane, simultaneous (13) and (16) can solve next three-dimensional coordinate P (x of drop point constantly of shuttlecock playing _P, y _P, z _P).

Images