Sterowanie przegubem obrotowym z wykorzystaniem sprzężenia

Politechnika WrocławskaWydział ElektronikiKierunek:Specjalność:Automatyka i Robotyka (AiR)Robotyka (ARR)PRACA DYPLOMOWAMAGISTERSKASterowanie przegubemobrotowym z wykorzystaniemsprzężenia zwrotnego od siły.Autor:Mariusz JaniakProwadzący pracę:dr inż. Marek Wnuk, I-6Ocena pracy:Wrocław 2005

Składam serdeczne podziękowania Panudr. Markowi Wnukowi za nieocenionąpomoc, wyrozumiałość, poświęcony czas orazwielkie zaangażowanie. Chciałem równieżpodziękować Panu Ryszardowi Krzywańskiemuza cenne rady oraz trud włożonyw wykonanie konstrukcji mechanicznej. Nakońcu chciałem podziękować firmie MAXONi jej przedstawicielowi, Panu Szewczykowi(firma UZIMEX) za bezpłatne przekazaniesilnika do celów badawczych

Spis treści1 Wstęp 51.1 Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 Wprowadzenie 62.1 Przeniesienie napędu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62.2 Siłownik z elastycznością szeregową . . . . . . . . . . . . . . . . . . . . . . . . . 72.3 Model matematyczny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3.1 Elastyczny sensor . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3.2 Wejścia systemu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3.3 Silnik i przekładnia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3.4 Sterownik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3.5 Model części energetycznej . . . . . . . . . . . . . . . . . . . . . . . . . . 102.3.6 Model z zamkniętą pętlą . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.3.7 Model z zamkniętą pętlą z dołączonym obciążeniem . . . . . . . . . . . . 123 Konstrukcja siłownika 133.1 Konstrukcja mechaniczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.1.1 Przeniesienie napędu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.1.2 Silnik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.2 Konstrukcja elektroniczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 193.2.1 Sterownik . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 223.2.2 Stopień mocy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283.2.3 Zasilacz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 314 Oprogramowanie 314.1 Oprogramowanie uruchomieniowe CodeWariorr . . . . . . . . . . . . . . . . . . 324.2 Struktura oprogramowania sterownika . . . . . . . . . . . . . . . . . . . . . . . 344.3 Struktura układu regulacji . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 364.4 Sterowanie silnikiem BLDC - komutacja . . . . . . . . . . . . . . . . . . . . . . 374.5 Implementacja regulatora PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . 404.6 Komunikacja z użytkownikiem . . . . . . . . . . . . . . . . . . . . . . . . . . . . 425 Interfejs użytkownika - dostępne funkcje 436 Przebieg i wyniki eksperymentu 456.1 Identyfikacja obiektu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 466.2 Badania obiektu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 506.3 Wyniki eksperymentów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 527 Podsumowanie 52Literatura 54Dodatek 551

Spis rysunków1 Model kolana z dołączonym siłownikiem liniowym . . . . . . . . . . . . . . . . . 72 Minimalny model siłownika z elastycznością szeregową . . . . . . . . . . . . . . 83 Pełen model siłownika z elastycznością szeregową . . . . . . . . . . . . . . . . . 94 Model części energetycznej siłownika z elastycznością szeregową . . . . . . . . . 115 Model siłownika z elastycznością szeregową z dołączonym obciążeniem . . . . . . 126 Widok siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 147 Widok mocowania przedniego łożyska . . . . . . . . . . . . . . . . . . . . . . . . 158 Widok mocowania tylnego łożyska . . . . . . . . . . . . . . . . . . . . . . . . . . 159 Widok tyłu siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1510 Widok przodu siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1511 Widok mocowania przekładni śrubowej . . . . . . . . . . . . . . . . . . . . . . . 1612 Widok karetki przekładni śrubowej . . . . . . . . . . . . . . . . . . . . . . . . . 1613 Widok silnika firmy MAXON wraz z koderem przyrostowo-impulsowy . . . . . . 1614 Widok uchwytu blokującego przednią ścianę efektora siłownika . . . . . . . . . . 1615 Widok czujnika siły . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1716 Widok sposobu mocowania czujnika siły . . . . . . . . . . . . . . . . . . . . . . 1717 Widok karetki siłownika przy zerowej sile wyjściowej . . . . . . . . . . . . . . . . 1718 Widok karetki siłownika przy sile wyjściowej równej -200N . . . . . . . . . . . . 1819 Widok karetki siłownika przy sile wyjściowej równej 200N . . . . . . . . . . . . . 1820 Schemat blokowy układu elektronicznego siłownika . . . . . . . . . . . . . . . . 1921 Schemat połączeń modułów ze sobą . . . . . . . . . . . . . . . . . . . . . . . . . 2022 Płytka sterownika – strona wierzchnia . . . . . . . . . . . . . . . . . . . . . . . . 2123 Płytka sterownika – strona spodnia . . . . . . . . . . . . . . . . . . . . . . . . . 2124 Płytka stopnia mocy – strona wierzchnia . . . . . . . . . . . . . . . . . . . . . . 2125 Płytka stopnia mocy – strona spodnia . . . . . . . . . . . . . . . . . . . . . . . 2126 Płytka zasilacza – strona wierzchnia . . . . . . . . . . . . . . . . . . . . . . . . . 2127 Płytka zasilacza – strona spodnia . . . . . . . . . . . . . . . . . . . . . . . . . . 2128 Schemat ideowy modułu DSP56F8323. . . . . . . . . . . . . . . . . . . . . . . . 2229 Schemat ideowy sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2630 Schemat ideowy układu generującego opóźnienie dla sygnału sterującego przyzmianie kierunku . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2731 Schemat ideowy zasilacza na płytce sterownika . . . . . . . . . . . . . . . . . . . 2832 Schemat ideowy stopnia mocy . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2933 Schemat ideowy półmostka mocy . . . . . . . . . . . . . . . . . . . . . . . . . . 3034 Schemat ideowy zasilacza . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3135 CodeWarrior IDE (ang. Integrated Development Environment) . . . . . . . . . . 3236 CW IDE z widocznym oknem debuggera . . . . . . . . . . . . . . . . . . . . . . 3337 CW IDE z widocznym oknem konfiguracji beana . . . . . . . . . . . . . . . . . . 3338 Diagram głównej pętli programu . . . . . . . . . . . . . . . . . . . . . . . . . . . 3539 Diagram przerwania cyklicznego . . . . . . . . . . . . . . . . . . . . . . . . . . . 3540 Diagram przerwania Input Capture . . . . . . . . . . . . . . . . . . . . . . . . . 3541 Schemat blokowy układu regulacji . . . . . . . . . . . . . . . . . . . . . . . . . . 3642 Algorytm komutacji silnika BLDC . . . . . . . . . . . . . . . . . . . . . . . . . . 3743 Opis uzwojeń oraz napięć zasilania silnika . . . . . . . . . . . . . . . . . . . . . 382

44 Widok stanowiska badawczego (1) . . . . . . . . . . . . . . . . . . . . . . . . . . 4645 Widok stanowiska badawczego (2) . . . . . . . . . . . . . . . . . . . . . . . . . . 4646 Zależność wartości pomiarowej F l od przesunięcia czujnika . . . . . . . . . . . . 4747 Zależność wartości pomiarowej F l od siły działającej na efektor . . . . . . . . . . 4748 Zależność siły działającej na sprężynę od przesunięcia sprężyny . . . . . . . . . . 4749 Zależność siły F m od wartości zadanej prądu I d . . . . . . . . . . . . . . . . . . 4850 Układ wykorzystany przy identyfikacji . . . . . . . . . . . . . . . . . . . . . . . 4951 Odpowiedzi modelu i siłownika na wymuszenie F m = 160N . . . . . . . . . . . . 5052 Odpowiedzi modelu i siłownika na wymuszenie F m = 193N . . . . . . . . . . . . 5053 Odpowiedzi modelu i siłownika na wymuszenie F m = 220N . . . . . . . . . . . . 5054 Odpowiedzi modelu i siłownika na wymuszenie F m = 248N . . . . . . . . . . . . 5055 Odpowiedzi modelu i siłownika na wymuszenie F m = 276N . . . . . . . . . . . . 5056 Odpowiedzi modelu i siłownika na wymuszenie F m = 303N . . . . . . . . . . . . 5057 Odpowiedzi siłownika z zablokowanym efektorem na wymuszenie F d = 234N ∗1(t − 10) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5158 Odpowiedzi siłownika z zablokowanym efektorem na wymuszenie F d = 117N ∗1(t − 10) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5159 Odpowiedź siłownika w trybie regulacji wyjściowej impedancji mechanicznej . . 5160 Rysunek złożeniowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5561 Tylnia ściana siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5662 Tylnia ściana efektora siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . 5763 Karetka przekładni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5864 Przednia ściana efektora siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . 5865 Przednia ściana siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5966 Kołnierz mocowania silnika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6067 Mocowanie enkodera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6068 Mocowanie silnika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6069 Główna prowadnica siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6170 Prowadnica efektora . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6171 Śruba przekładni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6172 Łożysko ślizgowe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6173 Podstawa siłownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 623

Spis tabel1 Parametry silnika EC 32 118893 firmy MAXON . . . . . . . . . . . . . . . . . . 182 Złącza Z1 i Z2 modułu DSP56F8323 . . . . . . . . . . . . . . . . . . . . . . . . 253 Złącze Z3 na płytce sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . 264 Złącze Z7 na płytce sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . 275 Tablica komutacji silnika BLDC . . . . . . . . . . . . . . . . . . . . . . . . . . . 386 Zapis sygnałów sterujących w postaci liczby 16-bitowej . . . . . . . . . . . . . . 397 Zapis stanu czujników Halla w postaci liczby 8-bitowej . . . . . . . . . . . . . . 398 Tablica komutacji silnika BLDC zastosowana w sterowniku . . . . . . . . . . . . 399 Tabela z wynikami eksperymentu . . . . . . . . . . . . . . . . . . . . . . . . . . 524

1 WstępWiększość komercyjnych aplikacji robotycznych realizuje zadanie osiągania pozycji lub śledzeniatrajektorii. Tradycyjne roboty potrafią realizować to zadanie z dużą prędkością, dokładnościączy powtarzalnością. Z powodzeniem zastępują więc ludzi w zadaniach gdzie wymaganesą takie umiejętności. Z drugiej strony, istnieje wiele zadań takich jak: chodzenie, bieganie,pływanie, chwytanie, manipulowanie, w których tradycyjne roboty radzą sobie znacznie gorzejod swoich biologicznych odpowiedników. Te zadania wymagają interakcji z otoczeniem. Wzadaniach takich ważne jest aby robot był w stanie mierzyć i kontrolować siły interakcji występującemiedzy nim a otoczeniem. Zadanie takie nazywa się zazwyczaj sterowaniem siłowym.Istnieją dwa ważne aspekty tego zadania. Pierwszym jest użycie algorytmów i wiadomości sensorycznychdla wypracowania pożądanych momentów w przegubach, zapewniających poprawnyrozkład sił w otoczeniu robota. Drugim jest możliwość dokładnego generowania pożądanych siłw przegubach robota. Ta praca dotyczy drugiego aspektu sterowania siłowego, opisuje napęd(siłownik), który generuje pożądaną siłę.Szukając inspiracji w organizmach żywych, podglądając rozwiązania napędów i przegubówstosowanych przez naturę, można zauważyć, że większość napędów (mięśni) ma charakter linowypodczas gdy przeguby mają charakter obrotowy. Dla przykładu można posłużyć się układemmięśniowym i kostnym człowieka. Noga człowieka składa się wyłącznie z przegubów obrotowych,mięśnie przytwierdzone do kości kurcząc się i rozkurczając wykonują ruch liniowy. Odpowiedniepołączenia między mięśniami i kośćmi sprawiają, że liniowy ruch mięśnia zamieniany jest naruch obrotowy w stawie człowieka.Podążając za naturą, w pracy zaproponowano napędzanie przegubu obrotowego siłownikiemliniowym. Siłownik z punktu widzenia przegubu powinien być źródłem czystej siły. Zapewnienietego warunku jest kluczową kwestią pracy. Kwestią dalszą z punktu widzenia sterowaniasiłowego jest sposób przeniesienia napędu.1.1 Cel i zakres pracyCelem pracy jest stworzenie siłownika z siłowym sprzężeniem zwrotnym, zdolnego do napędzaniawszelkiego typu przegubów obrotowych. Powstanie również stanowisko umożliwiającebadanie tego typu napędu. Aby osiągnąć główny cel pracy należy zrealizować następujące podcele:• zapoznać się z ideą sterowania siłowego oraz stosowanymi rozwiązaniami,• zaproponować konstrukcję siłownika z siłowym sprzężeniem zwrotnym,• zaproponować model matematyczny opisujący siłownik,• zaproponować sposób przeniesienia napędu liniowego na obrotowy,• opanować sterowanie silnikami bezkomutatorowymi prądu stałego BLDC,• opanować programowanie procesorów sygnałowych DSP,• wykonać rzeczywisty siłownik (mechanika i elektronika),• napisać oprogramowanie umożliwiające przeprowadzenie badań,• przeprowadzić identyfikację obiektu,5

• zweryfikować model matematyczny z obiektem rzeczywistym.2 WprowadzenieZadaniem napędu (siłownika) jest zamiana energii (elektrycznej, pneumatycznej, hydraulicznej)na siłę mechaniczną i ruch. Większość istniejących napędów, poza nielicznymi wyjątkami,słabo radzi sobie z precyzyjnym generowaniem sił w przegubach robota. Powodem tych niedokładnościsą m.in: tarcie, luzy w przekładni, bezwładność zwielokrotniona przez przekładnie.Zjawiska te powodują powstawanie szumu siłowego w napędzie. Przy sterowaniu pozycją lubtrajektorią robota zjawiska te minimalizowane są dzięki temu, że masa robota i napędu działająjak filtr dolnoprzepustowy. Niestety, przy sterowaniu siłowym, powstający szum może byćźródłem problemów.Dobry napęd sterowany siłowo posiada kilka ważnych parametrów: szerokość pasma przenoszeniasiły, mechaniczna impedancja wyjściowa, zakres dynamiczny, gęstość energii, gęstośćsiły. Pierwsze trzy parametry są zależne od siebie, zazwyczaj poprawienie jednego powodujepogorszenie pozostałych.Szerokość pasma przenoszenia 1 siły jest parametrem mówiącym jak dobrze siłownikpotrafi generować pożądaną siłę. Pasmo musi być wystarczające by przenieść pożądaną siłęprzez strukturę robota. Większość zadań w sterowaniu siłowym wymaga pasma o szerokościzaledwie kilku herców. Jednak zwiększenie szerokości pasma siłownika powoduje polepszeniezdolności robota.Mechaniczna impedancja wyjściowa jest zdefiniowana jako minimalna wartość siły wyjściowejsiłownika potrzebna dla danego przesunięcia obciążenia. W uproszczeniu może być postrzeganajako sztywność siłownika. Dla idealnego siłownika wielkość ta powinna być równazero. Mała impedancja oznacza że napęd jest widziany przez robota jako źródło czystej siły zpomijalną dynamiką.Zakres dynamiczny jest stosunkiem maksymalnej wartości siły wyjściowej do wartościminimalnej zmiany siły wyjściowej. Parametr ten mówi jak bardzo siłownik jest czuły na małesiły w odniesieniu do pełnego zakresu możliwości siłowych wyjścia. Duży zakres dynamicznyjest pożądany, ponieważ pozwala stosować napęd w aplikacjach wymagających zarówno dużychczułości jak i dużych sił. Ludzie mają duży zakres dynamiczny.Gęstość siły i gęstość energii są parametrami ważnymi ze względu nie tyle na pracęsiłownika, ile na konstrukcję robota. Mówią o zdolności siłownika do generowania siły lub dostarczaniaenergii do robota w odniesieniu do masy i objętości napędu. Dla dużych wartościtych parametrów siłownik o niewielkich wymiarach i masie potrafi dostarczyć dużo energii. Takiewłasności są bardzo pożądane w robotach mobilnych, gdzie mała masa i niewielkie wymiarymają kluczowe znaczenie.2.1 Przeniesienie napęduW pracy zaproponowano napędzanie przegubu obrotowego siłownikiem linowym. Takie połączeniejest naturalne i często spotykane w organizmach żywych. Liniowy napęd (mięsień)napędza staw obrotowy (np. kolano). Warto więc zastanowić się w jaki sposób przenieść ruchpostępowy na obrotowy oraz jak w takim przypadku siła będzie transformowała się w moment.1 Przyjmuje się 3dB pasmo przenoszenia.6

