PL241476B1

PL241476B1 - Sposób ustalenia pozycji obiektu, w szczególności człowieka

Info

Publication number: PL241476B1
Application number: PL424408A
Authority: PL
Inventors: Michał MEINA; Michał Meina; Krzysztof Rykaczewski
Original assignee: Univ Mikolaja Kopernika W Toruniu
Priority date: 2018-01-29
Filing date: 2018-01-29
Publication date: 2022-10-10
Also published as: WO2019177471A1; PL424408A1

Abstract

Sposób ustalenia pozycji obiektu, w szczególności człowieka z użyciem inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2), z których każda zawiera trójosiowy akcelerometr i trójosiowy żyroskop, charakteryzuje się tym, że przy zastosowaniu IMU2 (2), który zawiera dodatkowo trójosiowy magnetometr oraz IMU1 (1), który zawiera dodatkowo magnes permanentny, dokonuje się jednoczesnego pomiaru przyspieszenia, prędkości kątowej oraz pola magnetycznego indukowanego przez magnes permanentny, a następnie ich przetworzeniu w module odczytu i przetwarzania danych. Urządzenie do ustalania pozycji obiektu, w szczególności człowieka, składa się z co najmniej jednej pary inercyjnych jednostek pomiarowych połączonych z jednostką odczytu i przetwarzania danych. Pierwsza inercyjna jednostka pomiarowa w parze inercyjnych jednostek pomiarowych zawiera akcelerometr i żyroskop oraz magnes permanentny. Druga inercyjna jednostka pomiarowa zawiera akcelerometr, żyroskop i magnetometr.

Description

PL 241 476 B1

Opis wynalazku

Przedmiotem wynalazku jest sposób ustalenia pozycji obiektu w szczególności człowieka za pomocą inercyjnej nawigacji zliczeniowej, wykorzystujący metodę relatywnego pozycjonowania stóp człowieka do zwiększenia dokładności ustalanego położenia.

Stan techniki

Zastosowanie inercyjnej nawigacji zliczeniowej do ustalania pozycji obiektu opisał w swoich pracach Eric Foxlin, który w opublikowanym w listopadzie 2005 r. w artykule “Pedestrian tracking with shoe-mounted inertial sensors” opublikowanym w IEEE Computer Graphics and Applications Foxlin opisał system oparty na czujnikach zamontowanych na obuwiu pieszego wskazując jednocześnie na ograniczenia wynikające z odchylenia wyznaczanej pozycji od pozycji rzeczywistej. Foxlin założył na potrzeby swojego systemu, że przez pewien czas podczas chodu stopa jest w bezruchu.

W stanie techniki znane są także inne sposoby wyznaczania pozycji obiektu, w szczególności człowieka za pomocą czujników umieszczonych na obiekcie.

Patent US6549845 opisuje układ do określania lokalizacji osoby w obrębie budynku, który wykorzystuje magnetometr, magnesy, czujniki ciśnienia i CPU do obliczania długości i kierunku każdego kroku. Dane mogą być wyświetlane użytkownikowi, najlepiej na mapie lub na podłodze i mogą być przesyłane osobom poza budynkiem.

W patencie US8751151 opisano komputerową metodę lokalizacji trackera i generowania lub aktualizowania mapy lokalizacji na podstawie informacji z sensora inercyjnego, przy czym układ komputerowy obejmuje jeden lub więcej fizycznych procesorów z aprogramowanych przez jeden lub więcej modułów, a metoda obejmuje: identyfikowanie, za pomocą modułu wykrywania, co najmniej jednej cechy czujnika opartej na informacji z sensora inerc yjnego, która obejmuje wiele pomiarów ruchu trackera w danym położeniu, przy czym co najmniej jedna cecha czujnika jest identyfikowana w oparciu o pierwszy podzestaw wiele pomiarów; korelowanie, przez moduł wykrywania cech, co najmniej jednej cechy czujnika z cechą strukturalną położenia; określanie, za pomocą modułu lokalizacji i odwzorowywania, oszacowania położenia trackera na podstawie co najmniej drugiego podzbioru wielu pomiarów, który jest taki sam lub różny od pierwszego podzbioru wielu pomiarów, przy czym położenie cechy strukturalnej jest określane na podstawie oszacowania położenia; i generowanie lub aktualizowanie, za pomocą modułu lokalizacji i mapowania, mapy lokalizacji w oparciu o funkcję strukturalną.

