Licentiatseminarium. On Sensor Fusion Applied to Industrial Manipulators. Patrik Axelsson

Transkript

1 On Sensor Fusion Applied to Industrial Manipulators Division of Automatic Control Department of Electrical Engineering Linköping University

2 Översikt 2(46) 1. Introduktion 2. Modellering 3. Resultat 4. Sammanfattning och framtida arbete

3 Översikt 3(46) 1. Introduktion Reglerteknik Industrirobotik Problemformulering Tillståndsskattning 2. Modellering 3. Resultat 4. Sammanfattning och framtida arbete

4 Reglerteknik 4(46) Reglerteknik handlar om att få ett system (bil, robot, pappersmaskin, mobiltelefon,... ) att göra det man vill.

5 Reglerteknik 4(46) Reglerteknik handlar om att få ett system (bil, robot, pappersmaskin, mobiltelefon,... ) att göra det man vill. Reglerteknik underlättar applikationer som är farliga svåra repetitiva tråkiga...

6 Reglerteknik 4(46) Reglerteknik handlar om att få ett system (bil, robot, pappersmaskin, mobiltelefon,... ) att göra det man vill. Reglerteknik underlättar applikationer som är farliga svåra repetitiva tråkiga... Men även de som kräver hög nogrannhet hög produktionstakt...

7 Reglerteknik 5(46) System med: Insignal (det man kan ändra/påverka), Utsignal (det man vill styra/kan mäta), Störning (det man inte kan påverka). Störning Referens Insignal + Regulator System Utsignal

8 Reglerteknik 5(46) System med: Insignal (det man kan ändra/påverka), Utsignal (det man vill styra/kan mäta), Störning (det man inte kan påverka). Återkoppling Återkopplar det man kan mäta. Jämför med en referens. Regulator som beräknar en insignal. Störning Referens Insignal + Regulator System Utsignal

9 Industrirobotik 6(46) Första patentansökan Beviljades roboten Unimate lanserades robotinstallationen 1961 för General Motors.... mikroprocessorstyrda roboten IRB6 lanserades av ASEA (nuvarande ABB) 1973.

10 Industrirobotik 6(46) Första patentansökan Beviljades roboten Unimate lanserades robotinstallationen 1961 för General Motors.... mikroprocessorstyrda roboten IRB6 lanserades av ASEA (nuvarande ABB) Stora tillverkare: ABB (Sverige/Schweiz), KUKA (Tyskland), Motoman (Japan), FANUC Robotics (USA).

11 Joint 3 Joint 2 Joint 1 Joint 4 Joint 5 Joint 6 Industrirobotik 7(46) En seriell robot har n leder vilket innebär n frihetsgrader (DOF) (Ofta 6 st), Joint 4 Joint 5 Joint 6 n + 1 länkar, Aktuatorer: Elektriska motorer, Joint 3 Sensorer: Enkoder/resolver som mäter motorvinklarna q m. Joint 2 Verktygets position (TCP) beskrivs av ledvinklarna q a (armvinklar). Joint 1 För en ideal växellåda gäller q m = ηq a. I praktiken är det en vek koppling innehåller dynamik.

12 Industrirobotik 8(46) Modeller Användarprogram Banplanering r(t) Regulator u(t) y(t) B A C

13 Industrirobotik 8(46) Modeller Användarprogram Banplanering r(t) Regulator u(t) y(t) B A C Modeller Kinematiska modeller. Robotens rörelse utan påverkan av en yttre kraft. Framåt: X = Υ(q a ), Ẋ = Υ(q a) q a = J (q a ) q a, q a ( ) d Ẍ = J (q a ) q a + dt J (q a) q a.

14 Industrirobotik 8(46) Modeller Användarprogram Banplanering r(t) Regulator u(t) y(t) B A C Modeller Kinematiska modeller. Robotens rörelse utan påverkan av en yttre kraft. Invers: q a = Υ 1 (X), q a = J 1 (q a )Ẋ, q a = J 1 (q a ) (Ẍ ddt ) (J (q a)) q a.

15 Industrirobotik 8(46) Modeller Användarprogram Banplanering r(t) Regulator u(t) y(t) B A C Modeller Dynamiska modeller. Robotens rörelse när en yttre kraft påverkar. q = ( q T a q T m) T, M(q) q + C(q, q)+g(q) + T(q) + D(q) + F(q) = u.