Znając działająca siłę, moment można wyliczyć stosując prostą formułę:Iloczyn wektorowy (1) można zapisać w postaci:⃗M = ⃗r × ⃗ F (1)M = || ⃗ M|| = | ⃗ R|| ⃗ F | sin(θ) (2)Rozważmy prosty model kolana będącego przegubem obrotowym o jednym stopniu swobody, zdołączonym siłownikiem liniowym, rysunek 1. Siłownik przytwierdzony jest między punktamiRysunek 1: Model kolana z dołączonym siłownikiem liniowymA i B. W punkcie A wyjście siłownika łączy się z prętem |OA| o długości r k , będącym członemjednego z elementów przegubu (goleni). W punkcie B siłownik łączy się z drugim członemprzegubu (uda) przez pręt o długości L 2 . Punkt O jest punktem obrotu kolana.Korzystając z równania (2), moment w kolanie w punkcie O, można zapisać w postaci:M = ||⃗r × ⃗ F || = r k F sin(̸ OAB) (3)Dla zadanego momentu równanie na siłę wygląda następująco:gdzie kąt ̸OAB ma postać:F =Mr k sin(̸ OAB) , (4)̸ OAB = φ const + φ k − φ AB = φ const + φ k − arc tg r k sin(φ const + φ k ) − L 2L 1 + r k cos(φ const + φ k )Można nieco uprościć powyższe równania zakładając, że siłownik będzie zawsze równoległy douda (wtedy φ AB = 0) oraz że L 1 ≫ L 2 otrzymujemy wtedy:F =2.2 Siłownik z elastycznością szeregowąMr k sin(φ const + φ k )Standardowe napędy, takie jak np. silnik elektryczny z przekładnią redukcyjną, można przystosowaćdo zadania sterowania siłą poprzez zastosowanie czujnika obciążenia, którego sygnał(5)(6)7

zamyka pętlę sprzężenia zwrotnego układu regulacji. Standardowe czujniki obciążenia są projektowanetak by być sztywne dla danego obciążenia. Pozwala to przekazywać siły interakcjinie magazynując przy tym energii. Niestety, sztywne ogniwo w połączeniu z już dość sztywnymnapędem powoduje, że wzmocnienie w otwartej pętli jest bardzo duże. Oznacza to, że małezmiany położenia obciążenia powodują bardzo dużą zmianę siły w czujniku. Dla zapewnieniastabilności całości układu w zamkniętej pętli, wzmocnienie regulatora musi wtedy być bardzomałe. Małe wartości wzmocnienia regulatorów zazwyczaj nie zapewniają dobrej jakości sterowania.Małe wzmocnienia, duża impedancja napędu oraz sztywność czujnika sprawiają, że układnie jest odporny na duże przeciążenia oraz udary siłowe pochodzące z otoczenia. Ponadto niema własności filtrujących dla tarcia i nieliniowości przekładni, które występują w rzeczywistychaplikacjach.W laboratoriach MIT [1] powstał projekt napędu, w którym wspomniane niedogodnościzostały zminimalizowane a niektóre wręcz wyeliminowane. Napęd ten składa się z silnika elektrycznego,przekładni redukcyjnej oraz szeregowo dołączonej sprężyny o małej sztywności. Informacjao wyjściowej sile otrzymywana jest poprzez pomiar odkształcenia sprężyny. Tak zbudowanynapęd nazwano siłownikiem z elastycznością szeregową (ang. Series Elastic Actuator),rysunek 2. Siłownik taki topologicznie podobny jest do jakiegokolwiek napędu z czujnikiemRysunek 2: Minimalny model siłownika z elastycznością szeregowąobciążenia i układem sterowania z zamkniętą pętlą sprzężenia zwrotnego. Jednakże są dwiezasadnicze różnice wynikające z małej sztywności czujnika. Po pierwsze, sprężyna ma zdolnośćdo gromadzenia energii, czego nie można pominąć w rozważaniach teoretycznych. Po drugie,zintegrowanie czujnika o małej sztywności z niewielkim siłownikiem może być kłopotliwe.Podstawową zaletą elastyczności szeregowej jest to, że mała sztywność czujnika obniżawzmocnienie obiektu w układzie z zamkniętą pętlą sprzężenia zwrotnego. Można więc proporcjonalniezwiększyć wzmocnienie regulatora zachowując przy tym bezpieczny margines stabilnościukładu. Dzięki temu napęd z elastycznością ma małą impedancje wyjściową, jest odpornyna uderzenia siłowe oraz na zmianę obciążenia. Ponadto zwiększenie wzmocnienia regulatoraznacznie obniża negatywny wpływ tarcia oraz nieliniowości przekładni, w wyniku czego otrzymujemyniezakłócone wyjście siłowe.8

2.3 Model matematycznyRysunek 3 pokazuje elementy składowe siłownika z elastycznością szeregową. Szara liniaoddziela domenę sygnałową od domeny energetycznej. Podstawowymi elementami systemu są:• elastyczny sensor,• wejścia systemu.• sterownik,• silnik i przekładnia,Rysunek 3: Pełen model siłownika z elastycznością szeregową2.3.1 Elastyczny sensorElastyczny sensor składa się z dwóch części: sprężyny w części energetycznej oraz czujnikaprzesunięcia w części sygnałowej. Elastyczność zapewnia sprężyna ze stałą sztywności k s . Czujnikmierzy odkształcenie sprężyny reprezentujące siłę f l występującą w sprężynie. Zakłada się,że sprężyna nie ma histerezy, a zależność pomiędzy siłą a odkształceniem sprężyny zdefiniowanajest przez prawo Hooka:f l = k s x s (7)Odkształcenie sprężyny x s zdefiniowane jest jako różnica pomiędzy pozycją napędu x m i pozycjąobciążenia x l .x s = x m − x l (8)Niezalecane jest dokonywanie pomiaru x m , x l niezależnie i branie ich różnicy dla pomiaru siły.Lepszym rozwiązaniem jest bezpośredni pomiar odkształcenia elementu elastycznego czyli sprężyny.Zakłada się że pomiar jej odkształcenia jest wystarczającą reprezentacją siły wyjściowejsiłownika.2.3.2 Wejścia systemuW systemie wyróżnia się dwa wejścia:• f d - pożądana siła wyjściowa,9

• x l - przesunięcie obciążenia lub przegubu robota.Pożądana siła wyjściowa f d leży w części sygnałowej, jej wartość jest generowana przez sterowniknadrzędny. W idealnym przypadku siła wyjściowa siłownika jest równa sile f d co do amplitudyi fazy w całym zakresie spektrum częstotliwości.Przesunięcie obciążenia x l leży w części energetycznej. Podczas gdy siłownik używany jestw sytuacjach wymagających regulacji siły jego wyjście może być przytwierdzone do podłożalub do swobodnej masy poruszającej się w przestrzeni. Gdy wyjście przytwierdzone jest dopodłoża pozycja obciążenia jest stała. Jednakże gdy wyjście połączone jest z masą poruszającąsię w przestrzeni pozycja obciążenia definiowana jest jako funkcja siły w sprężynie oraz masyobciążenia. Istnieje energetyczna zależność pomiędzy sprężyną a obciążeniem, której nie możnapominąć.2.3.3 Silnik i przekładniaModel silnika w części energetycznej zawiera wzmacniacz, masę silnika oraz tarcie lepkie.Wzmacniacz wytwarza zadaną przez sterownik siłę działającą na masę silnika. Założono, żesiłownik jest w stanie wytworzyć dowolną pożądaną siłę wyjściową ograniczoną odgórnie siłąnasycenia F sat , oraz że dynamika wzmacniacza jest dostatecznie szybka by mogła być pominięta.W fizycznym prototypie siłownika wzmocnienie wzmacniacza K a równe jest jeden. Dla uproszczeniarównań K a zostało wciągnięte do wzmocnienia regulatora i wyeliminowane z dalszychrozważań.Zastępcza masa m m zawiera dynamiczną masę silnika oraz masę elementów przekładni widzianychpoprzez przełożenie przekładni. Należy pamiętać, że inercja widziana za pośrednictwemprzekładni, o przełożeniu N, jest N 2 razy większa.Tarcie lepkie b m również widziane jest poprzez przekładnie, i jest N 2 razy większe niż rzeczywistajego wartość.Do sterowania silnika użyto regulacji momentu napędowego.2.3.4 SterownikPętlę sprzężenia zwrotnego systemu zamyka pomiar siły wyjściowej siłownika. Uchyb regulacjipodawany jest na regulator PD ze wzmocnieniem K p części proporcjonalnej oraz K d częściróżniczkującej. Pamiętając wcześniejsze założenia o idealnym wzmacniaczu, siłę F m działającąna masę silnika można zdefiniować jako:F m (s) = (F d (s) − F l (s))(K p + K d s) (9)Do kontrolowania siłownika wystarczyłby regulator proporcjonalny, jednakże dodanie częściróżniczkującej pomaga zredukować oscylacje wynikające z obecności sprężyny.2.3.5 Model części energetycznejRysunek 4 przedstawia model części energetycznej siłownika z elastycznością szeregową.Można zdefiniować dwa zasadnicze równania dla części energetycznej opisujące siłę w sprężynieoraz dynamikę zastępczej masy silnika:f l = k s (x m + x l )m m ẍ m + b m ẋ m = f m − f l (10)10

Rysunek 4: Model części energetycznej siłownika z elastycznością szeregowąDokonując transformacji Laplace’a, równania można przepisać w postaci:F l (s) = k s (X m (s) + X l (s))X m (s) = F m(s) − F l (s)m m s 2 + b m sŁącząc równania (11), siłę w sprężynie F l można zapisać jako:F l (s) = F m(s) − (m m s 2 + b m s)X l (s)m mkss 2 + bmk ss + 1Równanie (12) opisuje dynamikę otwartej pętli dla siły F l w sprężynie jako funkcję siły silnikaF m oraz położenia obciążenia X l .2.3.6 Model z zamkniętą pętląJak widać na rysunku 3, otwartą pętlę części energetycznej moduluje sygnał zamykającypętlę siłowego sprzężenia zwrotnego. Łącząc model części energetycznej (równanie 12) z sygnałempochodzącym z regulatora (równanie 9) otrzymamy równanie dynamiki zamkniętej pętlidla siły w sprężynie:F l (s) = (K ds + K p )F d (s) − (m m s 2 + b m s)X l (s)m mkss 2 + bm+ksK dk ss + (K p + 1)Równanie dynamiki zamkniętej pętli dla siły F l w sprężynie jest funkcją siły zadanej F d i pozycjiobciążenia X l .Dla uproszczenia dalszej analizy modelu, warto zapisać równanie na siłę w sprężynie F l jakofunkcje niezależnych, izolowanych zmiennych. Można to uzyskać rozważając dwa przypadki.Pierwszy dotyczy sytuacji, w której koniec siłownika przytwierdzony jest na stałe do podłoża,co eliminuje z równania składnik odpowiadający za ruch obciążenia. Drugi dotyczy sytuacji, wktórej siła zadania jest stała oraz masa obciążenia porusza się swobodnie w przestrzeni. W tymprzypadku eliminujemy z dynamiki układu składnik, na który miała wpływ wartość zadanasiły.Rozważmy przypadek pierwszy. Dzięki wyeliminowaniu ruchu obciążenia, funkcja transmitancjipomiędzy siłą działającą na silnik F m a siłą na wyjściu siłownika F l może być zapisanaw postaci:F l (s)F m (s) = 1m m(14)kss 2 + bmk ss + 111(11)(12)(13)

Równanie (14) opisuje układ z otwartą pętlą sprzężenia zwrotnego. Po zamknięciu pętli sprzężeniazwrotnego funkcja transmitancji pomiędzy siłą zadaną F d a wyjściową siłą siłownika F lprzyjmuje postać:F l (s)F d (s) = (K d s + K p )m mkss 2 + bm+ksK (15)dk ss + (K p + 1)Dla siłownika z elastycznością szeregową wzmocnienie K p jest zazwyczaj bardzo duże, zatemmożna przyjąć że:K p + 1 ≃ K p (16)Przy dużym wzmocnieniu K p oraz dla małych częstotliwości, funkcja transmitancji jest bliskajedności. Granicznie, dla dużych częstotliwości zbliża się do zera. Wystąpienie zera w licznikuspowodowana jest obecnością części różniczkującej w regulatorze.Rozważmy teraz przypadek drugi, kiedy siła zadana jest stała oraz masa obciążenia poruszasię swobodnie. Równanie transmitancji na siłę na wyjściu w zależności od położeniaobciążenia X l , dla układu z zamkniętą pętlą, ma postać:F l (s)X l (s) = −(m m s 2 + b m s)m mkss 2 + bm+ksK dk ss + (K p + 1)Jest to impedancja wyjściowa układu. Impedancja dla małych częstotliwości jest równa zero.Dla dużych częstotliwości dąży ona do wartości k s , stałej sprężystości sprężyny.2.3.7 Model z zamkniętą pętlą z dołączonym obciążeniemPomimo tego, że podczas kontaktu z otoczeniem siłownik będzie miał zmienne warunki obciążenia,warto przeanalizować dynamikę układu z dołączoną masą poruszającą się swobodniew przestrzeni. Dla uproszczenia rozważań z równań zostało wyeliminowane tarcie lepkie. Rów-(17)Rysunek 5: Model siłownika z elastycznością szeregową z dołączonym obciążeniemnania (11) opisujące równowagę sił w sprężynie oraz dynamikę masy silnika, sprowadzają sięwtedy do:F l (s) = k s (X m (s) + X l (s))X m (s) = F m(s) − F l (s)m m s 2 (18)12