Z kolei patent US5899963 ujawnia urządzenie, które mierzy odległości, prędkości i wysokości poruszającego się obiektu lub osoby podczas biegu lub spaceru. Akcelerometry oraz czujniki obrotu są umieszczone w jednej podeszwie buta lub w zegarku lub na pasie użytkownika, wraz z układem elektronicznym, który wykonuje obliczenia matematyczne służące do wyznaczania odległości i wysokości. Nadajnik radiowy wysyła informacje o odległości i wysokości do zegarka lub innego urządzenia odbiorczego. Odbiornik w zegarku albo innym miejscu jest połączony z mikroprocesorem, który oblicza prędkość wyjściową w oparciu o długość kroku i czas, a odległości przebyte przez biegacza z sumy wszystkich wcześniejszych kroków. Wyjście mikroprocesora jest połączone z wyświetlaczem, który pokazuje przebyty dystans, prędkość lub wysokość skoku.

W patencie US6786877 opisano urządzenie czujnikowe, które generuje sygnał, odpowiadający co najmniej dwóm z trzech orientacyjnych aspektów odchylenia, nachylenia i obrotu ciała w rozmiarze człowieka, w stosunku do zewnętrznego układu odniesienia. Czujnik generuje pierwsze sygnały z czujników, które odpowiadają przyspieszeniom lub prędkościom obrotowym ciała wokół pewnych osi tego ciała. Czujnik może być zamontowany na ciele. Z czujnikiem sprzężony jest procesor sygnałowy generujący sygnały orientacji względem zewnętrznej ramki odniesienia, odpowiadające prędkości kątowej lub przyspieszeniu. Sygnały z pierwszego czujnika są odporne na zakłócenia ze źródeł elektromagnetycznych, akustycznych, optycznych i mechanicznych. Czujniki mogą być czujnikami prędkości. Integrator może dokonywać całkowania sygnału częstotliwości w czasie. Kompensator przesunięcia jest sprzężony z czujnikami prędkości i integratorem. Kompensacja przesunięcia może obejmować grawitacyjny czujnik przechyłu lub czujnik pola magnetycznego lub oba te elementy Weryfikator okresowo mierzy orientację ciała za pomocą środków innych niż czujniki, wrażliwe na przesunięcie. Weryfikator może wziąć pod uwagę charakterystyczne cechy ludzkiego ruchu, takie jak okresy bezruchu. Kompensator przesunięcia może być po części filtrem Kalmana, który może wykorzystywać dane statystyczne dotyczące ruchu ludzkiej głowy.

PL 241 476 B1

Z patentu CN103591959 znane jest rozwiązanie wykorzystujące pomiar w czasie rzeczywistym czujnika ciśnienia, przyspieszenia ruchu i kierunku, w celu oszacowania w czasie rzeczywistym informacji o pozycji osoby, o wysokiej dokładności pozycjonowania i dużej niezawodności.

Z kolei w publikacji CN206450230U ujawniono urządzenie do szacowania trajektorii pieszych, przy użyciu jednostki zbierającej dane i przesyłającej zebrane danych do jednostki przetwarzania danych, która wykonuje obliczanie pozycji i przetwarzanie danych, dając końcowe wyniki pozycjonowania. Ponieważ jako jednostkę przetwarzania danych według wynalazku można wykorzystać różne powszechnie znane zastosowania istniejących inteligentnych terminali, to koszt sprzętowy urządzenia według wynalazku obejmuje jedynie koszt części zbierającej dane i części przesyłającej dane, aby uzyskać precyzyjne urządzenie pozycjonujące, wykonujące zadanie przy niskich kosztach.

Z opisu patentu EP1985233 znany jest sposób obejmujący wykonanie fizycznego pomiaru wzdłuż trzech osi czujnika, np. trzech osi pomiaru magnetometru. Podczas ruchu dokonuje identyfikacji jednej z trzech osi która znacząco się nie zmieniła i wykorzystuje tę informację do poprawienia estymacji orientacji. Zastrzeżenia niezależne obejmują również: (1) sposób oszacowania ruchu ciała ruchomego wyposażonego w czujnik (2) sposób kalibrowania czujnika do wyznaczania macierzy obrotu.

Z opisu wzoru użytkowego CN205066775U znane jest urządzenie o wysokiej dokładności wykrywania dróg przemieszczeń, w którym: trójwymiarowy kąt ruchu poruszającego się ciała jest mierzony przez inercyjny czujnik lokalizacji, trójwymiarowa pozycja części ruchomej i trójwymiarowy kąt ruchu są mierzone za pomocą układu lokalizatora elektromagnetycznego, trójwymiarowa informacja o położeniu ruchomej mierzona jest w układzie identyfikacji wizualnej maszyny, wyjście, które jest wykorzystywane do przetwarzania danych w celu połączenia czujnika inercyjnego, elektromagnetycznego czujnika lokalizowania, systemu identyfikacji wizualnej maszyny, jednocześnie w celu połączenia danych z trzech podsystemów, tak aby uzyskać drogę przemieszczenia ruchomej części.