16 Industrirobotik 9(46) Modeller Användarprogram Banplanering r(t) Regulator u(t) y(t) B A C Banplanering Tar kommandona från användarprogrammet och skapar en geometrisk bana för TCP. Gör om den geometriska banan för TCP till hur axlarna ska röra sig. Lägger på hastighets- och accelerationskraven. Även fysiska krav på roboten tillkommer, t.ex. max moment för motorer/växellådor.

17 Industrirobotik 10(46) Modeller Användarprogram Banplanering r(t) Regulator u(t) y(t) B A C Regulator Framkoppling. Beräknar det moment som krävs för att klara av gravitationen. Beräknar motorreferenser från armreferenserna. Återkoppling. Mäter motorvinklarna och jämför med referensen. Ofta räcker det med en diagonal PID-regulator.

18 Problemformulering 11(46) d Vill styra q ref m X ref Regulator u Robot X q m Observatör Kan mätas

19 Problemformulering 11(46) d Vill styra q ref m X ref Regulator u Robot X q m Observatör Kan mätas Vill styra TCP men kan endast mäta motorvinklarna q m.

20 Problemformulering 11(46) d Vill styra q ref m X ref Regulator u Robot X q m Observatör Kan mätas Vill styra TCP men kan endast mäta motorvinklarna q m. Återkoppling av q m fungerar ej tillräckligt bra p.g.a. flexibla leder.

21 Problemformulering 11(46) d X ref Regulator u Robot X q m Observatör Vill styra TCP men kan endast mäta motorvinklarna q m. Återkoppling av q m fungerar ej tillräckligt bra p.g.a. flexibla leder. Om TCP kan mätas är det naturligt att återkoppla detta.

22 Problemformulering 11(46) d X ref Regulator u Robot X q m Observatör Vill styra TCP men kan endast mäta motorvinklarna q m. Återkoppling av q m fungerar ej tillräckligt bra p.g.a. flexibla leder. Om TCP kan mätas är det naturligt att återkoppla detta. Hur ska man göra istället för att mäta TCP?

23 Problemformulering 12(46) d X ref Regulator u Robot X q m Observatör

24 Problemformulering 12(46) d X ref Regulator u Robot X Ẍ q m Observatör Inför en mätning av TCP i form av accelerationen.

25 Problemformulering 12(46) d X ref Regulator u Robot X Ẍ q m X Observatör Inför en mätning av TCP i form av accelerationen. Skapa en observatör för att skatta TCP.

26 Problemformulering 12(46) d X ref Regulator u Robot X Ẍ q m X Observatör Inför en mätning av TCP i form av accelerationen. Skapa en observatör för att skatta TCP. Hur ska man skatta TCP?

27 Bayesiansk tillståndsskattning 13(46) Givet en modell för systemet x k+1 = f (x k, u k, v k ) p(x k+1 x k ) y k = h(x k, u k, e k ) p(y k x k ) samt y 1:k och u 1:k.

28 Bayesiansk tillståndsskattning 13(46) Givet en modell för systemet x k+1 = f (x k, u k, v k ) p(x k+1 x k ) y k = h(x k, u k, e k ) p(y k x k ) samt y 1:k och u 1:k. Då kan tillstånden x k beräknas från p(x k y 1:k ) = p(y k x k )p(x k y 1:k 1 ), p(y k y 1:k 1 ) p(x k+1 y 1:k ) = p(x k+1 x k )p(x k y 1:k ) dx k, R nx p(y k y 1:k 1 ) = p(y k x k )p(x k y 1:k 1 ) dx k, R nx där k = 1, 2,..., N.

29 Tillståndsskattning 14(46) En analytisk lösning till dessa ekvationer existerar endast i det linjära fallet. Mer känt som Kalman filtret. Approximativa lösningar måste användas i det olinjära fallet.

30 Tillståndsskattning 14(46) En analytisk lösning till dessa ekvationer existerar endast i det linjära fallet. Mer känt som Kalman filtret. Approximativa lösningar måste användas i det olinjära fallet. Här används Extended Kalman filter (EKF) (Extended Kalman smoother (EKS)) Approximerar systemet med en linjärisering av de olinjära ekvationerna. Antar additivt Gaussiskt brus.