Równanie swobodnie poruszającej się masy (bez tarcia) w przestrzeni jednowymiarowej, wdziedzinie Lapalce’a, ma postać:X l (s) = F l(s)m l s 2 (19)Pamiętając, że siła F m działająca na silnik jest funkcją prawa sterowania:F m (s) = (K p + K d s)(F d (s) − F l (s)) (20)i podstawiając równania 18, 19, 20, otrzymamy równanie na transmitancję układu, jako funkcjęsiły w sprężynie F l w zależności od siły zadanej F d :F l (s)F d (s) =m mksK d s + K ps 2 + K d s + (K p + 1 + mmm l)Równanie (21), z dwoma wyjątkami, jest takie samo jak model dynamiki siłownika z zamkniętąpętlą sprzężenia zwrotnego (13). Pierwsza różnica, wynikająca z wcześniejszego założenia,polega na braku tarcia lepkiego. Druga polega na obecności elementu mmm lw mianowniku transmitancjiprzy wyrazie s 0 .Warte zapamiętania są dwie rzeczy odnośnie elementu s 0 mianownika. Po pierwsze, dziękitemu że K p ≫ 1, aktualne jest założenie (16). Po drugie, element mmm lstaje się znaczący tylkow przypadku, kiedy masa obciążenia bliska jest zastępczej masie silnika. Na szczęście takasytuacja jest bardzo rzadka, ponieważ zadaniem siłownika jest napędzanie przegubów robota,które zazwyczaj przenoszą znaczne masy.Zakładając, że masa obciążenia jest znacząca, można się odwołać do równania (18) i uzyskaćzależność położenia obciążenia od siły zadanej:X l (s)F d (s) = 1m l s 2m mksK d s + K ps 2 + K d s + (K p + 1 + mmm l)Równanie (22) składa się z dwóch części. Pierwszą część można interpretować jako dynamikęmasy obciążenia na którą działa siła zadana, F d (s) = m l s 2 X l (s). Druga pochodzi z równania(21) i reprezentuje funkcję transmitancji siłownika z zamkniętą pętlą sprzężenia zwrotnego.Równanie (22) potwierdza że, siłownik, w zakresie pasma przenoszenia, widziany jest jakoźródło czystej siły, tak długo jak masa obciążenia jest znacząca w stosunku do zastępczej masysilnika.3 Konstrukcja siłownikaOpis konstrukcji siłownika został podzielony na dwie części:• część mechaniczną, opisującą szczegóły połączeń poszczególnych elementów siłownika,• część elektroniczną opisującą budowę oraz zasadę działania sterownika, stopnia mocy orazzasilacza.3.1 Konstrukcja mechanicznaPomysł mechanicznej budowy siłownika został zaczerpnięty z pracy doktorskiej [1]. Zaproponowanatam konstrukcja została z powodzeniem zastosowana w wielu robotach kroczących(21)(22)13

i nie tylko, których krótki opis również można znaleźć w tej pracy. Na końcowy kształt mechanicznejczęści siłownika największy wpływ miały dwa czynniki. Pierwszy, siłownik liniowynapędzany silnikiem obrotowym wymagał przekładni zamieniającej ruch obrotowy na liniowy.Przekładnia musiała spełniać wiele warunków, najważniejsze z nich to małą masa oraz znacznaodwracalność. Drugi, to sposób mocowania elastycznego czujnika i przeniesienia napędu zwyjścia przekładni, przez sensor, na wyjście siłownika.Rysunek 6: Widok siłownikaWidok siłownika przedstawiony jest na rysunku 6. Siłownik w znacznej większości składasię z aluminiowych kształtek. Mocowanie czujnika siły oraz łożyska ślizgowe wykonane zostałyz tworzywa sztucznego. Przekładnia śrubowa, prowadnice oraz pierścień mocowania silnikawykonane są ze stali. Przekładnia śrubowa oraz prowadnice zostały zakupione w całości i wymagałynieznacznej obróbki w celach umożliwienia mocowania. Pozostałe elementy zostaływytworzone własnymi środkami dzięki pomocy Pana Ryszarda Krzywańskiego. Szczegółowerysunki techniczne poszczególnych elementów można znaleźć w dodatku. W skład siłownikawchodzą następujące elementy:1. Koder przyrostowo-impulsowy.2. Mocowanie kodera.3. Silnik BLDC firmy MAXON.4. Mocowanie silnika.5. Pierścień podtrzymujący mocowanie silnika.6. Tylna ściana siłownika.7. Tylna ściana efektora siłownika.8. Sprężyna.9. Karetka przekładni śrubowej.10. Nakrętka przekładni śrubowej11. Przednia ściana efektora siłownika.12. Uchwyty blokujące przednią ścianę siłownika.13. Śruba przekładni.14

14. Czujnik siły.15. Podstawa.16. Główne prowadnice siłownika.17. Prowadnice efektora siłownika.18. Przednia ściana siłownika.19. Tulejki łożyska ciernego.20. Efektor siłownika.Rysunek 7: Widok mocowania przedniego łożyskaRysunek 8: Widok mocowania tylnego łożyskaRysunki 7, 8 przedstawiają sposób zamocowania śruby przekładni (13) w przedniej (18) itylnej (6) ścianie siłownika. Śruba umieszczona jest w niewielkich łożyskach kulkowych, któreznajdują się w odpowiednich gniazdach ścian siłownika.Rysunek 9: Widok tyłu siłownikaRysunek 10: Widok przodu siłownikaŚruba w tylnej części łączy się z silnikiem przez sprzęg mechaniczny. Widok tyłu siłownikaprzedstawia rysunek 9. Widać na nim sposób mocowania silnika poprzez tuleję mocującą (4).15

Tuleja mocująca, dzięki specjalnym zakończeniom, przytwierdzona jest do tylnej ściany siłownika(6), pierścieniem (5). Z przodu siłownika śruba kończy się w gnieździe łożyska i nie wychodzipoza przednią ścianę siłownika (18) – rysunek 10.Do silnika (3), poprzez uchwyt (2), przymocowany jest koder przyrostowo-impulsowy (1).Dzięki połączeniu wałów silnika i kodera, możliwy jest pomiar przesunięcia z dużą dokładnością.Rysunek 11: Widok mocowania przekładni śrubowejRysunek 12: Widok karetki przekładni śrubowejW skład przekładni śrubowej wchodzi nakrętka (10), która wykonuje właściwy ruch liniowy.Nakrętka zawiera w sobie kulki, które podobnie jak w łożysku, zmniejszają negatywny wpływtarcia, ponadto przyczyniają się do zwiększenia odwracalności przekładni. Nakrętka pokazanajest na rysunku 12. Do nakrętki przymocowana jest karetka (9), rysunek 11, której ruch, poprzezsprężyny powoduje przekazanie napędu do efektora siłownika. Prowadnice efektora (17)Rysunek 13: Widok silnika firmy MAXON wraz z koderemprzyrostowo-impulsowynę efektoraRysunek 14: Widok uchwytu blokującego przednią ścia-siłownikaprzytwierdzone są na stałe do ściany (7). Ściana (11) przymocowana jest do prowadnic efektorapoprzez uchwyty blokujące (12) co widać na rysunku 14.Czujnik siły (14) to potencjometr liniowy umieszczony w obudowie z tworzywa sztucznego– rysunek 15. Obudowa czujnika została zaprojektowana tak, aby była możliwość korekcji po-16

Rysunek 15: Widok czujnika siłyRysunek 16: Widok sposobu mocowania czujnika siłyłożenia czujnika. Dzięki wciskowym uchwytom mocującym, można łatwo zdjąć czujnik np. wcelu konserwacji czy wymiany. Sposób mocowania czujnika widać na rysunku 16.3.1.1 Przeniesienie napęduW siłowniku z elastycznością szeregową napęd przenoszony jest na wyjście siłownika poprzezelastyczny sensor (sprężyny). Odpowiednie umieszczenie sprężyn było kluczową kwestią,wpływającą na konstrukcje mechaniczną siłownika.Rysunek 17: Widok karetki siłownika przy zerowej sile wyjściowejMoment obrotowy silnika (3), poprzez przekładnie śrubową (10, 13) zamieniany jest siłęlinową działającą na karetkę (9). Karetka znajduje się pomiędzy przednią (11) i tylna (7)ścianą efektora. Pomiędzy karetką a ścianami efektora znajdują się sprężyny (8). Sprężynyzostały wstępnie napięte tak, by przy maksymalnym wychyleniu karetki (9) nie rozprężały siędo końca. Dzięki temu powinny działać w liniowym zakresie pracy.Położenie karetki przy zerowej sile na wyjściu siłownika pokazane jest na rysunku 17.Pojawienie się siły na wyjściu siłownika spowoduje, zmianę położenia ścian efektora (7, 11)17

Rysunek 18: Widok karetki siłownika przy sile wyjściowejrównej -200Nwej równejRysunek 19: Widok karetki siłownika przy sile wyjścio-200Nwzględem karetki (9). Jedna z par sprężyn ulegnie wtedy sprężeniu, druga rozprężeniu. Zmienisię również pozycja czujnika siły (14), którego ruchoma część przytwierdzonej jest do karetki(9). Sytuację taką przedstawia rysunek 18, kiedy na efektor siłownika działa siła -200N orazrysunek 19, kiedy działa siła 200N. Siła dodatnia powoduje wsunięcie efektora siłownika, siłaujemna – jego wysunięcie.3.1.2 SilnikW siłowniku zastosowano 80W bezkomutatorowy silnik prądu stałego EC 32 118893 szwajcarskiejfirmy MAXON. Firma ta słynie z bardzo wysokiej jakości oferowanych wyrobów. SilnikEC32 dzięki niewielkim wymiarom, małej wadze i dużej mocy idealnie nadaje się do napędzaniasiłownika. Należy pamiętać że takie własności są pożądane ze względu na możliwość uzyskaniawysokich wskaźników gęstości energii oraz gęstości siły.Parametr Wymiar WartośćCałkowita długość mm 94Średnica mm 32Masa g 263Moc mechaniczna W 80Napięcie zasilania V 36obrmin14800Max. prędkość obrotowaMax. moment mNm 450Rezystancja faza–faza Ω 1.83Max. prąd przy 5000rpm A 2.7Max. sprawność % 78mNmStała momentuA23obrStała prędkościminV416Stała mechaniczna ms 6.9Bezwładność wału silnika gcm 2 20Tabela 1: Parametry silnika EC 32 118893 firmy MAXON18

Widok silnika przedstawiony jest na rysunku 13. Podstawowe parametry silnika zawieratabela 1. Silnik BLDC przy zapewnieniu komutacji zachowuje się identycznie jak zwykły silnikkomutatorowy prądu stałego. Stosuje się te same modele, wzory i charakterystyki co dlazwykłego silnika DC, z tych względów nie będą one tu przytaczane. Więcej informacji teoretycznychdotyczących silników BLDC oraz sposobów ich sterowania można znaleźć na stroniefirmy MAXON [13].Silnik posiada wbudowane trzy czujniki Halla umożliwiające rozróżnienie sześciu przedziałówbezwzględnych położeń wału. Informacja z czujników wykorzystywana jest przez algorytmkomutacji. Wał silnika posiada przedłużenie, do którego można przymocować np. koderprzyrostowo-impulsowy w celu pomiaru położenia wyjścia siłownika.3.2 Konstrukcja elektronicznaSkonstruowany siłownik z elastycznością szeregową jest napędem elektrycznym wymagającymdo poprawnej pracy układu elektronicznego sterującego jego zachowaniem. Układ elektronicznyodpowiedzialny jest za całą domenę sygnałową siłownika oraz za małą część domenyenergetycznej. W części sygnałowej układ elektroniczny dokonuje pomiaru siły i położenia, wyliczasygnały sterujące oraz komunikuje się z użytkownikiem. W domenie energetycznej układelektroniczny steruje zastępczą masą silnika, poprzez wytworzenie siły F m .Rysunek 20: Schemat blokowy układu elektronicznego siłownika19

Schemat blokowy układu elektronicznego siłownika przedstawiony jest na rysunku 20. Sercemukładu jest moduł DSP56F8323, skupiający wszystkie sygnały pomiarowe oraz sterującesiłownika. Moduł odpowiedzialny jest za pomiary, sterowanie oraz komunikacje z użytkownikiem.Jednym z sygnałów pomiarowych jest odfiltrowany oraz wzmocniony sygnał z czujnikasiły, który podawany jest na jeden z kanałów przetwornika A/C modułu. Na drugie wejścieprzetwornika podawany jest odfiltrowany oraz wzmocniony sygnał z układu pomiaru prądu będącegoczęścią układu wykonawczego. Zadaniem układu wykonawczego jest dostarczenie energiido silnika. Sygnały sterujące układem wykonawczym pochodzą z bloku PWM oraz GPIO modułuDSP56F8323. Sygnały z czujników Halla zamontowanych na silniku, wykorzystywane przykomutacji, podawane są na blok TIMER-C modułu. Sygnały z kodera impulsowego, wykorzystywanedo określenia położenia wyjścia siłownika, podawane są na blok QDEC modułu. Modułkomunikuje się z komputerem nadrzędnym poprzez blok SCI. Blok JTAG modułu poprzez interfejsJTAG połączony jest nadrzędnym komputerem PC. Wykorzystuje się go przy ładowaniu,debugowaniu i uruchamianiu oprogramowania.Układ elektroniczny został zmontowany na trzech płytkach uniwersalnych, które zostałynazwane:• sterownik (rysunki 22, 23),• stopień mocy (rysunki 24, 25),• zasilacz (rysunki 26, 27).Schemat połączeń między nimi widać na rysunku 21.Rysunek 21: Schemat połączeń modułów ze sobą20

Rysunek 22: Płytka sterownika – strona wierzchniaRysunek 23: Płytka sterownika – strona spodniaRysunek 24: Płytka stopnia mocy – strona wierzchniaRysunek 25: Płytka stopnia mocy – strona spodniaRysunek 26: Płytka zasilacza – strona wierzchniaRysunek 27: Płytka zasilacza – strona spodniaUżycie płytek uniwersalnych podyktowane było względami praktycznymi. Układ elektronicznyewoluował, zmieniał się podczas prac nad siłownikiem. Czasami konieczne były zmianyoraz korekcje poprawiające jego funkcjonalność oraz niezawodność. Montaż na płytkach uniwer-21

