July1,2012 [AUTOMATICURRETSCORPIO]2.......................................................................................................................................21...........................................................................................................................................41.1....................................................................................................................41.1.1.......................................................................................................41.1.297UniversalTurret.....................................................................................................62............................................................................................................72.1...............................................................72.1.1................................................................................................72.1.2...................................................................................................82.1.3......................................................................................................................82.1.4....................................................................................................82.1.5...........................................................................................................82.1.6.................................................................................................92.1.7/........92.1.8...............................................................................92.2(Modules)....................................................................................112.2.1................................................................................................112.2.2LEGONXT(IntelligentBrick)....................................................112.2.3()()........................................................................................................................122.2.4()()...............................................................................................................................................132.2.5().....................................................................................................132.2.6..........................................................................................142.3..................................................................152.3.1.......................................................................................................152.3.2()...........................................172.4....................................................................................................................193.....................................................................................................................203.1OpenSourceComputerVisionLibrary.................................................................................203.1.1.............................................................................................213.1.2Gaussian(Smoothing)................................................................................22[AUTOMATICURRETSCORPIO] July1,201233.1.3..................................................................................233.1.4...............................................................................................243.1.5/................................263.2/........................................................................................................273.2.1FPS..................................................................................274...................................................294.1.....................................................................................................294.2......................................................................................................304.3.........................................................................................324.4.....................................................................................................344.5.....................................................................................365....................................................425.1............................................................................................425.1.1DC....................................................................................................425.1.2............................................................................................435.1.3..............................................................................................465.2....................................................................................475.2.1...............................................................................................................475.2.2.....................................................................................496................................................................................................516.1..........................................................................................516.2...........................................................................................52LegoNXTIntelligentBrick........................................................53B....................................................................................54........................................................................................55.................................................................................................................55........................................................................................................................................55C++.........................................................................................................56Matlab.....................................................................................................62....................................................................................................................................67 July1,2012 [AUTOMATICURRETSCORPIO]41. . . ,, (smart projectiles/missiles). , .. electromagnetic mass drivers () laser (HELLADS), [18] [19]. , DARPA (Defense Advanced Research Projects Agency), (target recognition) (tracking). , , (.. , .). , , (.. ) IFF(FriendFoeIdentification),.1.11.1.1 , . [20].399.X. , .(Scorpio),,,[25]. . . , , . , , , , , . [AUTOMATICURRETSCORPIO] July1,20125,().1:Scorpio[25]. , , , tormenta. , , 1000..(),(trebuchet) , (). , 60 , . : , 100 . Avaricum , .,,400,(34),. .July1,2012 [AUTOMATICURRETSCORPIO]61.1.297UniversalTurret AH1SUpgunnedCobra[1][26].201973(XM18830).110,21,50,80 60 . , .2:XM97UniversalTurret[26]. [AUTOMATICURRETSCORPIO] July1,201272.2.1 (, ) LEGO. (riskanalysis), , LEGO. .:1. .2. (Drafting).3. .4. .5. .6. .7. /.8. (GOTOSTEP5)(END),, . ,(a:,b:LEGO).2.1.1a. ,,. . .b. LEGO, ABS(acrylonitrilebutadienestyrene). . .. .. PCHPPaviliondv51080ev.July1,2012 [AUTOMATICURRETSCORPIO]82.1.2a. , . CADCAMCNC.b. LEGO DIGITAL DESIGNER, .,modular LEGO, , .2.1.3a. , . (microcontroller), internet, ,.b. LEGO MINDSTORMS NXT 2.0, .. .2.1.4a. . ..b. LEGO . on the go. . , . ().2.1.5a. . . . , .b. LEGO , . , . [AUTOMATICURRETSCORPIO] July1,20129 (.. ) , .2.1.6a. .b. , LEGO, . , ,,.,, , . , ,.2.1.7/a. ..b. . onthego.2.1.8a. .b. .,, , .,risk analysis, LEGO.July1,2012 [AUTOMATICURRETSCORPIO]10()LEGO()1 5 62 4 83 3 104 5 75 4 86 6 57 4 98 5 7 36 601:. , , LEGO,. [AUTOMATICURRETSCORPIO] July1,2012112.2(Modules)3:SCORPIOMKIIRobot.6modules(. 3), , 3 LEGO ( ).(),,.2.2.1. St37 5 , . , . .2.2.2LEGONXT(IntelligentBrick) (. 4). LEGONXT.[27].July1,2012 [AUTOMATICURRETSCORPIO]124:LegoNXTIntelligentBrick.2.2.3()()module,LEGO [2]. . B[22][23]. / /(gearbox)nA=1/2.1[21]. 11.5 Ncm 1=35cm1=39.5rpm[22]., . (tracking).5:q1.[AUTOMATICURRETSCORPIO] July1,2012132.2.4()() module , LEGO [2].,. / / (gearbox) nB= 1/5 [21]. 2=83.5cm 2=16.4rpm[22]. , , /,. , ,,.6:q2.2.2.5() 7:. module LEGO NXT [2]. , Oring , [4]. ., 900 , /. . July1,2012 [AUTOMATICURRETSCORPIO]14 . nC=1/3 /[21].8:.2.2.6.9:PhilipsNotebookCameraSPZ6500/27. (.9) , . [28]. , . , ( ) 3.[AUTOMATICURRETSCORPIO] July1,201215PC,. , , , laser, (triangulation) ,[5][6].2.3 , , . :- .- , LEGO, . .2.3.1:- LEGO [3]. , . .,. .- [3]. , ,(),.- [3]. , , . , .- . . July1,2012 [AUTOMATICURRETSCORPIO]16 . . , , , .10:. (), .11:. () . . .[AUTOMATICURRETSCORPIO] July1,201217 .2.3.2():- . , . , /,[3].- . . . ,.- ..- .- .: (3).... .TO12:.July1,2012 [AUTOMATICURRETSCORPIO]18 , (). module , . . () . (elbowupelbowdown). () . 13:q2(LEGODIGITALDESIGNER).[AUTOMATICURRETSCORPIO] July1,2012192.4 q1 q2 , LEGO . [8][14][22]:14:q1&q2. July1,2012 [AUTOMATICURRETSCORPIO]203.3.1OpenSourceComputerVisionLibraryOpenCVC++ . BSD1 . penCV :1. /.Matlab . , C++ . netbook Raspberry . OpenCV , Intel, ( ) . OpenCV .2. Bluetooth/ . C++ /.3. , Windows 7, Raspberry Debian Linux. HOpenCV (Windows, Linux, Mac OS, ). . .4. OpenCV 45.000 [29]. , (.. ProjectCyclops, Stanley) [30][31]. OpenCV, .1BerkeleySoftwareDistribution.[AUTOMATICURRETSCORPIO] July1,2012215. H OpenCV . 2.4 500 .15:[11].3.1.1 webcam . RGB.ToRGB, . . RGB , (Red), (Green), (Blue). , , , RGB.16:RGB[34].July1,2012 [AUTOMATICURRETSCORPIO]22 v HSV. HSV(Hue,H),(Saturation,S),(Value,V) [33] [35]. HSV, 17, RGB , HSV (hue saturation) . HSV .17:HSV[35]., . 17 ue saturation. , , ().3.1.2Gaussian(Smoothing) . /pixel,15,Gauss. Gauss . , ().Gauss[12]:2 22221( , )2x yG x y e oto +=(i)[AUTOMATICURRETSCORPIO] July1,201223(standarddeviation).,,,.3.1.3,RGBHSVGaussian.Gaussian:cvSmooth(thresholded, thresholded, CV_GAUSSIAN,9, 9); 3.1.3.1cvSmooth.:void cvSmooth ( const CvArr* src, CvArr* dst, int smoothtype = CV_GAUSSIAN, int param1=9, int param2=9, double param3=0, double param4=0 ); 18:cvHoughCircles. cvSmooth src dst.smoothtypeCV_GAUSSIAN.4[32]:1. Param1:.2. Param2:.3. Param3: Gaussian ( ). :0.3 0.8, 12xx xnn param o = + =0.3 0.8, 22yy ynn param o = + =(ii)4. Param4 : to , x = param3 & y =param4.To . .Gaussian19.July1,2012 [AUTOMATICURRETSCORPIO]243.1.4 . o21,21.19:Gaussian[11]. (thresholding). . , . , web camera, (thresholded) . cvHoughCircles().3.1.3.2cvHoughCircles.:CvSeq* CV_HOUGH_GRADIENT ( CvArr* image, void* circle_storage, int method, [AUTOMATICURRETSCORPIO] July1,201225 double dp, double min_dist, double param1, double param2, int min_radius, int max_radius ); 20:cvHoughCircles. 8bit.()()21:()A(). max_radius min_radius. CV_HOUGH_GRADIENT2.3.1 OpenCV. , min_dist . , . , 2/3 ( pixel) . , cvHoughCircles .22:.July1,2012 [AUTOMATICURRETSCORPIO]2623:SearchForTargetPixels/Color(.37).3.1.5/.37frame,, . 23 . , . OpenCV . , web ( frame) (..encoders) Bluetooth.StartInitializeMotorsA,B,CWebcam,Bluetooth,kCaptureFrameRGBtoHSVCvHoughCirclesCenterandradiusoftargetCircleexistsNoContinuesasinshape36[]YesSmoothingThresholding[AUTOMATICURRETSCORPIO] July1,2012273.2/.: Lumen (: lm) (). SI . . ,.To Lumen Candela. lm = cd X sr ( sr = solidangle=). Candela(:cd)().SI. , .candela.cd=lm/sr(sr=solidangle=). Lux (: lx) (). SI (International System)(). , , ,.lux=lm/m. lumen lux lux .,1000lumen1m21000lux,1lux=1lumen/m2. S. 100Watt1750lux. web lux . lux. , web,.3.2.1FPS webfps.,., , . , .,,July1,2012 [AUTOMATICURRETSCORPIO]28 . , ,encoders. web cameras , , (. 38), software(.24),.Tosoftware YouCamv5.0Cyberlink. CamSuiteControlPhilips.:24:MSoftware. fps 30fps. contrast . , . [AUTOMATICURRETSCORPIO] July1,2012294.4.1 SCORPIO MKII, DenavitHartenberg. (Direct Geometric Model). , :DenavitHartenbergTableLINK di i ai 1 l1+l2 q1 0 /22 0 q2 0 /2E l3 0 0 02:DenavitHartenbergSCORPIOMKII., (zero configuration) SCORPIOMII:25:(ZeroConfiguration)SCORPIOMKII.July1,2012 [AUTOMATICURRETSCORPIO]30, (i1)i[7]:1cos sin cos sin sin cossin cos cos cos sin sin( )0 sin cos0 0 0 1i i i i i i ii i i i i i i ii ii i ia a aa a aA qa a du u u uu u u u | | | |= | |\ .(1),:1 2 1 1 2 3 1 22 1 1 1 2 3 1 2 02 2 1 2 3 2( )00 0 0 1Ec c s c s l c sc s c s s l s sA qs c l l l c | | | |= |+ |\ .(2): ( )1 2, q q q =:..cosi ic q = , sini is q = ,i=1,2.4.2 (Direct Kinematic Model), Jacobian . (endeffector),:3 1 2 Xp l c s = 3 1 2 Yp l s s = 1 2 3 2( )Zp l l l c = + + (3)HJacobian:( ) XYZJJ q JJ ( (= ( ( (4):1 2, , X X q X qJ p p ( = ,1 2, , Y Y q Y qJ p p ( = ,1 2, , Z Z q Z qJ p p ( = ,(3)(4),Jacobian:3 1 2 3 1 23 1 2 3 1 23 2( )0l s s l c cJ q l c s l s cl s ( (= ( ( (5)[AUTOMATICURRETSCORPIO] July1,201231Jacobian . , , Jacobian, .:( )eeepJ q q ue (= = ( (6)HJacobian:(7) :( )1 1( )iP i E iJ q z p p = (8)1( )iO iJ q z =(9)block,:11 21 28( ) 80Ps sJ q c s ( (= ( ( ,21 22 128( ) 88Pc cJ q c ss ( (= ( ( ,(10)1 0( ) [0 0 1]TOJ q z = =,2 1 1 1( ) [ 0]TOJ q z s c = = ,(7),(8),(9),(10)Jacobian:3 1 2 3 1 23 1 2 3 1 23 2110( )001 0l s s l c cl c s l s cl sJ qsc ( ( ( ( = ( ( ( ( (11)1 21 2 6 2( ) ( )( )( ) ( )P PO OJ q J qJ qJ q J q ( (= ( ( July1,2012 [AUTOMATICURRETSCORPIO]32 Jacobian , . , ,...,xz, z . , (reduced)Jacobian:3 1 2 3 1 21( ) 01 0redl s s l c cJ q s ( (= ( ( (12) , , 22 block redJ (12), 1 2q q kt = = , k e .4.3 (q1,q2) . , (kinematic simulation) SCORPIO MKII, . , , , (DC ) . , , :1 11 1 01 11 20 00 0( )0 1 00 0 0 1c ss cA ql l ( ( (= ( + ( ,1 2 1 1 22 1 1 1 2 0 0 12 1 1 2 22 2 1 200( ) ( ) ( )00 0 0 1c c s c sc s c s sA q A q A qs c l l ( ( (= = ( + ( (13) :[AUTOMATICURRETSCORPIO] July1,2012330( )0 0 0 1x x x xy y y yEz z z zn o a pn o a pA qn o a p| | | |= | |\ . (14) (decoupling) , , (.26).26:SCORPIOMII.,:*3 2 10X Xp p l s c = = + *3 2 10Y Yp p l s s = = + *1 2 3 2 Z Zp l l p l c = + = (15) (15), SCORPIOII:1 tan xypq Arcp| |= | |\ . (16)1 223( )cos zp l lq Arcl| | += |\ . (17)July1,2012 [AUTOMATICURRETSCORPIO]34 1: , SCORPIO MII, . 2: O , (resolution) () turret(.5.2.2). ( ) , . , (triggermodule).4.42.., MATLAB, . , xy() x Oy . (links). , , ., (. (16), (17)). :% Generalized Joint Displacement Vectors Calculation (Inverse Kinematics) q1=atan((dpy(:)./dpx(:))); % q2=asin((dpz(:)-l(1)-l(2)-axy/tan(phi)-az)/l(3)); , (. 27, 28, 29). , , ...[AUTOMATICURRETSCORPIO] July1,20123527:SCORPIOMII.28:SCORPIOMII().July1,2012 [AUTOMATICURRETSCORPIO]3629:SCORPIOMKII(). :3:SCORPIOMII. , (l1,l2,l3)=(11.5,9,18)cm., (configuration space). ,E.4.5 , .. [AUTOMATICURRETSCORPIO] July1,201237 . , , (q1, q2). , o (triggering actuator) , . , , 2 . To on , 3 DC ().OpenCV:motorC->on(-80,2400); //Full Rotation 2.2.5. 1(MotorADOFq1).AT 1T 1 . In ,IIn ,IIIn ,IVn .[14][15][16]:1 11 1m mA mT T n T = = (18) 1 11 2m mo mT T n T = = (19):1 1IIInnn= > :12 1IVIIInnn= > :2.,(18)(19)Aq1:1 A AT n T = (20): 1II IVAI IIIn nnn n= > :(). eqJ B ,(.36).CoulombfT ,q1:2 , , ,, , , . . , (bias) . , .July1,2012 [AUTOMATICURRETSCORPIO]38(1) (1)1 1 1 eq fT J q B q T = + + (21), (20) (21), 1q , 1 :( )(1) (1)1 1 I IIIeq A fII IVn nJ q B q T Tn n+ = (22): (q1 ) q1 . q2 .,.(4barlinkage)[13].SCORPIOMKII,.30:4(4BarLinkage)[13].,1.., 2 . 2 . :1 11cm = ,2 8cm = ,3 12cm = ,4 7cm = 0.6 r cm = (. 31). , , 3 , :01 3 u = ,02 105 u = ,03 117 u = ,04 111 u = .mu .[AUTOMATICURRETSCORPIO] July1,20123931:2SCORPIOMKII. mu . :( )1 1(1 cos ) cos , sin sinm m m mr r r u u u uIA = + + (23)1 (.32).32:.,21u :02 105 u = 0 021 1(90 ) 105 u u + = 021 18 u = (24) 2 ( ). ( , ) x yI II , ( , ) x yA AA 5 5 10 10 5510July1,2012 [AUTOMATICURRETSCORPIO]40( , ) x yI I' ' ' I , ( , ) x yA A' ' ' A . (23)(24):22 ( ) ( )2 2y y x xA I A I' ' ' ' = + ( ) ( )2 2y y y y x x x xA A I I A A I I= + A A + + A A ( ) ( )2 22 21 2 21cos sin y y x x u uA I A I= + A A + + A A 22 ( )( )22 21 3 2 122 21 3 2 1cos sin sin sinsin (1 cos ) (1 cos ) cosm mm mq rq ru u uu u u= + ++ (25)mu 2q . 2 2( ) q q u = , SCORPIOMKII(.33).33:2...:32 q2 (.5.1.3)., 2 2( ) q q u = . BT 2T 2q . ( ), [13][17]:in outW W = 220 0qBd duu uAT = T} } 40200204060800 20 40 60 80q2(degrees)(degrees)q2=q2()[AUTOMATICURRETSCORPIO] July1,20124122( )( )B BnquuuTT = (26) Bn .:1VIBVnnn= > (27) 3 .upm d 2q . , (2)eqJ ,(2)eqB , : ( )(2) (2)2 2 Veq eq B f upVInJ q B q T T m g dn+ = (28) July1,2012 [AUTOMATICURRETSCORPIO]425.5.1 SCORPIO MII, . , , ( . 3), a priori . , (revolver), . , 2D ( ), (calibration) , . , e(t) , (tolerance) PD , (torque),,DC.5.1.1DCDC[13][14][16]:am a a adiV i R L edt= + + (29)a Te k e =(30)T aT k i = (31)fT J B T e e = + + (32)34:DC.[AUTOMATICURRETSCORPIO] July1,201243 (29) (32), DC . Vm , e (EMF) , kT , B , J (. 34). , R, L ( ). adiLdt , , aelaLRt = 1 10 () JBt = .., (31) (32), 1 , i[8][9][14]:curt e e + = A (33):cur T a fk i T A = ,JBt = , , Vm:*voltt e e + = A (34):2T m f avoltT ak V T Rk BRA =+ ,*2 aT aJRk BRt =+ : , .. 1Tk .5.1.2( )e(t). . (integral guy), (tolerance). , :July1,2012 [AUTOMATICURRETSCORPIO]44( )( ) ( )P Dde tu t K e t Kdt= + (35):PK :(proportionalgain)DK :(differentialgain) , (feedback loop) . , ( ), (.. , CPU), DK . , .:torquey=error_cur_y*Kp*1.5+ (error_cur_y - error_pre_y)*Kd; torquex=error_cur_x*Kp*scale+ (error_cur_x - error_pre_x)*Kd*scale; , 1.5 scale. 1.5 , scale ( PK DK ) . y 1 scale > .PK DK .2 ZieglerNichols PD. , 1(sigmoidal) . 2 . , , ,PcrKcr.,[9][15][16]., , [AUTOMATICURRETSCORPIO] July1,201245 , ad hoc (gaintuning). , ,. 4 (2 ) , PK . , :PK =0.1 DK =0.19* PK . (). (control block diagram) (.35).35:PD., (, , ). () , .,, , , . ,PK ,,,., , SamsungGalaxySII,. , (.4).July1,2012 [AUTOMATICURRETSCORPIO]46#lux KP KD130 0.11 0.0209100 0.09 0.017180 0.06 0.01144:(lux)PD.5.1.3 LEGO MINDSTORMS NXT 2.0 encoders, . SCORPIOMII, encoder , (operational region). , (, , ) . , (endeffector) 1 2 13 32.5( , ) , ,2 2 100 100q q t t t t ( (e ( ( .,encoderDC:if ((rotationy-160)&&(abs(rotationx)total; i++) { float* p = (float*)cvGetSeqElem( circles, i ); [AUTOMATICURRETSCORPIO] July1,201259 targetx = p[0]+5*p[2]/6; // adjust target's center (x axis) targety = 240-p[1]-7*p[2]/10; // adjust target's center (y axis) radius=p[2]; //radius of the target error_cur_y=abs(150-targety); //adjust current error having in mind that //the camero is a few cm lower that the turret (y axis) error_cur_x=abs(195-targetx); //adjust current error having in mind that //the camero is a few cm lower that the turret (x axis) torquey=error_cur_y*Kp*1.5+ (error_cur_y - error_pre_y)*Kd; //calculate torque (y axis) torquex=error_cur_x*Kp*scale+ (error_cur_x - error_pre_x)*Kd*scale; //calculate torque (x axis) rotationy=motorB->get_rotation(); //get encoder's measurements (motor B) if ((rotationy-160)&&(abs(rotationx)30))||(abs(torquey)>10)||(radiuson(0); motorA->on(0); } //---------------------------X_AXIS------------------------------------------------------------------------------------------------- if ((targetxtolx)){ if (abs(torquex)>=1.001){ motorA->on(-torquex); }//make motor A turn forward with speed X } else if((targetx>195)&(error_cur_x>tolx)){ if (abs(torquex)>=1.001){ motorA->on(torquex); }//make motor A turn forward with speed X } else{ printf("TARGET X found \n \r" ); flag_firex=true; motorA->on(0); motorA->off();//break the motor } July1,2012 [AUTOMATICURRETSCORPIO]60 //---------------------------Y_AXIS------------------------------------------------------------------------------------------------- if ((targetytoly)){ if (abs(torquey)>=1.001){ motorB->on(torquey); }//make motor A turn forward with speed X } else if((targety>150)&(error_cur_y>toly)){ if (abs(torquey)>=1.001){ motorB->on(-torquey); }//make motor A turn forward with speed X } else{ printf("TARGET Y found \n \r" ); flag_firey=true; motorB->on(0); motorB->off();//break the motor } //---------------------------------------------------------------------------------------------------------------------------- if ((flag_firex&&flag_firey)==true){ times_found++; if (times_found>=fire){ motorC->on(-80,2400); //Full Rotation (-50,1700) Sleep(50); printf("FIRE!!!! \n \r" ); motorA->stop();//break the motor motorB->stop();//break the motor system("pause"); return 0; } } } else{ rotationy=motorB->get_rotation(); rotationy=motorA->get_rotation(); //------MODE_A-> IF OUT OF BOUNDS THEN STOP motorB->stop();//break the motor motorA->stop();//break the motor cout