31 Tillståndsskattning 14(46) En analytisk lösning till dessa ekvationer existerar endast i det linjära fallet. Mer känt som Kalman filtret. Approximativa lösningar måste användas i det olinjära fallet. Här används Extended Kalman filter (EKF) (Extended Kalman smoother (EKS)) Approximerar systemet med en linjärisering av de olinjära ekvationerna. Antar additivt Gaussiskt brus. Particle filter (PF) Approximerar posterior fördelningen med ett stort antal partiklar med tillhörande vikter. Den optimala proposal fördelningen approximeras med ett EKF.

32 Översikt 15(46) 1. Introduktion 2. Modellering Robotmodell Accelerometermodell Skattningsmodeller 3. Resultat 4. Sammanfattning och framtida arbete

33 Robotmodell 16(46) Seriell robot med 2 DOF. z b q a2 q a1 ρb x S z S x b

34 Robotmodell 16(46) Seriell robot med 2 DOF. Olinjära vekheter Moment [Nm] q a q a m [arcmin]

35 Robotmodell 16(46) Seriell robot med 2 DOF. Olinjära vekheter. Olinjär friktion. Friktionsmoment [Nm] Motorhastighet [rad/s]

36 Robotmodell 16(46) Seriell robot med 2 DOF. Olinjära vekheter. Olinjär friktion. q a = ( ) T q a1 q a2, z b q a1 ρb q a2 z S x S q a m = ( q m1 /η 1 q m2 /η 2 ) T, x b τ a m = ( τ m1 η 1 τ m2 η 2 ) T,

37 Robotmodell 16(46) Seriell robot med 2 DOF. Olinjära vekheter. Olinjär friktion. z b q a1 q a2 z S q a = ( q a1 q a2 ) T, ρb x S q a m = ( q m1 /η 1 q m2 /η 2 ) T, x b τ a m = ( τ m1 η 1 τ m2 η 2 ) T, M a (q a ) q a + C(q a, q a ) + G(q a ) + T(q a q a m) + D( q a q a m) = 0, M m q a m + F( q a m) T(q a q a m) D( q a q a m) = τ a m.

38 Accelerometermodell 17(46) z b q a2 q a1 ρb x S z S x b ρ s (q a ) = R b/s (q a ) ( ρ b (q a ) + G b ) + b ACC

39 Accelerometermodell 17(46) Accelerationen från rörelsen. z b q a2 q a1 ρb x S z S x b ρ s (q a ) = R b/s (q a ) ( ρ b (q a ) + G b ) + b ACC ρ b (q a ) = Υ ACC (q a ), ρ b (q a ) = d2 dt 2 Υ ACC(q a ) = J ACC (q a ) q a + ( ) d dt J ACC(q a ) q a

40 Accelerometermodell 17(46) Accelerationen från rörelsen. Accelerationen från gravitationen. z b q a1 q a2 z S ρb x S x b ρ s (q a ) = R b/s (q a ) ( ρ b (q a ) + G b ) + b ACC G b = ( ) 0 g

41 Accelerometermodell 17(46) Accelerationen från rörelsen. Accelerationen från gravitationen. z b q a1 q a2 z S Rotationsmatris från Ox b z b till Ox s z s. ρb x S x b ρ s (q a ) = R b/s (q a ) ( ρ b (q a ) + G b ) + b ACC R b/s (q a ) = ( cos (qa1 + q a2 ) ) sin (q a1 + q a2 ) sin (q a1 + q a2 ) cos (q a1 + q a2 )

42 Accelerometermodell 17(46) Accelerationen från rörelsen. Accelerationen från gravitationen. z b q a1 q a2 z S Rotationsmatris från Ox b z b till Ox s z s. ρb x S Biasparameter. x b ρ s (q a ) = R b/s (q a ) ( ρ b (q a ) + G b ) + b ACC ( ) b ACC,x b ACC = b ACC,z

43 Skattningsmodeller 18(46) 1. Olinjär modell Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m ) T

44 Skattningsmodeller 18(46) 1. Olinjär modell Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Fysikaliska modellen ger kontinuerlig tillståndsmodell. ) T

45 Skattningsmodeller 18(46) 1. Olinjär modell Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Fysikaliska modellen ger kontinuerlig tillståndsmodell. Diskretisering med Euler framåt ger x k+1 = f (x k, u k ) + g(x k )v k. ) T