salnych pozwalał łatwo wprowadzać wszelkie poprawki, co nie byłoby możliwe przy projekciegotowej płytki drukowanej. Poza tym, układ zmontowany na płytce uniwersalnej swoją funkcjonalnościąi odpornością na czynniki zewnętrzne nie ustępuje układom zbudowanym na płytkachdrukowanych.3.2.1 SterownikSterownik odpowiedzialny jest za pomiary, obliczenia, komunikację oraz generowanie poprawnychsygnałów sterujących dla stopnia mocy.Sercem układu jest moduł DSP56F8323 [3] z mikrokontrolerem sygnałowym MC56F8323firmy Freescale. Schemat ideowy modułu przedstawiono na rysunku 28.Rysunek 28: Schemat ideowy modułu DSP56F8323.Wykorzystanie w pracy szybkiego mikrokontrolera sygnałowego spowodowane było kilkomawzględami. Po pierwsze, w siłowniku zastosowano wysokoobrotowy bezkomutatorowy silnikprądu stałego BLDC (ang. Brush-Less DC ), wymagający do poprawnej pracy szybkiego komutatoraelektronicznego zdolnego wytworzyć trzy fale prostokątne przesunięte względem siebie o120 ◦ . Po drugie, w układzie steruje się momentem silnika poprzez regulację prądu jego uzwojeń.Do stabilizacji wartości prądu zastosowano programowy regulator PID, oraz modulacjęwypełnienia impulsu (PWM). Po trzecie, regulacja wartości wyjściowej siły siłownika wymagazastosowania programowego regulatora PD. Po czwarte, układ musi zapewniać działaniew czasie rzeczywistym, zbierać pomiary, generować sygnały sterujące oraz komunikować się zużytkownikiem.Zastosowany mikrokontroler MC56F8323 idealnie nadaje się do tego zadania, ponieważ jednąz dziedzin jego zastosowania jest sterowanie wszelkiego typu silnikami, w tym BLDC. Ponadto22

cechuje się dużą szybkością działania, bogatą listą instrukcji przyśpieszających operacje matematyczne,wszechstronnym zestawem układów peryferyjnych rozszerzających dodatkowo zakresjego zastosowań oraz niewielką obudową LQFP64 pozwalającą na miniaturyzację układu. Otokrótka charakterystyka jego możliwości:• jednostka centralna:– 16-bitowy rdzeń zgodny z rodziną 56800E o podwójnej architekturze harwardzkiej,– 60MIPS przy częstotliwości zegara 60MHz,– jednotaktowy 16x16-bitowy równoległy MAC (ang. Multiplier-Accumulator),– cztery 36-bitowe akumulatory zawierające bity rozszerzeń,– jednostka operacji bitowych,– zestaw instrukcji wspierających operacje równoległe z unikalnymi trybami adresowania,– sprzętowe pętle DO i REP,– trzy wewnętrzne magistrale adresowe,– cztery wewnętrzne magistrale danych,– tryby adresowania i instrukcje typowe dla mikrokontrolerów,– efektywny kod wynikowy dla programów pisanych w C,– programowy stos dla podprogramów i przerwań o głębokości ograniczonej tylko przezdostępną pamięć,– interfejs JTAG/EOnCE umożliwiający debugowanie i programowanie,• zasoby pamięciowe:– 32kB pamięci programu Flash,– 4kB pamięci programu RAM,– 8kB pamięci danych Flash,– 8kB pamięci danych RAM,– 8kB pamięci Boot Flash,• moduł PWM:– 6 kanałów PWM: tryb niezależny; tryb par komplementarnych; tryb mieszany,– 15-bitowa rozdzielczość,– tryb symetryczny (ang. Center-Aligned) i asymetryczny (ang. Left-Aligned)– możliwość programowania czasu martwego,– 3 wejścia wyłączania awaryjnego (ang. Fault Input),– korekcja wypełnienia niezależnie dla górnego i dolnego sygnału PWM przez trzy wejściastanu prądu (ang. Current Sense Input) lub programowa,– możliwość ustawienia polaryzacji niezależnie dla każdego kanału,– możliwa modulacja 0% i 100%,23

• dwa przetworniki analogowo-cyfrowe (ADCs):– 12-bitowa rozdzielczość,– jednoczesny pomiar z dwóch wejść lub pomiar sekwencyjny do ośmiu próbek,– maksymalna częstotliwość zegara ADC: 5MHz,– możliwość pomiarów różnicowych,– możliwość korekcji próbek przez odejmowanie wcześniej zaprogramowanego przesunięcia,– możliwość zsynchronizowania pomiarów z modułem PWM,– możliwość generowania przerwań przy: końcu pomiaru, przekroczeniu zakresu pomiarowego,przejściu sygnału przez zero,• dwa moduły timerów (TMR):– cztery 16-bitowe układy czasowo-licznikowe,– możliwość zliczania w obu kierunkach,– możliwość łączenia kaskadowego,– możliwość liczenia modulo,– zliczanie pojedyncze lub ciągłe,– praca z ustaloną wartością początkową licznika,– oddzielny preskaler dla każdego układu czasowo-licznikowego,– funkcje IC (ang. Input Capture) i OC (ang. Output Compare) dla każdego z układówczasowo-licznikowych,• jeden dekoder kwadraturowy:– cyfrowe filtry na wejściach, zapewniające zliczanie tylko poprawnych zboczy,– 32-bitowy rejestr pozycji,– 16-bitowy rejestr różnicy pozycji (aktualnej i ostatnio odczytanej),– maksymalna częstotliwość zliczania równa częstotliwości magistrali IPBus ,– rejestr pozycji może być inicjalizowany programowo lub przez zewnętrzne zdarzenie,– 16-bitowy licznik obrotów w możliwością przeładowania,– możliwość podłączenia wejść do licznika ogólnego przeznaczenia w celu pomiaru niewielkichprędkości,– układ czuwający (ang. watchdog) wykrywający brak sygnału wejściowego,– opcjonalnie, możliwość pracy w trybie PPA (ang. Phase Pulse Accumulator),• wbudowany czujnik temperatury (dioda),• moduł FlexCAN (wersja 2.0 A/B),• dwa szeregowe interfejsy komunikacyjne SCI (ang. Serial Communication Interface),• dwa szeregowe interfejsy SPI (ang. Serial Peripherial Interface),24

• układ czuwający COP (ang. Computer-Operating Properly),• jedno wejście zewnętrznego przerwania,• 27 wejść/wyjść ogólnego przeznaczenia GPIO (ang. General Purpose I/O),• wbudowany układ resetu przy włączeniu zasilania oraz moduł generujący przerwanie przyzbyt niskim napięciu zasilania,• interfejs JTAG/EOnCE (ang. Enhanced On-Chip Emulation) umożliwiający debugowanieprocesora w czasie rzeczywistym,• pętla synchronizacji fazowej (PLL) sterowana programowo,• wbudowany generator sygnału zegarowego,• wykonany w technologii CMOS o wysokim stopniu odporności, wejścia tolerują napięciado 5V, zgodne z logiką TTL,• wbudowany stabilizator napięcia 2.6V do zasilania wewnętrznej logiki i pamięci,• dostępne tryby pracy energooszczędnej (Wait i Stop),• możliwość wyłączenia dowolnego układu peryferyjnego dla oszczędności energii.Złącze Z1Złącze Z21 AGND 1 GND2 VCC 2 VCC3 CAN RX (GPIOC2) 3 PHASEA0/TA0 (GPIOB7)4 CAN TX (GPIOC3) 4 PHASEB0/TA1 (GPIOB6)5 TC3 (GPIOC4) 5 INDEX0/TA2 (GPIOB5)6 TC1/RXD0 (GPIOC5) 6 HOME0/TA3 (GPIOB4)7 TC0/TXD0 (GPIOC6) 7 XTAL (GPIOC1)8 RESET 8 EXTAL (GPIOC0)9 PWMA0 (GPIOA0) 9 V REFH10 PWMA1 (GPIOA1) 10 ANA711 PWMA2/SS1 (GPIOA2) 11 ANA612 PWMA3/MISO1 (GPIOA3) 12 ANA513 PWMA4/MOSI1 (GPIOA4) 13 ANA414 PWMA5/SCLK1 (GPIOA5) 14 ANA315 IRQA 15 ANA216 FaultA0 (GPIOA6) 16 ANA117 FaultA1 (GPIOA7) 17 ANA018 FaultA2 (GPIOA8) 18 SCLK0 (GPIOB3)19 ISA0 (GPIOA9) 19 MOSI0 (GPIOB2)20 ISA1 (GPIOA10) 20 MISO0/RXD1 (GPIOB1)21 ISA2 (GPIOA11) 21 SS0/TXD1 (GPIOB0)22 – 22 –23 VCC 23 VCC24 GND 24 AGNDTabela 2: Złącza Z1 i Z2 modułu DSP56F832325

Większość wyprowadzeń ma więcej niż jedną funkcję. Wybór funkcji odbywa się przez programowanieodpowiednich rejestrów konfiguracyjnych. Więcej informacji na temat możliwościmodułu DSP56F8323 oraz samego mikrokontrolera można znaleźć w raporcie [3] oraz w notachaplikacyjnych [6], [7], [8]. Tabela 2 zawiera opis złącz Z1 i Z2 modułu DSP56F8323.Moduł DSP56F8323 to niewielka dwustronna płytka drukowana, która umieszczona jest wzłączach Z1 i Z2 płytki sterownika. Schemat ideowy płytki sterownika przedstawia rysunek 29Rysunek 29: Schemat ideowy sterownikaZłącze Z3 (tabela 3) służy do połączenia sterownika ze stopniem mocy. Skupia ono większośćsygnałów sterujących oraz pomiarowych całego układu.Nr. Sygnał Typ Opis1 DIR1 O Sygnał kierunku dla półmostka nr 12 DIR2 O Sygnał kierunku dla półmostka nr 23 DIR3 O Sygnał kierunku dla półmostka nr 34 EN1 O Sygnał włączający półmostek nr 15 EN2 O Sygnał włączający półmostek nr 26 EN3 O Sygnał włączający półmostek nr 37 PWR O Sygnał włączający zasilanie stopnia mocy8 HALL1 I Sygnał z czujnika Halla nr 19 HALL2 I Sygnał z czujnika Halla nr 210 HALL3 I Sygnał z czujnika Halla nr 311 F SENSE I Sygnał z czujnika siły12 I SENSE I Sygnał z czujnika prądu silnika13 +3.3V PWR Zasilanie +3.3V14 +5V PWR Zasilanie +5V15 GND PWR Masa16 GND PWR MasaTabela 3: Złącze Z3 na płytce sterownika26

Złącze Z6 umożliwia połączenie sterownika z komputerem nadrzędnym wyposażonym winterfejs RS-232. Układ MAX3232 (IC3) to przetwornica pojemnościowa, odpowiedzialna zadopasowanie napięć pomiędzy interfejsem RS-232 a płytką sterownika, której część logicznazasilana jest napięciem 3.3V.Złącze Z7 służy do podłączenia sygnałów z optycznego kodera przyrostowo-impulsowego.Opis złącza znajduje się w tabeli 4. Układ 74LVX14 (IC4) służy jako bufor dopasowującypoziomy napięć pomiędzy enkoderem (5V) a sterownikiem (3.3V).Nr. Sygnał Opis1 GND Masa2 +5V Zasilanie +5V3 PHASEA Sygnał dla kanału A dekodera kwadraturowego4 PHASEB Sygnał dla kanału B dekodera kwadraturowego5 INDEX Sygnał indeksu dla dekodera kwadraturowegoTabela 4: Złącze Z7 na płytce sterownikaDwie diody LED, żółta i czerwona, służą do sygnalizacji stanu pracy układu. TranzystoryT1 i T2 służą jako bufory pomiędzy diodami a wyjściami portu.Kondensatory C18-C22 oraz dławiki L3-L7 tworzą filtry dolnoprzepustowe filtrujące napięciazasilania modułu DSP56F8323 oraz stopnia mocy.Ponadto na płytce sterownika znajdują się trzy identyczne układy generujące opóźnienia dlasygnałów sterujących stopniem mocy. Zabezpiecza to poszczególne półmostki w stopniu mocyprzed zwarciem górnego i dolnego klucza przy zmianie sygnału sterującego (tzw. ShotThrough).Należy pamiętać, że czas wyłączenia tranzystora mocy jest większy niż czas jego załączenia.Trzeba więc wprowadzić opóźnienie pomiędzy wyłączeniem jednej z gałęzi półmostka z załączeniemdrugiej. Układ z rysunku 30 realizuje to zadanie.Rysunek 30: Schemat ideowy układu generującego opóźnienie dla sygnału sterującegoprzy zmianie kierunkuSygnał kierunku DIR podawany jest na dwa generatory monostabilne znajdujące się wukładzie IC5 (74HCT123). Jeden z nich reaguje na zbocze opadające drugi na narastające.Długość impulsu dla obydwu generatorów ustawiona została na ok. 8µs. Jest to czas, w którym27

tranzystory mocy będą w stanie bezpiecznie się wyłączyć. Sygnały z wyjść Q układu IC5podawane są na bramkę NAND (74HCT132) układu IC7. Aktywne zbocze powoduje pojawieniesię na wyjściu Q ujemnego impulsu. Na wyjściu bramki NAND stan wysoki pojawi się wtedygdy jeden z generatorów wygeneruje ujemny impuls. Nastąpi to przy każdej zmianie stanu nalinii DIR. Wyjście bramki NAND połączone jest z jednym z wejść bramki OR (74HCT32)IC6. Na jej drugie wejście podawany jest sygnał P W M sterujący półmostkiem. Stan wysokina wyjściu bramki NAND powoduje, że na wyjściu bramki OR również jest stan wysoki, którywyłącza półmostek.Płytka sterownika wyposażona jest ponadto w lokalny zasilacz dostarczający napięć 5V oraz3.3V. Schemat ideowy lokalnego zasilacza przedstawia rysunek 31. Układ zbudowany jest wRysunek 31: Schemat ideowy zasilacza na płytce sterownikaoparciu o dwa scalone stabilizatory liniowe (Low-Drop), LM2940 (IC1) – dający napięcie 5Voraz LM1085 (IC2) dający napięcie – 3.3V. Złącze Z4 służy do podłączenia zewnętrznego,niestabilizowanego źródła napięcia o wartości nie przekraczającej 10V, np. z zasilacza wtyczkowego.Napięcie to podawane jest na mostek prostowniczy B1, dzięki czemu polaryzacja napięciawejściowego może być dowolna. Dalej napięcie podawane jest na stabilizator IC1. Napięcie 5Vmoże być również doprowadzone do układu poprzez złącze Z5. Należy wtedy ustawić zworęJP1 w położenie 2-3. Dioda D1 sygnalizuje obecność napięcia +5V w układzie. Napięcie 3.3Vwytwarzane jest z napięcia 5V. Napięcie 5V podawane jest na filtry dolnoprzepustowe zbudowanez elementów L1, C5, C9 oraz L2, C6, C10. Filtry zapobiegają przedostawaniu się doukładu składowych wysokoczęstotliwościowych. Dalej napięcie podawane jest na stabilizatorIC2. Dioda D2 sygnalizuje obecność napięcia 3.3V w układzie.3.2.2 Stopień mocyStopień mocy odpowiedzialny jest za dostarczenie energii do silnika. W siłowniku zastosowanosilnik BLDC firmy Maxon o parametrach podanych w tabeli 1. Do poprawnej pracy silnikwymaga trzech sygnałów napięciowych, o amplitudzie maksymalnie 36V i polaryzacji zależnejod położenia wału silnika. Należy pamiętać, że silnik może pracować z maksymalną prędkością28