W patencie US9341683 opisano sposób określania kierunku ruchu urządzenia z narzędziem. Sposób obejmuje określenie danych pierwszego kierunku ruchu urządzenia przy użyciu czujnika bezwładnościowego. Sposób obejmuje określenie drugiego kierunku ruchu urządzenia przy użyciu magnetometru. Sposób obejmuje również obliczanie skorygowanych danych drugiego ruchu w oparciu o predefiniowane wzajemne położenie między pozycją narzędzia w stanie stacjonarnym a danymi drugiego kierunku ruchu. Sposób obejmuje ponadto określenie kierunku ruchu urządzenia w oparciu o dane pierwszego kierunku ruchu oraz skorygowane dane drugiego kierunku ruchu W zgłoszeniu CN105509736 ujawniono wieloskładnikowy sposób pozycjonowania w zamkniętym pomieszczeniu dla ratownictwa przeciwpożarowego. Sposób obejmuje następujące etapy: budowa wieloskładnikowego układu pozycjonowania wewnętrznego zawierającego trójosiowy akcelerometr, trójosiowy żyroskop, trójosiowy magnetometr, barometr, czytnik kart, identyfikator RFID (identyfikacja częstotliwości radiowych) i DSP (procesor sygnału cyfrowego); moduł odbierania sygnału pobiera sygnały trójosiowego akcelerometru, trójosiowego żyroskopu, trójosiowego magnetometru i barometru; moduł nawigacji zliczeniowej pobiera informacje o położeniu ratowników; moduł odczytu stanu ruchu określa stan ruchu ratowników; odpowiedni moduł korekcji koryguje lokalizacje ratowników; moduł korekcji oparty o istnienie ustalonego punktu w pomieszczeniu określa lokalizację ratowników w połączeniu z informacjami kartograficznymi w celu przeprowadzenia korekcji estymacji lokalizacji ratowników. Wieloskładnikowy sposób pozycjonowania w zamkniętym pomieszczeniu dla ratownictwa przeciwpożarowego zapewnia precyzyjną lokalizację wewnątrz pomieszczeń i jest łatwy w obsłudze oraz ma niską zależność od warunków zewnętrznych.

W patencie US8972182 opisano sposób śledzenia elementu umieszczonego na ręku lub na głowie, obejmujący: przymocowanie bezwładnościowej jednostki nawigacyjnej do stopy pieszego; wykonanie pomiaru związanego z położeniem jednostki nawigacyjnej; aktualizowanie pozycji i/lub orientacji przedmiotu trzymanego na ręku lub na głowie, który ma być śledzony, w oparciu przynajmniej w części o pomiar, przy czym aktualizowanie pozycji i/lub orientacji przedmiotu obejmuje określenie pozycji elementu względem jednostki nawigacyjnej. Układ śledzenia elementu umieszczonego na ręku lub na głowie zawiera: bezwładnościową jednostkę nawigacyjną przystosowaną do zamocowania do ciała pieszego w ustalonym położeniu przymocowaną do stopy pieszego i zdolną do pomiaru położenia jednostki nawigacyjnej względem nieruchomej ramki odniesienia; i czujnik umieszczony na ciele pieszego skonfigurowany do określania położenia i/lub orientacji elementu na ręku lub na głowie względem jednostki nawigacyjnej.

PL 241 476 Β1

Cel wynalazku

Celem wynalazku jest opracowanie sposobu ustalenia pozycji obiektu, w szczególności człowieka, poza zasięgiem systemów takich jak GPS za pomocą inercyjnej nawigacji zliczeniowej w połączeniu z ustalaniem wzajemnej pozycji stóp co znacząco zwiększa dokładność estymowanego położenia.

Istota wynalazku

Sposób ustalenia pozycji obiektu, w szczególności człowieka z użyciem inercyjnych jednostek pomiarowych IMU1 i IMU2, z których każda zawiera trójosiowy akcelerometr i trójosiowy żyroskop i przy zastosowaniu IMU2, który zawiera dodatkowo trójosiowy magnetometr oraz IMU1, który zawiera dodatkowo magnes permanentny, dokonuje się jednoczesnego pomiaru przyspieszenia, prędkości kątowej oraz pola magnetycznego indukowanego przez magnes permanentny, a następnie ich przetworzeniu w module odczytu i przetwarzania danych, charakteryzuje się tym, że obliczone pozycje są poprawiane poprzez analizowanie estymacji trajektorii ruchu IMU1 oraz IMU2 w oparciu o odczyty pola magnetycznego magnesu przy IMU1, indukowane na magnetometrze przy IMU2.