46 Skattningsmodeller 18(46) 1. Olinjär modell Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Fysikaliska modellen ger kontinuerlig tillståndsmodell. Diskretisering med Euler framåt ger Mätekvation: ( y k = R b/s (x 1,k ) x k+1 = f (x k, u k ) + g(x k )v k. ) ( x 2,k ( ) ) J ACC (x 1,k ) q a,k + d dt J + e ACC(x 1,k ) x 3,k + G k b ) T

47 Skattningsmodeller 19(46) 2. Linjär dynamikmodell Tillståndsvektor: x = ( x T 1 x T 2 x T 3) T = ( q T a q T a q T a ) T

48 Skattningsmodeller 19(46) 2. Linjär dynamikmodell Tillståndsvektor: x = ( x T 1 x T 2 x T 3) T = ( q T a q T a q T a Linjär dynamikmodell (Dubbelintegrator i diskret tid): x k+1 = Fx k + G u u k + G v v k ) T

49 Skattningsmodeller 19(46) 2. Linjär dynamikmodell Tillståndsvektor: x = ( x T 1 x T 2 x T 3) T = ( q T a q T a q T a Linjär dynamikmodell (Dubbelintegrator i diskret tid): x k+1 = Fx k + G u u k + G v v k Insignalen är tredjederivatan av armvinkelreferensen. ) T

50 Skattningsmodeller 19(46) 2. Linjär dynamikmodell Tillståndsvektor: x = ( x T 1 x T 2 x T 3) T = ( q T a q T a q T a Linjär dynamikmodell (Dubbelintegrator i diskret tid): x k+1 = Fx k + G u u k + G v v k Insignalen är tredjederivatan av armvinkelreferensen. Mätekvation: ( x1,k + K 1 ) (M a (x 1,k )x 3,k + C(x 1,k, x 2,k ) + G(x 1,k )) y k = ( ( ) ) R b/s (x 1,k ) J ACC (x 1,k )x 3,k + d dt J + e ACC(x 1,k ) x 2,k + G k b ) T

51 Skattningsmodeller 20(46) 3. Olinjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m ) T

52 Skattningsmodeller 20(46) 3. Olinjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. ) T

53 Skattningsmodeller 20(46) 3. Olinjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. Fysikaliska modellen + Euler framåt ger x 1,k + T s x 3,k x k+1 = x 2,k + T s x 4,k x 3,k + T s q a,k IN + g(x k)v k. x 4,k + T s Mm 1 (u k F(x 4,k ) + T(x 1,k x 2,k ) + D(x 3,k x 4,k )) ) T

54 Skattningsmodeller 20(46) 3. Olinjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. Fysikaliska modellen + Euler framåt ger x 1,k + T s x 3,k x k+1 = x 2,k + T s x 4,k x 3,k + T s q a,k IN + g(x k)v k. x 4,k + T s Mm 1 (u k F(x 4,k ) + T(x 1,k x 2,k ) + D(x 3,k x 4,k )) ) T Mätekvation: y k = ( 0 I 0 0 ) x k + e k

55 Skattningsmodeller 21(46) 4. Linjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. ) T

56 Skattningsmodeller 21(46) 4. Linjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. Fysikaliska modellen + linjär fjäder, dämpare och friktion ger 0 0 I ) ẋ = I x IN ( q a I 0. u Mm 1 K Mm 1 K Mm 1 D Mm 1 (D + F d ) 0 Mm 1 ) T

57 Skattningsmodeller 21(46) 4. Linjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. Fysikaliska modellen + linjär fjäder, dämpare och friktion ger 0 0 I ) ẋ = I x IN ( q a I 0. u Mm 1 K Mm 1 K Mm 1 D Mm 1 (D + F d ) 0 Mm 1 Diskretiseras med ZOH. ) T

58 Skattningsmodeller 21(46) 4. Linjär modell med accelerationen som insignal Tillståndsvektor: x = ( ) x T 1 x T 2 x T 3 x T T 4 = ( q T a qm a,t q T a q a,t m Löser ut armvinkelaccelerationen ur accelerometersignalen. Fysikaliska modellen + linjär fjäder, dämpare och friktion ger 0 0 I ) ẋ = I x IN ( q a I 0. u Mm 1 K Mm 1 K Mm 1 D Mm 1 (D + F d ) 0 Mm 1 Diskretiseras med ZOH. Mätekvation: y k = ( 0 I 0 0 ) x k + e k ) T