14000 obr , dlatego częstotliwość przełączania sygnałów może być duża. Stopień mocy musi więcminbyć dostatecznie szybki, by umożliwić silnikowi pracę na pełnych obrotach.Projekt pierwszego stopnia mocy opierał się na scalonym trzyfazowym stopniu mocy L6234firmy ST. Układ L6234 mógł pracować przy napięciu zasilania do 52V, prądzie do 4A i częstotliwościkomutacji do 50kHz. Dodatkowo posiadał wbudowaną logikę do sterowania kluczamimocy, diody ochronne oraz zabezpieczenie termiczne. Podczas testów okazało się jednak, żeukład nie jest odporny na chwilowe skoki prądowe występujące przy częstej zmianie kierunkuobrotów silnika. Niestety, takie zjawisko w siłowniku jest powszechne. Z tych względów należałozaprojektować stopień mocy odporny na przeciążenia, potrafiący pracować przy podobnychwarunkach zasilania, obciążenia i częstotliwości komutacji.Zdecydowano się na budowę dyskretnego stopnia mocy złożonego z trzech identycznych,niezależnie sterowanych, półmostków mocy. Schemat ideowy stopnia mocy przedstawia rysunek32. Na schemacie widać trzy identyczne układy logiczne zbudowane na bramkach OR 74HCT32Rysunek 32: Schemat ideowy stopnia mocy(IC4), NOR 74HCT02 (IC5) oraz NOT 74HCT14 (IC3). Każdy z nich służy do sterowaniagórnym i dolnym kluczem w odpowiednim półmostku, z wykorzystaniem dwóch sygnałów DIRxi ENx, gdzie x=1...3. Logiczna jedynka na linii DIR włącza górny klucz, logiczne zero dolny.Stan niski na linii EN powoduje włączenie półmostka, stan wysoki jego wyłączenie.Na płytce stopnia mocy znajduje się ponadto układ pomiaru prądu zbudowany w oparciu owzmacniacze operacyjne IC1C, IC1D. Napięcie z rezystora pomiarowego 0.1Ω R22 podawanejest na wzmacniacz różnicowy IC1C. Wzmocnienie wzmacniacza ustawione jest na poziomie5 V co daje zależność napięcia od prądu na poziomie 0.5 V , na wyjściu wzmacniacza. NapięcieV Az wyjścia wzmacniacza podawane jest na podwójny filtr dolnoprzepustowy RC zbudowanyz elementów R19, C7, C8 oraz R20, C9, C10. Filtr ma zadanie ograniczyć pasmo sygnałupodawanego na przetwornik A/C. Za filtrem znajduje się wtórnik napięciowy zbudowany na29

drugim wzmacniaczu IC1D. Zadaniem wtórnika jest buforowanie sygnału pomiarowego orazdopasowanie impedancji między źródłem a odbiornikiem.Przekaźnik K1 służy do włączenia napięcie zasilającego stopnie mocy. Sygnał P W R ze złączaZ1 podawany jest na bramkę AND 74HCT08 (IC2D) która steruje bazą tranzystora T1.Wykorzystanie tranzystora w tym miejscu było konieczne ze względu na to że przekaźnik wymagado załączenia prądu około 100mA. Dioda D1 tłumi wszelkie przepięcia mogące pojawićsię na uzwojeniu przekaźnika.Na płytce stopnia mocy umieszczono również układ pomiarowy dla siły zbudowany w oparciuo wzmacniacze operacyjne IC1A i IC1B. Sygnał z czujnika siły (potencjometru liniowego)podawany jest przez złącze Z2 na wtórnik napięcia zbudowany na wzmacniaczu IC1A. Z wyjściawzmacniacza sygnał podawany jest na, podobny jak poprzednio, filtr dolnoprzepustowyzbudowany z elementów R12, C1, C2 oraz R13, C3, C4. Za filtrem również znajduje się wtórniknapięciowy zbudowany w oparciu o drugi wzmacniacz IC1B.Złącze Z3 służy do podłączenia sygnałów z czujników Halla zamontowanych na silniku. Wyjściaczujników Halla są typu otwarty kolektor dlatego konieczne jest zastosowanie rezystorówR8-R10 wymuszających stan wysoki na wejściach bramek. Bramki AND z układu IC2 służąjako bufory dla poszczególnych sygnałów.Układ zbudowany w oparciu o elementy T2, D3, D2, R1 oraz C11 jest źródłem napięciaodniesienia 2.7V wymaganego przez półmostki mocy.Półmostki mocy (rysunek 33) zostały zbudowane w oparciu o projekt mgr. Marka Kabały, ichopis znaleźć można w pracy dyplomowej Roberta Szlawskiego [5]. Układ został przystosowanyRysunek 33: Schemat ideowy półmostka mocydo zasilania napięciem 36V. Klucze mocy sterowane są niezależnie logicznym sygnałem w standardzieTTL. Stan niski na linii CT R − 1U włącza górny klucz, stan wysoki na linii CT R − 1Dwłącza klucz dolny. Sterowanie sygnałami logicznymi TTL możliwe jest dzięki spolaryzowaniubaz tranzystorów T4 i T5, pracujących w układzie OB, napięciem 2.7V. Tranzystory T3, T7oraz T6, T8 pracują w układzie OC, ich zadaniem jest szybkie dostarczanie i odbieranie ładunkówz bramek tranzystorów mocy MOSFET. Diody D6 i D7 zabezpieczają tranzystory mocyprzed wszelkiego typu przepięciami, mogącym się pojawić przy pracy silnika.30

3.2.3 ZasilaczDo zasilania całości układu wykorzystano zasilacz, którego schemat ideowy widać na rysunku34. Zasilacz dostarcza niestabilizowane napięcie +36V do zasilania silnika oraz stabilizowaneRysunek 34: Schemat ideowy zasilaczanapięcie +5V do zasilania części elektronicznej układu. Napięcie +36V uzyskiwane jest poprzezwyprostowanie i odfiltrowanie napięcia zmiennego 24V dostarczonego do układu przez złączeZ1. Dla zmniejszenia tętnień napięcia zastosowano kondensator C1 o dużej pojemności. Napięcie+36V doprowadzone jest do złącza Z2 zasilacza. Stabilizowane napięcie +5V wytwarzanejest z napięcia +36V dzięki zastosowaniu przetwornicy impulsowej L4960 (IC1). Przetwornicapracuje w standardowej konfiguracji układu obniżającego (Step down). Więcej informacji na jejtemat można znaleźć w nocie aplikacyjnej [12]. Napięcie +5V doprowadzone jest do złącza Z3zasilacza.4 OprogramowanieOprogramowanie mikrokontrolera MC56F8323 zostało napisane w języku C, w dedykowanym,zintegrowanym środowisku CodeWariorr DSP firmy Metrowerks. Procesory sygnałowe, zewzględu na swoją architekturę oraz specyficzny zakres zastosowań, pełnię możliwości osiągająz oprogramowaniem napisanym w języku asemblera danego procesora. W pracy zdecydowaliśmysię przygotować oprogramowanie w języku C z kilku względów. Po pierwsze, wykorzystanyprocesor sygnałowy, dzięki swojej bogatej i różnorodnej liście instrukcji, pozwalał na osiągnięcieefektywnego wynikowego kodu dla programów przygotowanych w języku C. Ponadto językC charakteryzuje się większą uniwersalnością oraz przenośnością. Łatwiej jest tworzyć większeaplikacje, złożone z rozbudowanych struktur danych oraz wielu podprogramów. Analizaprogramu jest o wiele prostsza, łatwiej jest wprowadzać poprawki oraz usuwać błędy. Nie dasię ukryć, że asembler zastosowanego mikrokontrolera jest bardzo obszerny, opanowanie go wstopniu umożliwiającym napisanie oprogramowania sterującego siłownikiem byłoby zbyt czasochłonnew stosunku do możliwych korzyści. Warto również wspomnieć o tym, że wraz ześrodowiskiem CodeWarrior dostarczone zostały gotowe funkcje napisane w języku C, pozwalająceszybko i efektywnie korzystać z zasobów i możliwości użytego mikrokontrolera.31

4.1 Oprogramowanie uruchomieniowe CodeWariorrOprogramowanie sterownika siłownika zostało w całości przygotowane w zintegrowanym środowiskuprogramistycznym CodeWarrior Development Studio for 56800/E Hybrid Controllersver. 6.0 firmy Metrowerks. Wybór ten podyktowany był kilkoma względami. Po pierwsze, dostępnajest darmowa licencja umożliwiająca tworzenie w pełni funkcjonalnych aplikacji, którychwynikowy kod nie przekracza rozmiaru 8kB. Po drugie, nie znaleziono innego, równie wszechstronnego,darmowego środowiska uruchomieniowego dla procesorów rodziny MC56F8xxx. Potrzecie, interfejs JTAG [4] używany przy debugowaniu, został uruchomiony właśnie dla tegośrodowiska.Rysunek 35: CodeWarrior IDE (ang. Integrated Development Environment)Zintegrowane środowisko programistyczne CodeWarrior (rysunek 35) dostarcza użytkownikowinarzędzi i usług umożliwiających zarządzanie, tworzenie i testowanie oprogramowania.Najważniejsze z nich to:• rozbudowane zarządzanie projektem,• zaawansowany edytor kodu źródłowego, z rozpoznawaniem i uzupełnianiem składni językaC,• zintegrowany debugger i symulator,32

• narzędzie zarządzające budowaniem kodu make (automake),• skrośny kompilator języka C, asembler i linker dla DSP56800E.• Processor Expert dla MC56Fxxxx.Rysunek 36: CW IDE z widocznym oknem debuggera Rysunek 37: CW IDE z widocznym oknem konfiguracjibeanaRysunek 36 przedstawia IDE CodeWarrior z widocznym oknem debuggera, oknem zmiennychglobalnych, oknem rejestrów procesora oraz mapą pamięci.Bardzo pomocnym narzędziem przy tworzeniu aplikacji jest Processor Expert. Dzięki niemumożliwe jest szybkie przygotowywanie w pełni funkcjonalnych programów bez koniecznościpełnej znajomości zasobów użytego mikrokontrolera. Aplikacje składa się z komponentów nazywanychBeans. Beany posiadają graficzny interfejs użytkownika (rysunek 37), za pomocąktórego konfiguruje się poszczególne składowe systemu. Programista nie musi zagłębiać się zzawiłą i rozbudowaną dokumentację mikrokontrolera i jego układów peryferyjnych. Beany dostarczająrównież metod umożliwiających korzystanie z wybranych zasobów mikrokontroleralub funkcji, które z jego pomocą można zrealizować. Po zdefiniowaniu składowych systemu iwstępnej konfiguracji Beanów, Processor Expert wygeneruje kod w języku C zależny od typuwybranego mikrokontrolera, zawierający inicjalizację warstwy sprzętowej oraz metody realizującewybrane funkcje. Użytkownik może się wtedy skupić wyłącznie na realizacji właściwegozadania. Processor Expert dostarcza również narzędzi, które na bieżąco śledzą zużycie zasobówmikrokontrolera i w graficzny sposób przedstawiają to użytkownikowi (rysunek 35, oknoz dużym błękitnym kwadratem – zużycie nóżek procesora, okno w prawym górnym rogu –zużycie zasobów mikrokontrolera Resource Meter). W projekcie oprogramowania dla siłownikawykorzystano następujące beany:• QuadratureDecoder — obsługa wbudowanego dekodera kwadraturowego,• PWMMC — obsługa modułu PWM przystosowanego do sterowania silnikami,• ADC — obsługa wbudowanego przetwornika A/C,• BitIO — obsługa pojedynczych linii GPIO,33

• AsynchroSerial — obsługa asynchronicznej transmisji szeregowej,• Capture — obsługa funkcji IC timera,• TimerInt — obsługa przerwania cyklicznego,• 56F8323 — obsługa zastosowanego mikrokontrolera (główny bean systemu),Podczas uruchamiania oprogramowania okazało się że niektóre z beanów generują kod, któryniepoprawnie konfiguruje odpowiadające mu zasoby. Bean o nazwie PWMMC niepoprawnieustawiał rejestry PMDISMAP1 i PMDISMAP2 [8] mikrokontrolera. Bean o nazwie AsynchroSerialniepoprawnie implementował bufory cykliczne FIFO. Zdarzało się, że przy zerowej długościkolejki, wskaźniki zapisu i odczytu wskazywały różne miejsca w buforze. Najwięcej problemówsprawiała #pragma interrupt called, poprzedzająca każdą funkcję wywoływaną w obsłudze przerwania.Okazało się, że nie składała ona na stos wszystkich rejestrów procesora, które wykorzystywanebyły w przerwaniu. Aby temu zaradzić zamiast tego zastosowano #pragma interruptsaveall.4.2 Struktura oprogramowania sterownikaOprogramowanie sterownika siłownika realizuje wiele zadań wykonywanych równolegle, niezależnieod siebie. Główne z nich to:• sterowanie silnikiem BLDC - komutacja,• regulacja siły wyjściowej siłownika - algorytm PD,• regulacja prądu silnika - algorytm PID,• pomiar siły, prądu oraz przesunięcia,• gromadzenie danych pomiarowych z zadanym krokiem,• zadawanie wymuszenia na obiekt z zadanym krokiem,• komunikacja z użytkownikiem,Rysunek 38 przedstawia uproszczony diagram głównej pętli programu. Realizowane są w niejzadania o najniższym priorytecie z punktu widzenia regulacji siły oraz kontroli siłownika. Możnapowiedzieć, że zadania realizowane w tej części oprogramowania są najmniej wrażliwe czasowo.Przerwanie ich i wykonanie innego zadania nie wpływa na jakość regulacji oraz stabilnośćukładu. Do zadań realizowanych w głównej pętli programu należą: inicjalizacja zmiennych iurządzeń, kalibracja czujnika, komunikacja z użytkownikiem, gromadzenie danych pomiarowychoraz zadawanie sterowań na układ.Na rysunku 39 przedstawiony jest uproszczony diagram przerwania cyklicznego. Jest to jednaz najbardziej krytycznych czasowo części programu, posiada najwyższy priorytet. W przerwaniurealizowane są zadania związane z pomiarami wartości chwilowych oraz regulacją siły wyjścioweji prądu silnika. Przerwanie wywoływane jest co 100µs. Tyle wynosi okres regulacji prądu. Okresregulacji siły jest dziesięć razy większy i wynosi 1ms.Równie ważną częścią programu jest przerwanie IC wywoływane przy każdej zmianie stanuczujników Halla. Przerwanie to posiada również najwyższy priorytet. W rzeczywistości używanesą trzy identyczne przerwania związane z trzema czujnikami Halla zainstalowanymi w silniku.34

Rysunek 38: Diagram głównej pętli programuRysunek 39: Diagram przerwania cyklicznegoRysunek 40: Diagram przerwania Input Capture35