Korzystnie estymowane są trajektorie ruchu IMU1 oraz IMU2 na podstawie całkowania odczytów żyroskopu oraz akcelerometru. Korzystnie z wykorzystaniem metod z teorii filtrów Kalmana.

Korzystnie poprawianie estymacji trajektorii polega na odnalezieniu poprawek Tc i Cc poprzez dopasowanie teoretycznego pola magnetycznego, wynikającego z procesu kalibracji magnesu permanentnego do empirycznego wynikającego z estymacji trajektorii ruchu IMU1 oraz IMU2. Przy czym Tc = [xc, yc, Zc] jest translacją (poprawką) w układzie nawigacyjnym jednej ze stóp. Natomiast Cc jest operacją zadającą obrót w układzie nawigacyjnym.

Korzystnie dokonanie poprawek Tc oraz Cc opisuje poniższy algorytm: po każdorazowym zakończeniu fazy zerowej prędkości na nodze bez magnesu (tj. /sZV-nonmagnel) wykonaj następujące poprawki na nodze z magnesem:

PO^smagnet · ^— P^mognet +

C = C C ^magnet ’ '''magnet po każdorazowym zakończeniu fazy zerowej prędkości na nodze z magnesem (tj. isZV-magnet) wykonaj następujące poprawki na nodze bez magnesu:

P^^nonmagnet P^^nonmagnet + c = c c '^nonmagnet · c 'nonmagneH gdzie isZV-nonmagnet oznacza ten moment ruchu, gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, znajduje się w stanie bezruchu, a faza isZV-magnet oznacza ten moment ruchu gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, znajduje się w stanie bezruchu, posmagnef oznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, posnonmagnet oznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Cmagnef oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, Cnonmagnet, oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Tc oznacza wektor stanowiący poprawkę położenia, a Cc oznacza wektor stanowiący poprawkę obrotu.

Korzystnie przetworzone dane są obrazowane na wyświetlaczu jako punkty odpowiadające miejscu, w którym znajduje się obiekt w nawigacyjnym układzie współrzędnych.

Korzystnie punkty obrazowane są na planie przestrzeni, po której poruszał się obiekt.

Korzystnie poprawki Tc i Cc obliczane są poprzez minimalizację względem zmiennych Tc oraz Cc wyrażenia f(T_c, Cjnaczającego błąd dopasowania, korzystnie poprzez dopasowanie empiryczne wartości pola magnetycznego do pola teoretycznego, albo w innym korzystnym przykładzie wykonania poprzez odjęcie pola empirycznego od teoretycznego, w celu uzyskania pola geomagnetycznego, czyli pola magnetycznego Ziemi.

PL 241 476 Β1

Korzystnie przy dopasowaniu pola empirycznego do pola teoretycznego minimalizowane wyrażenie ma postać:

k f(T_c,C_c): = || magpos^Ę, C_c) - magteor ||² , _;=[ gdzie magpos^Tc, Cc) oznacza odczyty pola magnetycznego z fragmentu ścieżki od ostatniej fazy isZV na dowolnej nodze, która powstała poprzez zastosowanie przekształceń Tc oraz Cc do całego tego fragmentu ścieżki, a magteor, to wartości pola teoretycznego w tych samych momentach.

Korzystnie przy dopasowaniu różnicy pól do pola geomagnetycznego minimalizowane wyrażenie ma postać:

k f(T_c, C_c): = 2 ( II magpos.(T^ C_c) - magteor. || - geomagnetic·)².

r=l

Korzystnie magnes znajdujący się w inercyjnym module pomiarowym indukuje pole magnetyczne, którego kształt jest znany dzięki przeprowadzeniu procesu kalibracji, polegającego na odczytaniu wektora indukcji magnetycznej z IMU2 w ustalonych miejscach relatywnych do IMU1. Teoretyczne pole magnetyczne jest dopasowywane do pola empirycznego (kalibrowane) poprzez dostosowanie parametrów tak, aby wiernie odzwierciedlało ono pole magnetyczne indukowane przez magnes. Korzystnie kalibracja prowadzona jest poprzez triangulację z pomiarów, gdzie wartość natężenia pola magnetycznego w danym punkcie jest średnią barycentryczną z wartości sympleksu, wewnątrz którego leży ten punkt.

Korzystnie teoretyczne pole magnetyczne modelowane jest przy użyciu modelu dipolowego bądź korzystnie przy użyciu sum modeli dipolowych bądź korzystnie poprzez interpolację każdego punktu teoretycznego na podstawie triangulacji między znanymi odczytami. Przy czym należy znaleźć jeden/kilka parametrów, w zależności od parametryzacji pola, tak aby pole teoretyczne dopasowywało się do kierunków i wartości natężenia pola magnetycznego w punktach pomiaru.

Przedmiot wynalazku został przedstawiony w przykładach wykonania na rysunku, na którym fig. 1 przedstawia schemat urządzenia, fig. 2 przedstawia schemat blokowy sposobu ustalania pozycji, fig. 3 przedstawia przykładowy montaż sensorów IMU1 oraz IMU2 na butach, fig. 4 przedstawia wykres obrazujący poprawkę na pojedynczym korku w 3D, fig. 5 przedstawia wykres pokazujący fazy ruchu stopy i moment, w którym następuje pomiar. Fig. 6 pokazuje porównanie między klasycznym algorytmem nawigacji inercyjnej w porównaniu z opisanym w tym dokumencie sposobem. Fig. 7 przedstawia przykładowy interfejs dla służb pożarniczych, w których jeden strażak może obserwować swoją pozycję jak i pozostałych strażaków biorących udział w akcji ratunkowo-gaśniczej.

Przykład I

Za pomocą inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2) umieszczonych na obuwiu poruszającej się osoby, przy czym każda jednostka pomiarowa (1) i (2) jest umieszczona na innej stopie, mierzono w jednocześnie przyspieszenie, prędkość kątową oraz pole magnetyczne otaczające osobę. Każda z inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2) zawiera akcelerometr i żyroskop, jedna z nich dodatkowo magnetometr (5), a druga magnes permanentny (4).

W wyniku pomiarów zgromadzono dane, które składają się z dwu par szeregów czasowych odczytów akceleracji, prędkości kątowych oraz jednego szeregu czasowego odczytów pola indukcji magnetycznej. Porównanie zastosowania klasycznego algorytmu inercyjnej nawigacji zliczeniowej znanego np. z prac Foxlina w porównaniu do sposobu opisanego w poniższym przykładzie przedstawia fig. 6, na której zilustrowano przejście człowieka po linii prostej na odcinku 59 [m], z którego można wywnioskować błąd dla ustalenia położenia na płaszczyźnie na poziomie 22.75 [cm] i 13.26 [cm] odpowiednio dla klasycznego algorytmu nawigacji zliczeniowej i niniejszego przykładu wykonania wynalazku. Fig. 6 pokazuje również błąd estymacji wysokości ścieżki, która powinna być ustalona na 0 [m] z uwagi na przejście po równej powierzchni, a jest ustalona średnio z błędem 1.15 [m] dla klasycznego algorytmu nawigacji zliczeniowej oraz na poziomie 31.1 [cm] dla opisanego w poniżej sposobu.

Otrzymane dane przetworzono w module odczytu i przetwarzania danych (3). Przetworzone dane zostały wyświetlone na wyświetlaczu (6) jako ścieżka po, której osoba przemieszczała się w nawigacyjnym układzie współrzędnych, po której poruszał się obiekt dla każdej nogi osob

PL 241 476 Β1 no. Przetwarzanie danych o przyspieszeniu, prędkości kątowej i polu magnetycznym opisuje poniższy algorytm:

1. Dokonaj obliczeń zmian orientacji, prędkości i położenia na podstawie odczytów z sensorów IMU1 (1) oraz IMU2 (2).

2. Wyznacz Tc oraz Cc na podstawie sposobu opisanego niżej.

3. Po każdorazowym zakończeniu fazy zerowej prędkości na nodze bez magnesu (tj. isZV-nonmagnet) wykonaj następujące poprawki na nodze z magnesem:

P^^magnet ^— P^magnet + c -CC '-'magnet · c ^magnet ·

4. Po każdorazowym zakończeniu fazy zerowej prędkości na nodze z magnesem (tj. isZV-magnet) wykonaj następujące poprawki na nodze bez magnesu:

P^^nonmagnet P^^nonmagnet +

C - - c c '^nonmagnet ’ c nonmagne!' gdzie isZV-nonmagnet oznacza ten moment ruchu, gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, znajduje się w stanie bezruchu, a faza /sZVmagnet oznacza ten moment ruchu gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, znajduje się w stanie bezruchu, posmagnetoznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, posnonmagnet oznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Cmagnef oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, Cnonmagnet oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Tc oznacza wektor stanowiący poprawkę położenia, a Cc oznacza wektor stanowiący poprawkę obrotu.

Poprawki Tc i Cc są obliczone poprzez minimalizację względem zmiennych Tc oraz Cc wyrażenia /(T_c, C_c.) oznaczającego błąd dopasowania, korzystnie poprzez dopasowanie empiryczne wartości pola magnetycznego do pola teoretycznego, albo w innym korzystnym przykładzie wykonania poprzez odjęcie pola empirycznego od teoretycznego, w celu uzyskania pola geomagnetycznego (pola magnetycznego Ziemi). Przy dopasowaniu pola empirycznego do pola teoretycznego minimalizowane wyrażenie ma postać:

k.

f(^T _crC_c): = || magpos/T^Cj - magteor^ ||² , i=l gdzie magpos(Tc, Cc) oznacza odczyty pola magnetycznego z fragmentu ścieżki od ostatniej fazy isZV (na dowolnej nodze), która powstała poprzez zastosowanie przekształceń Tc oraz Cc do całego tego fragmentu ścieżki, a magteor, to wartości pola teoretycznego w tych samych momentach.

Przy dopasowaniu różnicy pól do pola geomagnetycznego minimalizowane wyrażenie ma postać:

k

C^: = £( || magpos^T^Cj - magteor. || - geomagnetic.)²i=l

Magnes (3) znajdujący się w inercyjnym module pomiarowym IMU1 (1) indukuje pole magnetyczne wokół siebie. Teoretyczne pole magnetyczne modelowane przez proces kalibracji urządzenia jest dopasowywane do pola empirycznego (kalibrowane) poprzez dostosowanie parametrów tak, aby wiernie odzwierciedlało ono pole magnetyczne indukowane przez magnes (4). Kalibracja prowadzona jest poprzez triangulację z pomiarów, gdzie wartość natężenia pola magnetycznego w danym punkcie jest średnią barycentryczną z wartości sympleksu, wewnątrz którego leży ten punkt. Przetworzo

PL 241 476 Β1 ne dane z sensorów inercyjnych IMU1 (1), oraz IMU2 (2) odtwarzają ścieżkę poruszania się sensorów (1) i (2) wraz z orientacją we wspólnym nawigacyjnym układzie odniesienia (jakim jest Ziemia).

Urządzenie do ustalania pozycji człowieka, składa się z co najmniej jednej pary inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2) połączonych z jednostką odczytu i przetwarzania danych (3). Pierwsza inercyjna jednostka pomiarowa IMU1 (1) zawiera akcelerometr i żyroskop oraz magnes permanentny (4). Druga inercyjna jednostka pomiarowa IMU2 (2) zawiera akcelerometr, żyroskop i magnetometr (5). Inercyjne jednostki pomiarowe IMU1 (1) i IMU2 (2) komunikują się z jednostką odczytu i przetwarzania danych za pomocą kabla, nie uwidocznionego na rysunku. Jednostka odczytu i przetwarzania danych (3) wyposażona jest w moduł radiowy do przekazywania pozycji osoby. Jednostka odczytu i przetwarzania danych (3), wyposażona jest w wyświetlacz, na którym ukazywane są punkty odpowiadające pozycji osoby.

Przykład II

Dwójka strażaków przeprowadzająca akcje ratunkowo-gaśniczą w budynku podczas pełnego zadymienia wyposażona jest w urządzenie do ustalania pozycji człowieka. Dwie pary dwu inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2) zintegrowanych z obuwiem strażackim przy czym (1) i (2) jest umieszczony jest na innym bucie u każdego strażaka. W badaniu mierzono jednocześnie przyspieszenie, prędkość kątową oraz pole magnetyczne otaczające obuwie. Każda z inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2) dla każdego urządzenia zawiera akcelerometr i żyroskop, jedna z nich dodatkowo magnetometr (5), a druga magnes permanentny (4).

Otrzymane dane przetworzono w jednostce odczytu i przetwarzania danych (3). Przetworzone dane zobrazowano na wyświetlaczu (6) jako ścieżki przemieszczania się strażaków oraz miejsce odpowiadające miejscu, w którym znajduje się obiekt w zadanym układzie współrzędnych. Informacja o położeniu przesyłana jest do zewnętrznego systemu zarządzania akcją ratunkowo-gaśniczą w celu monitorowania bezpieczeństwa strażaków oraz monitorowania postępów ich pracy przez dowodzącego akcją ratunkowo-gaśniczą. Moduł odczytu i przetwarzania danych (3) za pośrednictwem modułu radiowego (7) przesyła informacje o położeniu do drugiego urządzenia drugiego strażaka celem wyświetlania jej na wyświetlaczu (6) celem informowania strażaków o ich wzajemnym położeniu. Wyświetlacz (6) służy jednocześnie do przechowywaniu informacji o ścieżce przemieszczania celem umożliwienia wycofania się do punktu zbiorczego w przypadku utraty orientacji bądź w przypadku znacznie ograniczonej widoczności z powodu gęstego zadymienia. Wyświetlacz (6) używany jest jako podręczne urządzenie mobilne bądź zintegrowany jest z hełmem strażackim. Fig. 7 ukazuje widok dostępny na wyświetlaczy jednego ze strażaków - ukazuje on aktualną swoją pozycję i ścieżkę przejść oraz pozycje i ścieżkę przejścia drugiego strażaka. Przetwarzanie danych o przyspieszeniu, prędkości kątowej i polu magnetycznym opisuje poniższy algorytm:

1. Dokonaj obliczeń zmian orientacji, prędkości i położenia na podstawie odczytów z sensorów IMU1 (1)oraz IMU2 (2).

2. Wyznacz Tc oraz Cc na podstawie sposobu opisanego niżej.

3. Po każdorazowym zakończeniu fazy zerowej prędkości na nodze bez magnesu (tj isZV-nonmagnet) wykonaj następujące poprawki na nodze z magnesem:

P^magnet · P^^magnet + c = c c '-'magnet ‘ '-'magnet

4. Po każdorazowym zakończeniu fazy zerowej prędkości na nodze z magnesem (tj. isZVnonmagnet) wykonaj następujące poprawki na nodze bez magnesu:

P^^snonmagner · ^— PO^snonmagner +

C = r c ^nonmagnet ‘ c nanmagnet' gdzie isZV-nonmagnet oznacza ten moment ruchu, gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, znajduje się w stanie bezruchu, a faza isZV-magnet oznacza ten moment ruchu gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, znajduje się w stanie bezruchu, posmagnetoznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, posnonmagnet oznacza pozycję stopy, na której zamon-

Claims

PL 241 476 Β1 towano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Cmagnet oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, Cnonmagnet oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Tc oznacza wektor stanowiący poprawkę położenia, a Cc oznacza wektor stanowiący poprawkę obrotu.

Poprawki Tc i Cc są obliczone poprzez minimalizację względem zmiennych Tc oraz Cc wyrażenia /(T_c, C_c.) oznaczającego błąd dopasowania, korzystnie poprzez dopasowanie empiryczne wartości pola magnetycznego do pola teoretycznego, albo w innym korzystnym przykładzie wykonania poprzez odjęcie pola empirycznego od teoretycznego, w celu uzyskania pola geomagnetycznego (pola magnetycznego Ziemi). Przy dopasowaniu pola empirycznego do pola teoretycznego minimalizowane wyrażenie ma postać:

k

Zfc ej: = y || magpos.(r_c, C_t.) - magteor. ||² , /=l gdzie magpos/(Tc, Cc) oznacza odczyty pola magnetycznego z fragmentu ścieżki od ostatniej fazy isZV na dowolnej nodze, która powstała poprzez zastosowanie przekształceń Tc oraz Cc do całego tego fragmentu ścieżki, a magteor, to wartości pola teoretycznego w tych samych momentach.

Przy dopasowaniu różnicy pól do pola geomagnetycznego minimalizowane wyrażenie ma postać:

k = || magpos.^, C_t) — magteor. || — geomagnetic_f.)² .

i=l

Magnes (4) znajdujący się w inercyjnym module pomiarowym IMU1 (1) generuje pole magnetyczne wokół siebie. Teoretyczne pole magnetyczne modelowane przez proces kalibracji urządzenia jest dopasowywane do pola empirycznego (kalibrowane) poprzez dostosowanie parametrów tak, aby wiernie odzwierciedlało ono pole magnetyczne wytworzone przez magnes (4). Kalibracja polega na dopasowaniu pola dipolowego i/lub sumy pół dipolowych, przy czym należy znaleźć jeden/kilka parametrów, w zależności od parametryzacji pola, tak aby pole teoretyczne dopasowywało się do kierunków i wartości natężenia pola magnetycznego w punktach pomiaru. Przetworzone dane z sensorów inercyjnych IMU1 (1), oraz IMU2 (2) odtwarzają ścieżkę poruszania się sensorów (1) i (2) wraz z orientacją we wspólnym nawigacyjnym układzie odniesienia (jakim jest Ziemia),

Urządzenie do ustalania pozycji człowieka, składa się z co najmniej jednej pary inercyjnych jednostek pomiarowych IMU1 (1) i IMU2 (2) połączonych z jednostką odczytu i przetwarzania danych (3). Pierwsza inercyjna jednostka pomiarowa IMU1 (1) zawiera akcelerometr i żyroskop oraz magnes permanentny (4). Druga inercyjna jednostka pomiarowa IMU2 (2) zawiera akcelerometr, żyroskop i magnetometr (5). Inercyjne jednostki pomiarowe IMU1 (I) i IMU2 (2) komunikują się z jednostką odczytu i przetwarzania danych za pomocą połączenia radiowego. Jednostka odczytu i przetwarzania (3) danych wyposażona jest w moduł radiowy do przekazywania pozycji osoby, na którym ukazywane są punkty odpowiadające pozycji osoby.

Zastrzeżenia patentowe

1. Sposób ustalenia pozycji obiektu, w szczególności człowieka z użyciem inercyjnych jednostek pomiarowych IMU1 i IMU2, z których każda zawiera trójosiowy akcelerometr i trójosiowy żyroskop i przy zastosowaniu IMU2, który zawiera dodatkowo trójosiowy magnetometr oraz IMU1, który zawiera dodatkowo magnes permanentny, dokonuje się jednoczesnego pomiaru przyspieszenia, prędkości kątowej oraz pola magnetycznego indukowanego przez magnes permanentny, a następnie ich przetworzeniu w module odczytu i przetwarzania danych, znamienny tym, że obliczone pozycje są poprawiane poprzez analizowanie estymacji trajektorii ruchu IMU1 oraz IMU2 w oparciu o odczyty pola magnetycznego magnesu przy IMU1, indukowane na magnetometrze przy IMU2.

PL 241 476 Β1
2. Sposób według zastrz. 1, znamienny tym, że estymowane są trajektorie ruchu IMU1 oraz IMU2 na podstawie całkowania odczytów żyroskopu oraz akcelerometru.
3. Sposób według zastrz. 1 albo 2, znamienny tym, że poprawianie estymacji trajektorii polega na odnalezieniu poprawek Tc i Cc poprzez dopasowanie teoretycznego pola magnetycznego, wynikającego z procesu kalibracji magnesu permanentnego do empirycznego wynikającego z estymacji trajektorii ruchu IMU1 oraz IMU2.
4. Sposób według zastrz. 1 albo 2, znamienny tym, że dokonanie poprawek Tc oraz Cc opisuje poniższy algorytm: po każdorazowym zakończeniu fazy zerowej prędkości na nodze bez magnesu (tj. isZV-nonmagnet) wykonaj następujące poprawki na nodze z magnesem:

POStnagnet ’ ~ PO^magnet +

C -cc magnet · c magnet' po każdorazowym zakończeniu fazy zerowej prędkości na nodze z magnesem (tj. isZV-magnet) wykonaj następujące poprawki na nodze bez magnesu:

POSnnnmagnet ‘ P^^nonmagnet +

C = c· c '-'nanmagnet < '-'nonmagnet' gdzie isZV-nonmagnet oznacza ten moment ruchu, gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, znajduje się wstanie bezruchu, a faza isZV-nonmagnet oznacza ten moment ruchu gdy stopa, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, znajduje się w stanie bezruchu, posmagnet oznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy z-awierający akcelerometr, żyroskop i magnes permanentny, posnonmagnet oznacza pozycję stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Cmagnet oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnes permanentny, Cnonmagnet oznacza macierz obrotu stopy, na której zamontowano inercyjny moduł pomiarowy zawierający akcelerometr, żyroskop i magnetometr, Tc oznacza wektor stanowiący poprawkę położenia, a Cc oznacza wektor stanowiący poprawkę obrotu.
5. Sposób według zastrz. 1, znamienny tym, że przetworzone dane są obrazowane na wyświetlaczu jako punkty odpowiadające miejscu, w którym znajduje się obiekt w nawigacyjnym układzie współrzędnych.
6. Sposób według zastrz. 1, znamienny tym, że punkty obrazowane są na planie przestrzeni, po której poruszał się obiekt.
7. Sposób według zastrz. 3 albo 4, znamienny tym, że poprawki Tc i Cc obliczane są poprzez minimalizację względem zmiennych Tc oraz Cc wyrażenia f(T_c, oznaczającego błąd dopasowania, korzystnie poprzez dopasowanie empiryczne wartości pola magnetycznego do pola teoretycznego, albo w innym korzystnym przykładzie wykonania poprzez odjęcie pola empirycznego od teoretycznego, w celu uzyskania pola geomagnetycznego będącego polem magnetycznym Ziemi.
8. Sposób według zastrz. 7, znamienny tym, że przy dopasowaniu pola empirycznego do pola teoretycznego minimalizowane wyrażenie ma postać:

k f(T_c, Cj: = 2 II magpos^, C_c) - magteor ||² , i-l gdzie magpos/(Tc,Cc) oznacza odczyty pola magnetycznego z fragmentu ścieżki od ostatniej fazy isZV na dowolnej nodze, która powstała poprzez zastosowanie przekształceń Tc oraz Cc, do całego tego fragmentu ścieżki, a magteor, to wartości pola teoretycznego w tych samych momentach.