59 Översikt 22(46) 1. Introduktion 2. Modellering 3. Resultat Simuleringsresultat Skattning av kovariansmatriser Beräkning av accelerometerns position och orientering Experimentella resultat 4. Sammanfattning och framtida arbete

60 Simulering 23(46) Olinjära skattningsmodellen och EKF med och utan bias-kompensering.

61 Simulering 23(46) Olinjära skattningsmodellen och EKF med och utan bias-kompensering. Simulering... SIM1:... utan modellfel, SIM2:... med modellfel i dynamik- och accelerometermodellen samt bias, SIM3:... med endast modellfel i accelerometermodellen samt bias.

62 Simulering 23(46) Olinjära skattningsmodellen och EKF med och utan bias-kompensering. Simulering... SIM1:... utan modellfel, SIM2:... med modellfel i dynamik- och accelerometermodellen samt bias, SIM3:... med endast modellfel i accelerometermodellen samt bias. Mätbrusets kovariansmatris R från mätdata. Processbrusets kovariansmatris Q från ett optimeringsproblem. e k = (x k ˆx k ) 2 + (z k ẑ k ) 2

63 Simulering 23(46) Olinjära skattningsmodellen och EKF med och utan bias-kompensering. Simulering... SIM1:... utan modellfel, SIM2:... med modellfel i dynamik- och accelerometermodellen samt bias, SIM3:... med endast modellfel i accelerometermodellen samt bias. Mätbrusets kovariansmatris R från mätdata. Processbrusets kovariansmatris Q från ett optimeringsproblem. e k = (x k ˆx k ) 2 + (z k ẑ k ) 2 Optimerar Q för alla 3 simuleringar med och utan bias-kompensering. Utvärdera olika kombinationer av simuleringar och skattade Q.

64 Simulering 24(46) Banor z [m] z [m] x [m] x [m] Grön del: Används för utvärdering. Röd stjärna: Startpunkten (medsols rörels). Blå cirkel: TCP då q a = ( 0 0 ) T.

65 Simulering 25(46) z [m] z [m] x [m]

66 Simulering 25(46) z [m] z [m] x [m] e [mm] e [mm] e [mm] Tid [s] COV1 COV2 COV3 COV4 COV5 SIM SIM SIM

67 Simulering 26(46) z [m] z [m] x [m]

68 Simulering 26(46) z [m] e [mm] z [m] x [m] Tid [s]

69 Simulering 27(46) För att skattningen ska bli bra krävs det små modellfel i den dynamiska modellen, bra val av processbrusets kovariansmatris, god information om position och orientering av accelerometern.

70 Skattning av kovariansmatriser 28(46) Skatta Q med Expectation Maximisation (EM) algoritmen. 1. Välj ett startvärde Q 0 S n v + och sätt l = 0.