Częstotliwość ich wywoływania zależy od prędkości obrotowej silnika. W przerwaniu realizowanajest wyłącznie funkcja komutacji.W systemie istnieją jeszcze inne przerwania, takie jak:• przerwanie przetwornika A/C,• przerwanie odbiornika SCI,• przerwanie nadajnika SCI,które nie będą tu opisane ponieważ ich kod został wygenerowany automatycznie przez użyteBeany dostępne w środowisku CodeWarrior.4.3 Struktura układu regulacjiRysunek 41 przedstawia schemat blokowy układu regulacji. Widać na nim dwie pętle regulacji.Pierwsza z nich, wewnętrzna, to pętla regulacji prądu silnika z regulatorem PID. Wartościązadaną dla obiektu jest wartość wyjściowa z regulatora siły. Możliwe jest bezpośrednie zadawaniewymuszenia na regulator prądu, w celach identyfikacji obiektu oraz doboru nastawregulatora. Dla uproszczenia rysunku możliwość ta nie została na mim uwzględniona. Pomiarwartość chwilowej prądu, pochodzi z kanału nr 1 przetwornika A/C. Uchyb regulacji podawanyjest na regulator PID. Wyjście regulatora połączone jest z modułem PWM, który modulujewypełnienie sygnału sterującego silnik.Rysunek 41: Schemat blokowy układu regulacjiW celu spełnienia założeń teoretycznych, nastawy regulatora prądu powinny być dobranetak, by prąd silnika był w stanie nadążać za zmianami sygnału wejściowego. Tylko wtedydynamika pętli regulacji prądu będzie pomijalna. Dla poprawy jakości sterowania, częstotliwośćpętli regulacji prądu jest większa niż częstotliwość pętli regulacji siły.Pętlę regulacji siły zamyka pomiar jej wartości chwilowej, pochodzący z kanału nr 0 przetwornikaA/C. Wartość zadaną siły ustala użytkownik w postaci dyskretnej tablicy wymuszeń36

o rozmiarze maksymalnie 250 punktów. Symbolicznie na rysunku przedstawione jest to w postaciinterfejsu RS-232 i układu komunikacji szeregowej SCI. Uchyb regulacji podawany jest naregulator PD, zgodnie z modelem teoretycznym. Dobór nastaw regulatora dyskutowany będziew następnym rozdziale.Na rysunku 41 znajdują się również bloki, które nie wchodzą bezpośrednio w żadną z pętliregulacji ale są równie ważne w punktu widzenia kontroli siłownika. Pierwszy, to blok komutacji,sterowany przez czujniki Halla, ustalający stan linii DIR oraz decydujący który z sygnałówP W M jest w danej chwili aktywny. Drugi, to blok dekodera kwadraturowego dostarczającegoinformacji o pozycji wyjścia siłownika. Sygnały wejściowe tego bloku pochodzą z enkoderaoptycznego. Informacja o pozycji wyjścia siłownika potrzebna jest przede wszystkim w badaniachnad modelem siłownika oraz, z pewnością, w większości układów, w których siłownikmoże być zastosowany.4.4 Sterowanie silnikiem BLDC - komutacjaSilnik bezkomutatorowy prądu stałego (BLDC), tak jak wcześniej wspomniano, wymaga dopracy komutatora elektronicznego. Stator silnika BLDC składa się z trzech uzwojeń połączonychw gwiazdę lub trójkąt. Rotor silnika zawiera magnesy stałe rozmieszczone na jego powierzchni.Zadaniem komutatora jest zasilanie odpowiednich uzwojeń silnika w zależności od położeniajego wału. Pozycję wału określa zestaw trzech czujników Halla, potrafiących rozróżnić 6 pozycji.Rysunek 42: Algorytm komutacji silnika BLDCZgodnie z dokumentacją firmy MAXON [13], algorytm komutacji w formie przebiegów czasowychprzedstawiony jest na rysunku 42. Można go zapisać w postaci tablicy komutacji –tablica 5.Do zasilania poszczególnych uzwojeń silnika użyto trzech półmostków mocy sterowanychsygnałem DIR i EN. Dla przypomnienia, stan wysoki na linii DIR, powoduje podłączeniewybranego uzwojenia do plusa zasilania. Stan niski powoduje podłączenie uzwojenia do masy37

Hall 1 Hall 2 Hall 3 W1 W2 W31 0 1 - + NC1 0 0 - NC +1 1 0 NC - +0 1 0 + - NC0 1 1 + NC -0 0 1 NC + -Tabela 5: Tablica komutacji silnika BLDCzasilania. Logiczna jedynka na linii EN powoduje ustawienie wyjścia półmostka w stan wysokiejimpedancji.Rysunek 43 przedstawia opis uzwojeń oraz napięć zasilania silnika. Dla przypomnienia, kierunekstrzałki oznacza napięcie o wyższym potencjale, np napięcie U 1−2 będzie dodatnie wtedy,gdy uzwojenie W2 będzie podłączone do plusa zasilania zaś uzwojenie W1 do masy.Rysunek 43: Opis uzwojeń oraz napięć zasilania silnikaZastosowany mikrokontroler 56F8323 posiada kilka funkcji ułatwiających i usprawniającychsterowanie wszelkiego typu silnikami, w tym BLDC. W siłowniku zostały wykorzystane dwie znich: maskowanie dowolnego sygnału PWM oraz zadanie wartości wypełnienia dla wszystkichkanałów przez zapis wyłącznie jednego rejestru kanału 0. Pierwsza z funkcji pozwala łatwo włączaći wyłączać sygnały sterujące poszczególnymi półmostkami. W innych mikrokontrolerach,chcąc wyłączyć kanał PWM należałoby wpisać do jego rejestru wypełnienia wartość zero. Takierozwiązanie byłoby niekorzystne z punktu widzenia układ regulacji ponieważ wartości wypełnieniaustala regulator prądu. Druga z funkcji pozwala szybko zmieniać wartość wypełnieniasygnału generowanego przez każdy kanał modułu PWM. Zapis wyłącznie jednego rejestru odpowiedzialnegoza wypełnienie w kanale 0 powoduje ustawienie rejestrów pozostałych kanałówna tę samą wartość. Opisane funkcje mikrokontrolera są łatwo dostępne dzięki zastosowaniubeanu o nazwie PWMMC. Metoda PWMC1 Mask obsługuję funkcję maskowania dowolnegokanału. Funkcja automatycznego zapisu rejestrów wypełnienia wybiera się z menu beanu.38

ENMASKbit 15-11 10 9 8 7 - 6 5 4 3 2 1 0wartość 0 DIR3 DIR2 DIR1 0 PWM5 PWM4 PWM3 PWM2 PWM1 PWM0Tabela 6: Zapis sygnałów sterujących w postaci liczby 16-bitowejKorzystając z tabeli 5 można wyznaczyć wartości sygnałów sterujących poszczególnymi półmostkami,dla każdego z sześciu położeń wału silnika. Sygnały sterujące można zgrupować izapisać w postaci liczby 16-bitowej w sposób przedstawiony w tabeli 6. Dolne sześć bitów odpowiadaza maskowanie wybranych kanałów PWM. Logiczna jedynka oznacza, że kanał ma byćzamaskowany, czyli wyłączony. W sterowniku wykorzystywane są tylko kanały 0, 2 i 4. Bity8–10 odpowiadają za pożądane stany na liniach EN. Podobnie można zgrupować sygnały zczujników Halla i zapisać je w postaci liczby 8-bitowej, w sposób przedstawiony w tabeli 7.bit 7 6 5 4 3 2 1 0wartość 0 0 0 0 0 Hall1 Hall2 Hall3Tabela 7: Zapis stanu czujników Halla w postaci liczby 8-bitowejMając tak pogrupowane sygnały można stworzyć tablicę komutacji, której indeksy pól odpowiadająstanom czujników Halla, a wartości pól odpowiadają pożądanym stanom sygnałówsterujących (tabela 8). Indeks 0 i 8 odpowiadają niedozwolonym stanom czujników Halla.ENMASKwartość\bit 15-11 10 9 8 7 - 6 5 4 3 2 1 00x073F 0 1 1 1 0 1 1 1 1 1 10x032B 0 0 1 1 0 1 0 1 0 1 10x053A 0 1 0 1 0 1 1 1 0 1 00x032E 0 0 1 1 0 1 0 1 1 1 00x062E 0 1 1 0 0 1 0 1 1 1 00x063A 0 1 1 0 0 1 1 1 0 1 00x052B 0 1 0 1 0 1 0 1 0 1 10x073F 0 1 1 1 0 1 1 1 1 1 1Tabela 8: Tablica komutacji silnika BLDC zastosowana w sterownikuSygnały sterujące dla tych pól dobrane są tak, by wszystkie półmostki mocy były wtedy wyłączone.Dotychczasowy opis dotyczył sytuacji, w której silnik obraca się zgodnie z ruchem wskazówekzegara. Chcąc obracać silnikiem w przeciwnym kierunku należy, przed wyborem sygnałówsterujących, zanegować sygnał z czujników Halla. Więcej na ten temat można znaleźć w nocieaplikacyjnej [11].Funkcja komutacji bldcComm na podstawie aktualnego stanu czujników Halla, pożądanegokierunku obrotów oraz tablicy komutacji 8, zadaje sterowania na stopień mocy i silnik. Jejprototyp znajduje się w pliku MyFun.h.39

