AS-84.7 Paikannus- ja navigointimenetelmät Ratkaisut. ) Kun tiedetään pelkästään etäisyys tunnetusta kohteesta saadaan mahdollinen olinpaikka ajattua ympyälle, jonka keskipiste on kohteen paikka ja säde juui mitattu etäisyys. Kun havaitaan etäisyys kahteen tunnettuun kohteeseen saadaan paikka ajattua kahden muodostuneen ympyän kahteen leikkauspisteeseen. Peiaatteessa, mikäli kohteita olisi enemmän, saataisiin paikka atkaistua yksiselitteisesti. Käytännössä mittauskohinan takia useammat ympyät eivät leikkaa toisiaan samassa pisteessä. Tällöin atkaisu saadaan epälineaaisesta pienimmän neliösumman tehtävästä, joka voidaan atkaista optimointialgoitmilla, esim Levenbeg-Maquad:lla. Tämän tyyppiset algoitmit tavisevat kuitenkin hyvän alkuavauksen paikalle, jotta globaalioptimi, eli oikea paikka, löytyy. Alkuavaus voidaan saada vaikka atkaisemalla analyyttisesti paikka kahdelle kohteelle. Paikan määittämiseksi tavitaan etäisyys ja kulma tunnettuun kohteeseen. Etäisyys on tässä tapauksessa jo mitattu, joten tavitaan vielä kulma. Se saadaan kun huomataan, että kahden kohteen tapauksessa kummatkin kohteen ja havatsijan paikat muodostavat kolmion, jonka sivuina ovat mitatut etäisyydet kumpaankin kohteeseen sekä kohteiden välinen tunnettu etäisyys. Kun kolmion sivut tunnetaan myös kulmat voidaan atkaista. Mekitään kohteiden välistä etäisyyttä d :lla. = + d x x y y Kosinilauseesta saamme d :n ja d :n välisen kulman. d = d + d d d cos( α) d + d d α = cos dd () lasketaan vielä d :n suunta y y θ = tan x x tällöin paikka voidaan atkaista ()
x cos = x+ d θ α () y = y + d θ α (4) sin sijoitetaan nyt annetut avot yhtälöihin(-) ja saadaan ( ) 0.05 + + + 0 0 0.05 α = cos =.47ad 0.05 0.0 0.0 θ = tan = 0 + tämän jälkeen voidaan laskea paikka x = + 0.05cos 0.47 = 0 y = 0.0 + 0.05sin 0.47 = 0 Laskemalla samoin viheellisellä lukemalla saadaan ( ) 0.05 + + + 0 0 9.95 α = cos =.40ad 0.05 x = + 0.05cos 0.40 = 0.5 y = 0.0 + 0.05sin 0.40 = 0.06 eli x = 0.5/ 0. = 5 dop y = 0.06/ 0. = 0.6. dop Kun pelkästään kulma kahteen kohteeseen tunnetaan on paikka ajattu ympyän kaaelle. Eli kun havaitaan kulmat useampaan kohdepaiin saadaan useampia ympyäajoitteita joiden leikkauspisteistä löytyy mahdollinen atkaisu, samoin kuin edellisessä tehtävässä. Eona on se että emme suoaan saa ympyöiden sädettä ja ympyät kulkevat tällä ketaa kohteiden kautta. Yksi atkaisu on laskea ympyöiden säteet ja keskipisteet ja sen jälkeen atkaista leikkauspiste kuten edellisessä kohdassa.
Seuaavassa käytetään hyväksi kahta tulosta: Jos kohdepain muodostama kulma ympyän eunalta katsottuna on α niin saman kohdepain välinen kulma katsottuna ympyän keskipisteestä on α. θ Segmentin pituus x= sin, missä on ympyän säde ja θ on vastaavan sektoin kulma. Näitä tuloksia hyväksi käyttäen voidaan laskea ympyän säde kohdepain välinen etäisyys, ja keskipisteen paikka on x =, missä x on sin( α ) 80 α x = x + cosθ 80 α y = y + sin θ y y θ = tan x x kun sama tehdään toiselle kohdepaille olemme suunnilleen samassa tilanteessa kuin edellisen tehtävän alussa. Seuaavaksi atkaistaan leikkauspiste. d d = = = + d x x y y α d + d d = cos dd θ ja y y = tan x x x = x cos + d θ α y = y + d θ α sin kun näihin yhtälöihin sijoitetaan annetut numeoavot saadaan
x =.5 y =.5 θ = 45 sama näkyy alla olevassa kuvassa. Siinä majakat on mekitty pienillä tyhjillä ympyöillä, suuemmat ympyät vastaavat kukin yhden kulmamittauksen aiheuttamaa ajoitetta. Oikea paikka löytyy kahden ympyän leikkauspisteessä. 8 7 6 5 4 0 - - 0 4 5 6 7 8 9
AS-84.7 Localization and navigation methods Solutions fo execise. ) When only the distance to a known taget location is available, the location of the senso can be esticted to a cicle segment, the cente of which is the known location and adius the measued distance. When distances to two known locations ae obseved the location of the senso is esticted to the two intesection positions of the cicle segments. In pinciple, when obsevation to moe known locations would be available the location of the senso could be solved uniquely. In pactice, due to senso noise, the cicles do not intesect in a single position. In this case the poblem can be fomulated as a non-linea least squaes poblem, which can be solved e.g. with the Levenbeg-Maquad optimization algoithm. Howeve, these kinds of algoithms equie a good initial guess fo the location in ode to convege to the coect location (global optimum). The initial guess can be acquied, fo example, by solving analytically the senso location by means of two known senso locations. In ode to detemine the senso location, both distance and angle to a known location ae equied. In ou case we have the distance but the angle is still needed. The angle can be acquied by obseving that in case of two known taget locations both locations and the obseve fom a tiangle the sides of which equal the measued distances to both taget locations and the mutual distance between the known taget locations. Let s mak the distance between the known locations of the tagets by d. = + d x x y y Fom the cosine law we get the angle between d and d. d = d + d d d cos( α) d + d d α = cos dd () let s calculate also the diection of d θ y y tan = x x ()
then the location of the senso can be solved x cos = x+ d θ α () y = y + d θ α (4) sin let s now assign the given values to the equations (-) which yields ( ) 0.05 + + + 0 0 0.05 α = cos =.47ad 0.05 0.0 0.0 θ = tan = 0 + now we can calculate the senso location x = + 0.05cos 0.47 = 0 y = 0.0 + 0.05sin 0.47 = 0 Similaly, by assigning the faulty value we get ( ) 0.05 + + + 0 0 9.95 α = cos =.40ad 0.05 x = + 0.05cos 0.40 = 0.5 y = 0.0 + 0.05sin 0.40 = 0.06 that is x = 0.5/ 0. = 5 dop y = 0.06/ 0. = 0.6 dop
. When only the angles to known tagets ae obseved, the location of the obseve (i.e. senso) is esticted to a cicle ac segment. Consequently, when seveal pais of tagets ae obseved, seveal cicle ac segments ae acquied. The possible solution fo the location of the obseve can then be found fom thei intesection positions, similaly as in the pevious poblem. The only diffeence is that we do not diectly get the adius of the cicles and that the locations of the tagets ae on the cicle ac segments. One way to solve the poblem is to fist calculate the adiuses and the cente points of the cicles and afte that calculate thei intesection point as in the pevious section. The following two esults will be utilized: If the angle of the taget pai seen fom the cicle bode is α then the angle fomed by the same taget pai seen fom the cente of the cicle is α. θ Length of the segment is x= sin, whee is the adius of the cicle and θ is the angle of the coesponding secto. x By utilizing these esults the adius of the cicle can be computed =, whee x sin( α ) is the mutual distance within the taget pai and location of the cente point is 80 α x = x + cosθ 80 α y = y + sin θ y y θ = tan x x when this is done also fo anothe taget pai we ae appoximately at the same situation as in the beginning of the pevious poblem. So we poceed by solving fo the cicle intesection i.e. the obot pose: d d = = = + d x x y y α d + d d = cos dd
θ and y y = tan x x x cos = x + d θ α y = y + d θ α sin when the given numeical values ae assigned to these equations we get x =.5 y =.5 θ = 45 The same can be seen in the figue below. In the figue the beacons have been maked with small empty dots. The lage cicles coespond each the constaint of a single angle measuement. The coect senso location can be found at the intesection point of the two cicles. 8 7 6 5 4 0 - - 0 4 5 6 7 8 9