71 Skattning av kovariansmatriser 28(46) Skatta Q med Expectation Maximisation (EM) algoritmen. 1. Välj ett startvärde Q 0 S n v + och sätt l = Beräkna M i = ( J 1,i 1 I ) P ξ,s ( J1,i 1 I )T ) ) + (ˆx s i N i N F 1(ˆx s i 1 N (ˆx ) s i N F 1(ˆx s i 1 N ) T med hjälp av Q l.

72 Skattning av kovariansmatriser 28(46) Skatta Q med Expectation Maximisation (EM) algoritmen. 1. Välj ett startvärde Q 0 S n v + och sätt l = Beräkna M i = ( J 1,i 1 I ) P ξ,s ( J1,i 1 I )T ) ) + (ˆx s i N i N F 1(ˆx s i 1 N (ˆx ) s i N F 1(ˆx s i 1 N ) T med hjälp av Q l. 3. Beräkna uppdateringen av Q enligt Q l+1 = 1 N 1 N i=2 F 2 (ˆxs i 1 N )M i ( F 2 (ˆxs i 1 N ) ) T.

73 Skattning av kovariansmatriser 28(46) Skatta Q med Expectation Maximisation (EM) algoritmen. 1. Välj ett startvärde Q 0 S n v + och sätt l = Beräkna M i = ( J 1,i 1 I ) P ξ,s ( J1,i 1 I )T ) ) + (ˆx s i N i N F 1(ˆx s i 1 N (ˆx ) s i N F 1(ˆx s i 1 N ) T med hjälp av Q l. 3. Beräkna uppdateringen av Q enligt Q l+1 = 1 N 1 N i=2 F 2 (ˆxs i 1 N )M i ( F 2 (ˆxs i 1 N ) ) T. 4. Stanna om L Ql (y 1:N ) L Ql m (y 1:N ) γ, annars sätt l = l + 1 och gå till steg 2.

74 Skattning av kovariansmatriser 29(46) Jämför med föregående metod (Alt. 1) samt en metod som beräknar Q direkt från modellen och glättade tillstånden (Alt. 2). Monte Carlo simuleringar med olika startvärden.

75 Skattning av kovariansmatriser 29(46) Jämför med föregående metod (Alt. 1) samt en metod som beräknar Q direkt från modellen och glättade tillstånden (Alt. 2). Monte Carlo simuleringar med olika startvärden. Resultat: EM algoritmen konvergerar till samma lösning för alla startvärden. Det samma gäller för Alt. 2. EM algoritmen ger lägst skattningsfel e [mm] Tid [s]

76 Beräkning av accelerometerns orientering 30(46) z b y b x b

77 Beräkning av accelerometerns orientering 30(46) Söker en transformation ρ s = κr a/s ρ a + ρ 0. z b y b x b z b y b x b

78 Beräkning av accelerometerns orientering 30(46) Söker en transformation ρ s = κr a/s ρ a + ρ 0. z b Definiera residualerna e k = ρ s,k κr a/s ρ a,k ρ 0. y b x b z b y b x b

79 Beräkning av accelerometerns orientering 30(46) Söker en transformation ρ s = κr a/s ρ a + ρ 0. z b Definiera residualerna y b x b e k = ρ s,k κr a/s ρ a,k ρ 0. Optimeringsproblem: minimise N k=1 e k 2 subject to det(r a/s ) = 1 R T a/s = R 1 a/s z b y b x b

80 Beräkning av accelerometerns orientering 31(46) Mät gravitationen i olika orienteringar x s z s y s z s y s z b g y b x b z s y s y s x s x s z s x s y s z s x s z s y s x s

81 Beräkning av accelerometerns orientering 31(46) Mät gravitationen i olika orienteringar x s z s y s z s y s z b g y b x b z s y s y s x s x s z s x s y s z s x s z s y s x s Vet att ρ s ska anta följande värden: ρ 1 s = ( 0 0 g ) T ρ 4 s = ( 0 g 0 ) T, ρ 2 s = ( 0 g 0 ) T, ρ 5 s = ( g 0 0 ) T, ρ 3 s = ( 0 0 g ) T,, ρ 6 s = ( g 0 0 ) T.

82 Beräkning av accelerometerns position 32(46) Rotera axel 1 med konstant hastighet. Innebär att accelerationen pekar in mot rotationscentrum i ett plan ortogonalt mot gravitationen.

83 Beräkning av accelerometerns position 32(46) Rotera axel 1 med konstant hastighet. Innebär att accelerationen pekar in mot rotationscentrum i ett plan ortogonalt mot gravitationen. Beräkna ett analytiskt uttryck för accelerationen och jämför med mätningen. Det analytiska uttrycket beror på de okända positionsparametrarna.

84 Beräkning av accelerometerns position 32(46) Rotera axel 1 med konstant hastighet. Innebär att accelerationen pekar in mot rotationscentrum i ett plan ortogonalt mot gravitationen. Beräkna ett analytiskt uttryck för accelerationen och jämför med mätningen. Det analytiska uttrycket beror på de okända positionsparametrarna. Accelerometers orientering anses vara känd, d.v.s. transforamtionen från Ox a y a z a till Ox s y s z s är känd. Orientera accelerometern så att gravitationen är riktad längs en av axlarna i Ox s y s z s.

85 Beräkning av accelerometerns position 33(46) L 1 r w z w x r s / w w z s a s l 3 x s l 1 L 2 z b x y L 3 bf y b x bf θ xb r w r s L 1 a s x w rw r s / w rs z s al s 1 z w y w l 3 L 4 zw r s / w y s x s l 2 y bf y b x bf θ xb z z b bf y b r s x b z b bf x s l 3 θ x bf x b θ y b x bf x b L 1 a s r s / w z w l 2 x s y bf y b x bf L 1 rw zw y y s w θ r s / w z s rw rs l 3 L 2 x b r s a s x w l 1 x s z b bf l 3 y b θ x b x bf

86 Beräkning av accelerometerns position 33(46) z z b bf y b θ x bf x b z b bf L 1 r w L 1 rw r s rs x w a s z w r s / w l 3 a s z s x s l 1 L 2 z b bf r s / w x l [r s 2 z ws ] b = [ ] Q bf /b y y s w l 3 L 2 z b x b x θ y b x bf x b y L 3 bf y b x bf θ xb r w Accelerometerns position ges av r s b r s L 1 a s x w rw r s / w rs x s z s al s 1 z w y w l 3 L 4 zw r s / w L y 1 bf ) ([r w ] bf + [r s/w ] bf θ x b y b x bf rw r s a s l 3 x w y s zw r s / w l 3 z s x s. l 2 l 1 x s y bf y b x bf θ xb θ y b x bf x b

87 Beräkning av accelerometerns position 33(46) z z b bf y b θ x bf x b z b bf L 1 r w r s x w a s z w r s / w l 3 z s x s l 1 L 2 z b bf θ y b x bf x b xb r w r s a s x w rw r s / w rs L a L s y 1 1 r s / w x bf l s y [r b 2 z ws ] b = [ ] Q bf /b x bf rw b y y s w θ x b r r w s l Derivering två 3 gånger L 2 ger accelerationen θ y b x bf x b z b x b x y L 3 bf y b x bf Accelerometerns position ges av r s ) ([r w ] bf + [r s/w ] bf z. w [a s ] b = S(ω)S(ω) [ Q bf /b ] θ b L 1 x s z s al s 1 r s z w y w l 3 L 4 a s zw r s / w l 3 x w y s r s / w ([r w ] bf + [r s/w ] bf ). l 3 z s x s l 2 l 1 x s y bf y b x bf θ xb

88 Beräkning av accelerometerns position 34(46) Jämförelse med mätningen från accelerometern ger ( 0 θ 2 ) ( ) ( l2 a M θ 2 = s,x + θ 2 ) L 1. 0 l 3 a M s,y

89 Beräkning av accelerometerns position 34(46) Jämförelse med mätningen från accelerometern ger ( 0 θ 2 ) ( ) ( l2 a M θ 2 = s,x + θ 2 ) L 1. 0 l 3 a M s,y Ytterligare två robotkonfigurationer ger 0 0 θ 2 a M c1 s,x,c1 + θ c1 2 L 1 0 θ c1 2 0 a M s,y,c1 θ c l 1 0 θ c2 2 0 l 2 a = M s,z,c2 + θ c2 2 L 3 a 0 0 θ c3 2 l 3 M. s,y,c2 }{{} a M θ c3 2 s,x,c3 + θ c3 2 L l a }{{} M s,z,c3 }{{} A b

90 Beräkning av accelerometerns pose Resultat 35(46) Accelerometern monteras i 5 olika positioner och orienteringar. z s 1 z y a 2 za x a a 3 x y a a 4 x a z a 5 z a ya xa za y s x s y a x a y a

91 Beräkning av accelerometerns pose Resultat 35(46) Accelerometern monteras i 5 olika positioner och orienteringar. z s 1 z y a 2 za x a a 3 x y a a 4 x a z a 5 z a ya xa za y s x s y a x a y a Rotationsmatriserna ska likna (a, b, c, d cos(45 ) ) a 3 b 3 0 R 1 a/s = 0 0 1, R 2 a/s = 0 0 1, R 3 a/s = c 3 d a 5 b 5 0 R 4 a/s = 1 0 0, R 5 a/s = c 5 d 5 0

92 Beräkning av accelerometerns pose Resultat 36(46) M.h.a. kvarternioner kan rotationsskillnaden mellan sann och skattad rotationsmatris beräknas. Test ϑ

93 Beräkning av accelerometerns pose Resultat 36(46) M.h.a. kvarternioner kan rotationsskillnaden mellan sann och skattad rotationsmatris beräknas. Test ϑ Transformera mätningarna och jämför med vad man vill ha. z s y s x s Acceleration [m/s 2 ]

94 Beräkning av accelerometerns pose Resultat 36(46) M.h.a. kvarternioner kan rotationsskillnaden mellan sann och skattad rotationsmatris beräknas. Test ϑ Transformera mätningarna och jämför med vad man vill ha. z s y s z b x s y b Acceleration [m/s 2 ]

95 Beräkning av accelerometerns pose Resultat 37(46) Skattad position. Test Est. pos. (ˆl) [cm] = ˆl l M [cm] Std. for ˆl [cm] ( ) T 15.5 ( ) T 1.0 ( ) T 0.5 ( ) T 16.9 ( ) T 1.8 ( ) T 0.3 ( ) T 21.4 ( ) T 1.6 ( ) T 0.6 ( ) T 5.9 ( ) T 0.4 ( ) T 0.4 ( ) T 16.5 ( ) T 1.0 ( ) T 0.6

96 Experiment Skattning av verktygspositionen 38(46) Experiment utförda på ABB IRB Endast axel 2 och 3 samt påverkan från handleden minimerad.

97 Experiment Skattning av verktygspositionen 38(46) Experiment utförda på ABB IRB Endast axel 2 och 3 samt påverkan från handleden minimerad. Sann bana uppmätt med ett lasersystem från Leica Geosystems

98 Experiment Skattning av verktygspositionen 38(46) Experiment utförda på ABB IRB Endast axel 2 och 3 samt påverkan från handleden minimerad. Sann bana uppmätt med ett lasersystem från Leica Geosystems z [m] x [m]

99 Experiment Skattning av verktygspositionen 38(46) Experiment utförda på ABB IRB Endast axel 2 och 3 samt påverkan från handleden minimerad. Sann bana uppmätt med ett lasersystem från Leica Geosystems Den sanna banan ej synkroniserad med de andra mätningarna Fel kan införas vid valideringen. Osäkerhet i programmerad TCP Skattad och mätt position justeras för att minimera statiska fel. z [m] x [m]

100 Experiment Skattning av verktygspositionen 39(46) 6 observatörer OBS1: EKF med den olinjära modellen. OBS2: EKS med den olinjära modellen.

101 Experiment Skattning av verktygspositionen 39(46) 6 observatörer OBS1: EKF med den olinjära modellen. OBS2: EKS med den olinjära modellen. OBS3: EKF med linjär dynamikmodell. OBS4: PF med linjär dynamikmodell.

102 Experiment Skattning av verktygspositionen 39(46) 6 observatörer OBS1: EKF med den olinjära modellen. OBS2: EKS med den olinjära modellen. OBS3: EKF med linjär dynamikmodell. OBS4: PF med linjär dynamikmodell. OBS5: EKF med olinjär modell där accelerationen används som insignal. OBS6: Linjär dynamisk observatör m.h.a. polplacering med linjär modell där accelerationen används som insignal.

103 Experiment Resultat för OBS1 och OBS2 40(46) Ger samma skattning. Svänger med högre frekvens än den sanna banan. Biastillstånden påverkar inte. Nuvarande MATLABimplementering ej i realtid för någon av observatörerna. Sann bana OBS1 OBS2 z [m] z [m] e [mm] x [m] Sample

104 Experiment Resultat för OBS3 och OBS4 41(46) Båda följer den sanna banan. OBS3 går förbi i hörnen. Bias-tillstånd för både motor- och accelerometer-mätningen krävs för att få bra resultat. Nuvarande MATLABimplementering ej i realtid för någon av observatörerna. Sann bana OBS3 OBS4 z [m] z [m] e [mm] x [m] Sample

105 Experiment Resultat för OBS5 och OBS6 42(46) Observatören med linjära modellen fungerar bra. Observatören med olinjära modellen svänger med högre frekvens än den sanna banan. Ingen kompensering för bias. Nuvarande MATLABimplementering i realtid för båda observatörerna. Sann bana OBS5 OBS6 z [m] z [m] e [mm] x [m] Sample

106 Översikt 43(46) 1. Introduktion 2. Modellering 3. Resultat 4. Sammanfattning och framtida arbete

107 Sammanfattning 44(46) Skattning av robotens TCP m.h.a. Bayesianska metoder och en accelerometer. Metod för att skatta orienteringen och positionen för accelerometern på roboten har presenterats. Skattning av processbrusets kovariansmatris m.h.a. EM-algoritmen. Experimentell utvärdering av 6 olika observatörer på en ABB IRB4600.

108 Framtida arbete 45(46) Diskretisering av kontinuerlig tillståndsmodell. Känslighetsanalys av modellparametrar. Tidsberoende kovariansmatriser för att klara av kraftiga ändringar i banan. Utöka observatörerna till 6 DOF. Fler sensorer? Reglering från skattad position.

109 Tack 46(46) Tack för att ni lyssnade!