4.5 Implementacja regulatora PIDW siłowniku do regulacji wartości siły wyjściowej oraz prądu silnika użyto programowego,dyskretnego regulatora PID. Algorytm ten wymaga dość dużej mocy obliczeniowej w szczególnościszybkich operacji arytmetycznych takich jak sumowanie i mnożenie. Dla zapewnienia dużejczęstotliwości regulacji zastosowanie szybkiego mikrokontrolera sygnałowego było koniecznością.Postać czasową idealnego regulatora proporcjonalno-całkująco-różniczkującego (PID) możnazapisać [2]:gdzie:u(t) = k p (e(t) + 1 T c• u(t) — sygnał wyjściowy regulatora,• e(t) — sygnał błędu,• k p — współczynnik wzmocnienia,• T c — czas całkowania,• T d — czas różniczkowania.∫t0e(τ)dτ + T dde(t)dtPrzez analogie można napisać prawo regulacji regulatora dyskretnego, zastępując całkowaniesumowaniem, a różniczkowanie - różnicowaniem [2]. Dla algorytmu pozycyjnego i całkowaniametodą prostokątów sposobem różnic wstecznych [2] dyskretny algorytm PID przyjmuje postać:gdzie:u(i) = k p(e(i) + T sT ci∑n=0• u(i) — sygnał wyjściowy regulatora i–tym kroku,• e(i) — sygnał błędu w i–tym kroku,• e(i − 1) — sygnał błędu w i-1 kroku,• k p — współczynnik wzmocnienia,• T c — czas całkowania,• T d — czas różniczkowania.• T s — okres próbkowania.(23)e(n) + T dT s(e(i) − e(i − 1)) ) (24)TPo wprowadzeniu podstawienia: K p = k p , K i = k sTp T i, K d = k d p T dokreślającego indywidualnewzmocnienie każdej z części regulatora, wzór (24) przyjmie postać:gdzie:i∑u(i) = K p e(i) + K i e(n) + K d (e(i) − e(i − 1)) (25)n=040

• K p — wzmocnienie części proporcjonalnej,• K i — wzmocnienie części całkującej,• K d — wzmocnienie części różniczkującej.Regulator PID zastosowany w siłowniku działa zgodnie z algorytmem opisanym równaniem(25). Chcąc uzyskać rzeczywiste wartości wzmocnienia wykorzystując liczby stałoprzecinkowe,każdy z parametrów K p , K i , K d zdefiniowano przez dwie wielkości: k i Skala k , w następującysposób:K =k(26)gdzie:• k ∈ [16384, 32768],• Skala k ∈ [1, 30]2 Skala kParametry regulatora zawarte są w strukturze MJ sPIDparams, której definicja wygląda następująco:typedef struct{Word16 ProportionalGain;Word16 ProportionalGainScale;Word16 IntegralGain;Word16 IntegralGainScale;Word16 DerivativeGain;Word16 DerivativeGainScale;Word16 PositivePIDLimit;Word16 NegativePIDLimit;Word32 PositiveIntegralLimit;Word32 NegativeIntegralLimit;Word32 IntegralPortionK_1;Word16 InputErrorK_1;} MJ_sPIDparams;Prototyp funkcji realizującej algorytm PID wygląda następująco:/*k_p*//*Skala_kp*//*k_i*//*Skala_ki*//*k_d*//*Skala_kd*//*górne ograniczenie wyjścia regulatora*//*dolne ograniczenie wyjścia regulatora*//*górne ograniczenie części całkującej*//*dolne ograniczenie części całkującej*//*wartość sumy w k-1 kroku*//*wartość błądu w k-1 kroku*/Word16 MC1_controllerPIDtype1(Word16, Word16, MJ_sPIDparams*);Funkcja MC1 controllerPIDtype1 zwraca wartość wyjściową regulatora u(i) wyliczoną na podstawiewartości zadanej (parametr 1), wartości mierzonej (parametr 2) oraz parametrów regulatora(parametr 3). Wartość wyjściowa regulatora u(i) ∈ [ PositivePIDLimit, NegativePIDLimit ] .Wartość ograniczenia części całkującej wylicza się w następujący sposób:P ositiveP IDLimit ∗ 2IntegralGainScale+1P ositiveIntegralLimit = (27)IntegralGainNegativeP IDLimit ∗ 2IntegralGainScale+1NegativeIntegralLimit = (28)IntegralGainChcąc przywrócić warunki początkowe, przy każdorazowym uruchomieniu regulatora należywyzerować elementy IntegralPortionK 1 oraz InputErrorK 1.41

4.6 Komunikacja z użytkownikiemKomunikacja z użytkownikiem odbywa się poprzez terminal znakowy podłączony do portuszeregowego pracującego z prędkością 19200Bd. W sterowniku transmisję szeregową, napoziomie sprzętowym, obsługuje moduł SCI1 mikrokontrolera 56F8323. Konfiguracja modułuSCI1 została przeprowadzona z wykorzystaniem Procesor Experta oraz dedykowanego Beanuo nazwie AsynchroSerial. Bean, na poziomie programowym, dostarcza mechanizmów do obsługibuforów cyklicznych FIFO (ang. First In First Out), zarówno dla nadajnika i odbiornika.Dostarcza również metod umożliwiających zapisywanie, odczytywanie oraz sprawdzenie stanubuforów. Rozmiary buforów wynoszą 260 znaków. W sterowniku wykorzystano następującemetody:• AS1 RecvChar — odczytuje znak z bufora odbiorczego,• AS1 SendChar — zapisuje znak do bufora nadawczego,• AS1 GetCharsInRxBuf — sprawdza stan bufora odbiorczego,• AS1 GetCharsInTxBuf — sprawdza stan bufora nadawczego,Funkcje te realizują niskopoziomowy dostęp do portu szeregowego sterownika.Warstwę wyższą, wykorzystywaną w procesie komunikacji, stanowi prosta powłoka. Wyglądpowłoki wzorowany był na powłokach dostępnych w systemach operacyjnych Linux. Powłokarealizuje następujące zadania:• wyświetlenie powitalnego logo,• wyświetlanie znaku zachęty,• obsługa linii komend,• interpretacja wprowadzanych znaków (rozpoznawanie rozkazów),• dostarczenie informacji o rozpoznanym rozkazie (kod rozkazu),• wyświetlanie komunikatów informacyjnych.Powłoka jest w rzeczywistości prostym interpreterem komend. Obsługą rozkazów zajmuje sięprogram główny. W tym czasie powłoka jest wyłączona. Funkcje obsługi powłoki zawarte są wmodule o nazwie shell. Dla użytkownika dostępne są dwie funkcje:• shell — funkcja obsługi powłoki,• shell init — funkcja inicjalizacji powłoki.Powłoka do poprawnej pracy wymaga dwóch zmiennych. Pierwsza, typu Ord str, to tablica zawierającadefinicje rozkazów rozpoznawanych przez powłokę. Kod rozkazu to pozycja w tablicy.Pozycja 0 tablicy zarezerwowana jest na rozkaz wyświetlający powitalne logo. Tablica powinnakończyć się strukturą z polami o wartośći NULL. Druga, typu Shell str, zawiera informacje ostanie powłoki.Definicje struktur znajdują się w pliku nagłówkowym shell.h.42

5 Interfejs użytkownika - dostępne funkcjeJak wspomniano w rozdziale 4.6 do komunikacji z użytkownikiem potrzebny jest prosty terminalznakowy pracujący na komputerze nadrzędnym. Konfiguracja portu szeregowego komputerapowinna być następująca: 19200, 8, N, 1.Po włączeniu zasilania sterownika na ekranie terminala powinno pojawić się powitalne logowraz ze znakiem zachęty:SILOWNIK Z ELASTYCZNOSCIA STEROWANY SILOWOautor:Mariusz JaniakWroclaw 2005 PWr AiR ARRLAB 010[MJForce]%Rozkazy wprowadza się wykorzystując klawiaturę komputera nadrzędnego. Przyjęcie rozkazunastępuje po naciśnięciu klawisza ENTER. Użytkownik do dyspozycji ma następujące rozkazy:• logo — wyświetla powitalne logo,• demo — włącza/wyłącza program demonstracyjny,• start — uruchamia sterowanie,• stop — zatrzymuje sterowanie,• send — wysyła pomiary,• load — wczytuje tablicę wymuszenia,• set n x — ustawia krok sterowania na x*1ms,• get n — wyświetla aktualnie ustawiony krok sterowania,• set m x — ustawia krok pomiarowy na x*1ms,• get m — wyświetla aktualnie ustawiony krok pomiarowy,• load rc — wczytuje parametry regulatora prądu,• load rf — wczytuje parametry regulatora siły,• f i — regulacja siły/prądu,Po wpisaniu rozkazu start sterownik rozpoczyna działanie, włącza zasilanie stopnia mocy,uruchamia silnik oraz pętle regulacji. Zachowanie siłownika będzie zależało od ustawionychparametrów oraz wprowadzonych wymuszeń. Domyślnie ustawiona jest regulacja siły zwymuszeniem wprowadzonym za pomocą rozkazu load. Po czasie wynikającym z rozmiaruwymuszenia oraz ustawionego kroku sterowania siłownik wyświetli informacje o zakończeniuzadawania sterowania. Po tym fakcie wartością zadaną będzie ostatnia pozycja z tablicy wymuszeń.Podobnie po czasie wynikającym z rozmiaru tablicy pomiarów oraz ustawionego krokupomiarowego, siłownik wyświetli informacje o zakończeniu pomiarów. Podczas pracy nie możnazmieniać żadnych parametrów siłownika.43

Rozkaz stop zatrzymuje sterowanie, pomiary oraz wyłącza zasilanie stopnia mocy. Rozkazten można wpisać w dowolnym momencie, nawet wtedy gdy pomiary oraz sterowania nie zostałyukończone.Można wyłączyć pętlę regulacji siły i pozostawić wyłącznie pętlę regulacji prądu, wpisującrozkaz f i. Wtedy wprowadzone wymuszenie będzie podawane bezpośrednio na regulator prądu.Funkcja ta jest bardzo pomocna przy doborze nastaw dla regulatora prądu. Pętlę regulacji siłymożna włączyć wpisując ponownie rozkaz f i.Rozkaz demo zmienia domyślne zachowanie siłownika. Siłownik sam wylicza wartość zadanąsiły tak by symulować działanie sprężyny o zadanej sprężystości. Wartością sterującą dlasiłownika jest współczynnik sprężystości sprężyny. Podobnie jak poprzednio wczytuje się gorozkazem load. W trybie demo po wydaniu komendy start siłownik przechodzi kalibracje położeniatak by zero wypadało w połowie dopuszczalnego zakresu ruchu. W tym celu dojeżdżaefektorem do tylnej ścianki siłownika dla określenia położenia brzegowego, następnie przesuwasię w położenie środkowe. Od tej pory położenie zerowe znajduje się w środku dopuszczalnegozakresu ruchu. Aby powrócić do domyślnego działania siłownika należy powtórnie wpisaćrozkaz demo.Rozkaz load wczytuje kształt wymuszenia, które może składać się maksymalnie z 250 punktów.Plik z wymuszeniem musi mieć ustalony format. Po pierwsze, musi być to plik tekstowy.Po drugie, każde z wymuszeń musi być liczbą dziesiętną z zakresu: [-2047,2047] w normalnymtrybie pracy, (200,2000) w trybie demo. Po trzecie, każde z wymuszeń musi znajdować się wnowej linii. Po czwarte, plik musi być zakończony znakiem ’!’. Przykładowy plik z wymuszeniemmoże wyglądać następująco:01000-1000!Parametry regulatorów wczytuje się za pomocą rozkazów load rf dla regulatora siły orazload rc dla regulator prądu. Plik z parametrami, tak jak poprzednio, powinien być plikiemtekstowym zakończonym znakiem ’!’ z każdym parametrem zaczynającym się od nowej linii.Format pliku jest następujący:k_pSkala_kpk_iSkala_kik_dSkala_kd!Znaczenia oraz zakresy odpowiednich pól zostały opisane w rozdziale 4.5.Zebrane pomiary można przesłać do komputera nadrzędnego rozkazem send. Aby zapisaćje na dysku należy postępować według następującej procedury:1. Wprowadź rozkaz send (bez ENTER).2. Włącz funkcje przechwytywania pliku tekstowego w terminalu.44

3. Naciśnij ENTER.4. Czekaj na napis: #Nacisnij dowolny klawisz !!!.5. Wyłącz przechwytywanie w terminalu.6. Naciśnij ponownie ENTER.W pliku zawarte są podstawowe informacje o parametrach siłownika takie jak: krok sterowaniaN, krok pomiarowy M oraz parametry regulatorów prądu i siły. Pomiary ustawione są w kolumnach,które oznaczają odpowiednio: chwilą czasową t, wartość mierzoną siły F l , wartość zadanąsiły F d , wartość mierzoną prądu I m , wartość zadaną prądu I d oraz położenie X m . W wierszachpodane są wartości poszczególnych wielkości pomiarowych w danych chwilach czasu. Formatpliku dostosowany został do formatu obsługiwanego przez program GnuPlot. Przykładowy plikz pomiarami wygląda następująco:# SILOWNIK Z ELASTYCZNOSCIA STEROWANY SILOWO# Wyniki pomiarow dla:# N = 1# M = 1# Kp S_Kp Ki S_Ki Kd S_Kd# 32715 15 16545 17 20972 18# Kp S_Kp Ki S_Ki Kd S_Kd# 26215 17 16385 30 20972 21# t Fl Fd Im Id Xm0 -87 0 0 0 01 -82 0 0 0 02 -74 0 0 0 03 -63 0 0 0 04 -49 0 0 0 0#Nacisnij dowolny klawisz !!!Rozkazy set n x, set m x służą do ustawiania kroku, odpowiednio: sterowania i pomiarowego.Parametr x musi być liczbą całkowitą z zakresu [1,65534]. Krok sterowania/pomiarowybędzie równy x*1ms.Rozkazy get n, get m służą do wyświetlania ustalonej wartości kroku, odpowiednio: sterowaniai pomiarowego.Rozkaz logo wyświetla ponownie powitalne logo.6 Przebieg i wyniki eksperymentuW celu przeprowadzenia eksperymentów, zbudowano prowizoryczne stanowisko badawczedające możliwość obciążenia siłownika znacznymi masami. Widok stanowiska badawczegoprzedstawiają rysunki 44 i 45. Ponadto do badań użyto precyzyjnej suwmiarki oraz 5kg odważników.Pomiarów sił, prądów i przesunięć dokonywał sterownik siłownika.45

Rysunek 44: Widok stanowiska badawczego (1) Rysunek 45: Widok stanowiska badawczego (2)6.1 Identyfikacja obiektuIdentyfikację obiektu rozpoczęto od znalezienia stałych skalujących poszczególne sygnałypomiarowe oraz sygnały wartości zadanych. Zakresy poszczególnych sygnałów są następujące:F l ∈ [−2047, 2047], F d ∈ [−2047, 2047], I m ∈ [−4095, 4095], I d ∈ [−2047, 2047],X m ∈ [−2147483647, 2147483648].W pierwszej kolejności zbadano czujnik siły. Interesowała nas zależność pomiędzy wartościąpomiarową F l a przesunięciem. Eksperyment polegał na przesuwaniu czujnika siły o stałąwartość 5mm i każdorazowym zbieraniu pomiarów z czujnika. Charakterystyka przedstawionana rysunku 46 obrazuje otrzymane wyniki. Oczywiście charakterystyka jest liniowa, ponieważzastosowano liniowy potencjometr. Współczynnik skalujący (nachylenie prostej) wynosi okołoWS X = 110 jedn. . Oznacza to, że przesunięcie czujnika o 1mm spowoduje zmianę odczytu o 110mmjednostek.Następnie zbadano jak odczyt z czujnika zależy od siły działającej na efektor siłownika.Pierwszą czynnością było zablokowano karetki siłownika. Następnie do efektora doczepiano5kg odważniki (w polu grawitacyjnym ziemi odpowiadało to sile około 50N). Ponieważ niedysponowaliśmy większą liczbą odważników pomiar został dokonany tylko w czterech punktach.Rysunek 47 przedstawia otrzymane wyniki. Charakterystyka jest liniowa ponieważ w siłownikuzastosowano liniowe sprężyny. Wyznaczono współczynnik skalujący WS F = 4.4 jedn.N .Łącząc charakterystyki 46 i 47 otrzymamy zależność siły działającej na sprężynę F od przesunięciasprężyny X – rysunek 48. Z prawa Hooka (7), współczynnik nachylenia prostej tołączna stała sprężystości sprężyn. Stałą sprężystości k s można w prosty sposób wyliczyć korzystającz poprzednich dwóch współczynników skalujących. Współczynnik WS X należy wyrazić46

40009003500800Odczyt z czujnika3000250020001500Odczyt z czujnika700600500400300100020050010000 5 10 15 20 25 30 35Przesuniecie [mm]00 20 40 60 80 100 120 140 160 180 200F l[N]Rysunek 46: Zależność wartości pomiarowej F l od przesunięciaczujnikaRysunek 47: Zależność wartości pomiarowej F l od siłydziałającej na efektor600500400F [N]30020010000 0.002 0.004 0.006 0.008 0.01 0.012 0.014 0.016 0.018 0.02X [m]Rysunek 48: Zależność siły działającej na sprężynę od przesunięcia sprężynyw jedn. , stąd: mk s = WS X · 10 3[ ]110 · 103 kg= = 25000(29)WS F 4.4s 2Współczynnik skalowania dla odczytu położenia można było wyznaczyć korzystając jedyniez danych technicznych kodera przyrostowo-impulsowego oraz przekładni. Jeden pełen obrótkodera odpowiada 4096 impulsom, skok śruby przekładni wynosi 0.0025m na obrót, zatem:WS Xm = 4096[ jedn.0.0025 = 1638400 mWspółczynnik skalowania dla odczytu prądu wynika z parametrów przetwornika A/Coraz układu elektronicznego. Zakres pomiarowy 12 bitowego przetwornika A/C wynosi 3.3V.](30)47

Wzmocnienie toru pomiarowego prądu zostało dobrane tak, by napięciu 0.5V odpowiadał prąd1A. Współczynnik skalujący pomiar prądu wynosi więc WS I = 0.5·4095 ≈ 620 jedn. . Współczynnikten, z punktu widzenia sterowania siłownikiem, nie jest zbytnio interesujący. O wiele3.3 Abardziej interesująca jest zależność siły F m od prądu zadanego I d . Prąd I d w rzeczywistymsiłowniku odpowiada sile F m w modelu siłownika. W celu zbadania tej zależności należałowykonać następujące czynności:1. Zablokować efektor siłownika.2. Rozewrzeć pętlę siłowego sprzężenia zwrotnego (rozkaz f i).3. Dobrać parametry regulatora prądu.4. Zadać wymuszenie I d .5. W stanie ustalonym zebrać pomiary z czujnika siły.350300250200Fm [N]1501005000 200 400 600 800 1000 1200Wartosc zadana praduRysunek 49: Zależność siły F m od wartości zadanej prądu I dO ile rozwarcie pętli siłowego sprzężenia zwrotnego nie było kłopotliwe, o tyle dobranie nastawregulatora prądu było zadaniem trudnym i czasochłonnym. Początkowo próbowano dobieraćnastawy na podstawie odpowiedzi silnika na skok przy rozwartej pętli regulacji prądu. Układokazał się jednak zbyt szybki w stosunku do częstotliwości zbierania pomiarów. Trudno było, napodstawie otrzymanych wyników, wyznaczyć odpowiednie stałe czasowe układu. Po zgrubnychprzybliżeniach z poprzedniej metody nastawy regulatora prądu zostały dobrane eksperymentalnie.Badano odpowiedź układu na różne wymuszenia, przy różnych warunkach obciążenia.Ostatecznie poszczególne wzmocnienia dla regulatora prądu ustalono na poziomie: K p ≈ 1.0253,K i ≈ 0.0354, K d ≈ 0.1585. Pomiarów należało dokonywać w stanie ustalonym ponieważ wtedy,przy zablokowanym efektorze, siła F m = F l . Wyniki pomiarów pokazane są na rysunku 49.Można zauważyć, że dla małych wartości prądu siła F m równa jest 0. Powyżej pewnej wartościcharakterystyka jest liniowa. To przesunięcie wynika z oporów tarcia silnika oraz przekładni, na48

pokonanie których potrzebny jest pewien niezerowy prąd (moment). Współczynnik nachyleniaprostej wynosi WS F m = 0.3224 [ ]Njedn. , przesunięcie 110[jedn.].Najtrudniejszym etapem prac badawczych była identyfikacja nieznanych parametrów modelu,zastępczej masy m m oraz tarcia lepkiego b m . Identyfikację przeprowadzono dla modeluz otwartą pętlą (12). Warunki eksperymentu były identyczne jak przy wyznaczaniu zależnościF m = f(I d ), czyli rozwarta pętla siłowego sprzężenia zwrotnego oraz zablokowany efektor siłownika.Dzięki zablokowaniu efektora z równania (12) pozbyliśmy się członu odpowiedzialnegoza przesunięcie. Eksperyment polegał na pomiarze odpowiedzi układu na skok siły F m . Następniedane pomiarowe zostały wprowadzone do programu MATLAB gdzie odbywał się właściwyproces identyfikacji. W tym celu został zbudowany model (rysunek 50) w SYMULINK-u, którydawał możliwość porównywania odpowiedzi modelu z odpowiedzią siłownika. Pierwsze przy-Rysunek 50: Układ wykorzystany przy identyfikacjibliżenie nieznanych parametrów uzyskano stosując następujące działania:1. Dokonano transformacji transmitancji modelu z postaci ciągłej w postać dyskretną (ControlToolbox).2. Dla modelu w postaci dyskretnej oraz wcześniej zebranych danych pomiarowych przeprowadzonoidentyfikacje (System Identyfication Toolbox).3. Dokonano transformacji transmitancji z dziedziny dyskretnej w ciągłą (Control Toolbox).4. Z transmitancji usunięto człony które nie występują w modelu siłownika.Tak uzyskany model następnie dostrajano ręcznie. Niestety okazało się że nie da sie dopasowaćodpowiedzi rzeczywistego układu do odpowiedzi modelu drugiego rzędu. Rzeczywisty układposiadał pewne opóźnienie, którego nie można było zaniedbać. Z tego względu dodano członopóźniający pomiędzy siłą zadaną F m a wejściem modelu, co widać na rysunku 50. Dla takzmodyfikowanego układu udało się dobrać nieznane parametry: zastępczą masę m m = 15.9kg,tarcie lepkie b m = 948.1 kg oraz opóźnienie T sdel = 16ms. Rysunki 51-56 przedstawiają odpowiedzimodelu teoretycznego (12) i obiektu rzeczywistego na różne wartości siły F m . Pojawieniesię opóźnienia wynika z dynamiki układu regulacji prądu. Regulator prądu nie nadąża za zmianamiwartości zadanej. W teoretycznym modelu (rysunek 3) dynamika wzmacniacza zostałapominięta. Badania pokazały, że zaniedbanie dynamiki wzmacniacza powoduje duże różnice wodpowiedzi układu rzeczywistego i teoretycznego.49

180250160140200120150100F l[N]80F l[N]1006040502000modelobiekt−200 0.05 0.1 0.15 0.2 0.25t [ms]modelobiekt−500 0.05 0.1 0.15 0.2 0.25t [ms]Rysunek 51: Odpowiedzi modelu i siłownika na wymuszenieF m = 160NRysunek 52: Odpowiedzi modelu i siłownika na wymuszenieF m = 193N250300200250150200150F l[N]100F l[N]10050500modelobiekt−500 0.05 0.1 0.15 0.2 0.25t [ms]0modelobiekt−500 0.05 0.1 0.15 0.2 0.25t [ms]Rysunek 53: Odpowiedzi modelu i siłownika na wymuszenieF m = 220NRysunek 54: Odpowiedzi modelu i siłownika na wymuszenieF m = 248N300350250300200250200150F l[N]F l[N]15010010050500modelobiekt−500 0.05 0.1 0.15 0.2 0.25t [ms]0modelobiekt−500 0.05 0.1 0.15 0.2 0.25t [ms]Rysunek 55: Odpowiedzi modelu i siłownika na wymuszenieF m = 276NRysunek 56: Odpowiedzi modelu i siłownika na wymuszenieF m = 303N6.2 Badania obiektuBadania siłownika rozpoczęto od doboru parametrów regulatora siły (PD). Pierwsze ichprzybliżenie otrzymano stosując się do wskazówek zawartych w pracy [1]. Niestety dla wzmoc-50

nień wyliczonych tą metodą układ miał tendencje do wzbudzania się. Z tego względu nastawyzostały dobrane eksperymentalnie. Badano odpowiedź układu na różne wymuszenia, przy różnychwarunkach obciążenia. Ostatecznie poszczególne wzmocnienia dla regulatora siły ustalonona poziomie: K p ≈ 1.6357, K d ≈ 0.1585.F l[N]25020015010050wartosc zadanawartosc zmierzona00 50 100 150 200 250 300 350 400 450 500t [ms]F l[N]14012010080604020wartosc zadanawartosc zmierzona00 50 100 150 200 250 300 350 400 450 500t [ms]600500wartosc zadanawartosc zmierzona300250wartosc zadanawartosc zmierzona400200F m[N]300200F m[N]1501001005000−1000 50 100 150 200 250 300 350 400 450 500t [ms]−500 50 100 150 200 250 300 350 400 450 500t [ms]Rysunek 57: Odpowiedzi siłownika z zablokowanymefektorem na wymuszenie F d = 234N ∗ 1(t − 10)Rysunek 58: Odpowiedzi siłownika z zablokowanymefektorem na wymuszenie F d = 117N ∗ 1(t − 10)Następnie zbadano odpowiedź układu na skok wartości zadanej siły F d . Rysunki57, 58 pokazują odpowiedzi układu na wymuszenie, odpowiednio F d = 234N ∗ 1(t − 10) iF d = 117N ∗ 1(t − 10). Można zauważyć że odpowiedź układu ma charakter oscylacyjny. Za powstanieoscylacji odpowiedzialne są elastyczności siłownika. Ich wpływ na układ jest mniejszydla większych wartości sił zewnętrznych F l . Widać też że rzeczywista wartość siły jest mniejszaniż wartość zadana. Wynika to z postaci zastosowanego regulatora. W celu zminimalizowaniabłędu regulacji w stanie ustalonym należałoby wprowadzić człon całkujący. Charakterystyki57, 58 pokazują również przebieg wartość zadanej i mierzonej siły F m (prądu), gdzie wyraźniewidoczne jest opisane w punkcie 6.1 opóźnienie wzmacniacza (regulatora prądu).180160k=500k=1000k=2000140120100Fl [N]806040200−20−0.12 −0.1 −0.08 −0.06 −0.04 −0.02 0 0.02Xm [m]Rysunek 59: Odpowiedź siłownika w trybie regulacji wyjściowej impedancji mechanicznej51

Następnie zbadano działanie siłownika w roli napędu z regulowaną wyjściową impedancjąmechaniczną. W tym celu należało przejść w tryb demo (rozkaz demo). Eksperyment polegałna zadaniu pewnej sztywności k i poruszaniu efektorem siłownika. Otrzymane wyniki pokazanesą na rysunku 59.Widać, że zależności siły F l od położenia X m , w przybliżeniu, jest liniowa. Wokolicach zerowego położenia pojawia się pewna nieliniowość. Jej obecność wynika z istnienianiezerowej strefy nieczułości silnika (opis w punkcie 6.1). Dla małych sił F l , uchyb regulacjipodawany na wzmacniacz (regulator prądu) jest niewystarczający na pokonanie oporów tarciasilnika i przekładni. Sprężyny zaczynają się sprężać, wartość pomiarowa z czujnika siły wzrasta,niestety, silnik jeszcze się nie obraca.6.3 Wyniki eksperymentówWyniki otrzymane w eksperymentach przeprowadzone na siłowniku z elastycznością szeregowązostały zebrane w tabeli 9Parametr Symbol Wymiar Wartośćjedn.Wsp. skalujący czujnik siły dla przesunięcia WS X mm110jedn.Wsp. skalujący czujnik siły dla siły WS F N4.4jedn.Wsp. skalujący czujnik położenia WS Xm m1638400jedn.Wsp. skalujący czujnik prądu WS I A620NWsp. skalujący siłę F m WS F m jedn.0.3224Strefa nieczułości siły F m Zero F m jedn. 110kgZastępcza sprężystość sprężyn k s s250002Zastępcza masa silnika m m kg 15.9kgZastępcze tarcie lepkie b m s948.1Opóźnienie T del ms 16Tabela 9: Tabela z wynikami eksperymentu7 PodsumowanieW wyniku niniejszej pracy powstał siłownik z siłowym sprzężeniem zwrotnym, który z powodzeniemmoże być zastosowany do napędzania przegubów obrotowych różnego typu robotówmanipulacyjnych i mobilnych (kroczących). Powstało również stanowisko badawcze umożliwiającebadanie tego typu napędu.Pierwszy etap prac polegał na zapoznaniu się z ideą sterowania siłowego oraz ze stosowanymirozwiązaniami w tej dziedzinie. Analizując istniejące rozwiązania zdecydowano się nakonstrukcję liniowego siłownika z elastycznością szeregową, który charakteryzuje się bardzo dobrymiparametrami ważnymi z punktu widzenia sterowania siłowego. Został zaproponowanymodel matematyczny – transmitancyjny, opisujący dynamikę siłownika. Zaproponowano równieżsposób przeniesienia napędu z liniowego na obrotowy. Była to kluczowa kwestia jeżelichodzi o zastosowanie siłownika w roli napędu dla przegubów obrotowych robotów.Następny etap prac polegał na zaprojektowaniu i wykonaniu konstrukcji mechanicznej i elektronicznejrobota. Zdecydowano się na zastosowanie silnika bezkomutatorowego prądu stałego(BLDC), ponieważ przy niewielkich wymiarach i małej masie oferuje dużą moc oraz moment52

napędowy. Ze względu na to że nie dysponowaliśmy wiedzą i doświadczeniami na temat silnikówBLDC, dlatego, w pierwszej kolejności, opanowano sposoby sterowania nimi. Okazało sięże do sterowania silnikami BLDC idealnie nadaje się mikrokontroler sygnałowy MC56F8323.Ponieważ inne alternatywy były mało interesujące zdecydowano się wykorzystać ten układ wsterowniku siłownika. W związku z tym powstał moduł z mikrokontrolerem MC56F8323 [3]oraz interfejs uruchomieniowy JTAG [4], które wraz z oprogramowaniem CodeWarrior DSPstanowiły kompletne środowisko uruchomieniowe dla procesora DSP. Środowisko to pozwoliłoszybko opanować specyficzne aspekty programowania mikrokontrolera sygnałowego DSP. Częśćelektroniczna, w znacznej większości, została zbudowana na płytkach uniwersalnych ponieważten sposób montażu umożliwia łatwe nanoszenie poprawek. Część mechaniczną w całości wykonałPan Ryszard Krzywański według wcześniej przygotowanego projektu. Siłownik w znacznejwiększości został wykonany z aluminium, z niewielkim dodatkiem elementów z tworzywasztucznego i stali.Następnie stworzono oprogramowanie sterujące siłownikiem umożliwiające przeprowadzaniebadań. Stworzono przyjazny interfejs użytkownika, wzorowany na powłokach Linuxowych,pozwalający w prosty i przyjazny sposób komunikować się z operatorem. Od strony użytkownikawymagane jest posiadanie terminala z portem szeregowym. Zaimplementowane rozkazyumożliwiają przeprowadzenie kompletnych badań, od identyfikacji obiektu, poprzez dobór nastawregulatorów, do badań samego siłownika. Możliwe jest zadawanie dowolnych kształtówwymuszeń oraz zbieranie danych pomiarowych.Ostatnim etapem prac były badania siłownika. W pierwszej kolejności zidentyfikowana nieznaneparametry obiektu (współczynniki skalujące) oraz parametry modelu (współczynnik sprężystościk s , masę m m oraz tarcie lepkie b m ). Następnie dobrano nastawy poszczególnych regulatorów.Okazało się, że założenie o braku dynamiki wzmacniacza jest niepoprawne. Układposiadał znaczne opóźnienie, którego nie można było pominąć podczas identyfikacji obiektu.Ostatecznie zbadano odpowiedź układu na skok wartości zadanej siły oraz zachowanie układujako źródła o regulowanej mechanicznej impedancji wyjściowej.Badania pokazały, że siłownik można traktować jako źródło czystej siły z niewielką dynamiką.Dynamika układu ujawniała się przy małych wartościach siły zadanej, kiedy nieliniowościwynikające z obecności tarcia miały znaczący wpływ na układ. Dla większych sił siłownik zachowywałsię znacznie lepiej.W przyszłości warto zbadać charakterystyki częstotliwościowe oraz fazowe siłownika. Wartorównież określić parametry takie jak:• pasmo przenoszenia siły,• mechaniczna impedancja wyjściowa,• zakres dynamiczny,• gęstość siły, gęstość energii.Największym sukcesem byłoby zastosowanie opisanego napędu w konkretnej aplikacji robotycznejnp. w robocie kroczącym.53

Literatura[1] D.W. Robinson Design and analysis of series elasticity in closed-loop actuator force control,PHD thesis MIT, 2000 .[2] J. Brzózka Regulatory i układy automatyki, Wydawnictwo MIKOM 2004.[3] M. Janiak Moduł z mikrokontrolerem MC56F8323, Raport ICT serii SPR nr 4/2005, Wrocław2005.[4] M. Janiak ICD - interfejs uruchomieniowy JTAG dla DSP56800E, Raport ICT serii SPRnr 5/2005, Wrocław 2005.[5] R. Szlawski Implementacja i badanie algorytmów sterowania robotem dwukołowym, PracaDyplomowa Magisterska PWr, Wrocław 2004.[6] Technical Data 56F8323 16-bit Hybrid Controller, MC56F8323/D Rev 1.0, Freescale Inc.,2003.[7] DSP56800E 16-bit Digital Signal Processor Core Reference Manual, DSP56800ERM/D Rev2.0, Motorola Inc., 2002.[8] MC56F8300 Periherial User Manual, MC56F8300UM/D Rev 2003/08/19, Metrowerks,2003.[9] CodeWarrior Development Tools IDE 5.2 User’s Guide, UNIS spol. s r.o., 2003.[10] Processor Expert Help for CodeWarrior Plug-in for Motorola, UNIS spol. s r.o., 2003.[11] 3-Phase BLDC Motor Control witch Hall Sensors Using DSP56F80x, AN1916D Rev 1.0,Motorola Inc., 2002.[12] L4960 power switching regulator datasheet, SGS-Thomson, 1995[13] www.maxonmotor.com54

DodatekRysunek 60: Rysunek złożeniowy55

Rysunek 61: Tylnia ściana siłownika56

Rysunek 62: Tylnia ściana efektora siłownika57

Rysunek 63: Karetka przekładniRysunek 64: Przednia ściana efektora siłownika58

Rysunek 65: Przednia ściana siłownika59

Rysunek 66: Kołnierz mocowania silnikaRysunek 67: Mocowanie enkoderaRysunek 68: Mocowanie silnika60

Rysunek 69: Główna prowadnica siłownikaRysunek 70: Prowadnica efektoraRysunek 71: Śruba przekładniRysunek 72: Łożysko ślizgowe61

Rysunek 73: Podstawa siłownika62