close

Вход

Забыли?

вход по аккаунту

?

Метод определения относительной ориентации.

код для вставкиСкачать
pb0510.fm Page 57 Monday, September 27, 2010 9:15 AM
” правление подвижными объектами и навигаци¤
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
”ƒ 621.396.988.6
ћ≈“ќƒ ќѕ–≈ƒ≈Ћ≈Ќ»я
1
ќ“Ќќ—»“≈Ћ№Ќќ… ќ–»≈Ќ“ј÷»»
Ћ.Ѕ. –апопорт
?????? м???д ????д?л???? ???????ц?? ????д?г? ??л? ????????ль?? л???ль??г? г????????. ???д??л?????, ч?? ??л? ????д???? ??л??? ??????????? ??мл? ?л? ? ???л???м??м
???????????? ?? ??????, ??????????? м??ьш?? ?????? ????? ??????????? ??????? ????г?ц?????? ?????м.
???????? ?????: ??????????? ????г?ц??, ???????ц?? ? ????????????, ????????? ????г?ц??, д?ль??м????? ??м??????.
¬¬≈ƒ≈Ќ»≈
???д??л?г?????, ч?? ?? ????д?м ??л? ??????л??? ?????ль?? ????м??? ????г?ц?????? ??????.
??????? ??????д?л??? ?? ??????????? ??л? ????м ??????м, ч?? ? лю??? м?м??? ???м???, ??
??????? м??? ??? ??????? Ђ??д??ї ?? м???? ч?????? ????????? ?????м? GPS ?л? GLONASS,
?л? GALILEO. ??????д?м? ?? м???? ???? ??????
дл? ????д?л???? ???? ?гл???? ????м????? ???????ц?? ? ???ц???? ??м?????? м?????????? ????????. ?л? ????д?л???? м?????????? ???????? ???м????? ?? ?д??г?м ???м????? ш??л м??д? д??м?
????м????м? ??????д?м?, ?? ??????? м???, ч????? д?ль??м????? ??м??????, ??????ч????м?? ?д?????м????м ?л??????м ?? ч????ьм? ????????м?.
????м??м ?????? ???????? ???????, ???????? ? ??????????? ????г?ц??. ??л?? д???ль???
???????? м???? ?????, ?????м??, ? ???г? [1].
? г??ц?????ч????? ?????м? ????д???? ????д?л????? ??л?????? ????????? ? лю??? м?м??? ???м???. ?лг????м? ??ч??л???? ??л?????? ????????? ??????? ? ???????????? ??????ль??? д???м????? ???????????ю??? ??????????? ?????м
[2?4]. ??л?????? ?????? ????г?ц?????? ????м????? ??ч??л????? ?? ?????? д?ль??м????? ??м??????. ??????д?м?, ?? ??????? м???, ч?????
???????? дл? ????д?л???? ???? ??м?????? ??л?1
?????? ????л???? ??? ?????????? ??дд????? ????
(г???? 08-0900064) ? ??м?л?????? ???г??мм? 15 ??????
???.
ѕ – ќ Ѕ Ћ ≈ ћ џ ” ѕ – ј ¬ Ћ ≈ Ќ » я є 5 ? 2010
????? ??????г? ц????? ??????? ? ?д??г? ???м????? ш??л ?????м? GPS ? ч???? ????м????.
? ?????? ????д?л???? ???????ц?? ????д?г? ??л? ? ???????????? л???? м???д д???????ц??ль??г?
????????ль??г? ????ц??????????? [1], ?????л?ю??? ????д?л??ь м?????????? ??????? ? ?????м??????? ??ч????ью ? ??м??ью ??????? ??м??????
???л? ?????ш???? ??????? ???д?????ч??????.
????л?м? ?????ш???? ??????? ???д?????ч??????
????????? ??ш????? л?????????. ???????? м??г?
??ч??л???ль??? м???д?? ?? ??ш????. ??льш?????? ?? ??? ???д???? ? ??д?ч? ???д????ч??г? ц?л?ч??л????г? ???г??мм????????. ??? ??????ч????? ???л???ц?? м???д?, ???дл??????г? ? д?????
??????, дл? ?????ш???? ??????? ???д?????ч?????? ???м???л??ь м???д?, ????????? ? ???????
[5?7].
???л? ?????ш???? ??????? ???д?????ч??????
????м???????м?? ??д?ч? ???д???? ? ??д?ч? ????д?л???? ???????ц?? ?? ????????м ??м??????м,
????????? ??? ??д?ч? ????? [8]. ?? ??ш???ю ????????? м???????? ?????. ? ??????? [9, 10] ????м?????? ?????????? ??д?ч? ?ц???? ???????ц??
? ?????????? м???д?? ???????????г? ?ц????????.
? ????????? ?????? ?? ?????г???ю??? ???????
??ль???ц?? ? ????г??ц?? ? ????ц??ль??м? ???????м?. ?? ц?ль ??????? ? ???????? ????г? ??ч??л???ль?? ??????????г? м???д? ??ш???? ??д?ч? ????? ?? ?д?????ч??м ??????м ??м??????м.
??????????ь ???дл??????г? м???д? ???люч????? ?
???м?????? м???д? ????м????ч????г? ???д?л????? дл? ?ч??? м????ц? ????? ??????? ??м??????,
??????? ? ????м ?л?ч?? ?? ??л????? д??г???ль???.
57
pb0510.fm Page 58 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
???????ль??? ??д ??л?????ль?? ????д?л?????
??????? м????ц? ?? ?????л??? ??ш??ь ??д?ч? ? ????м ??д? ? ??м??ью ш????? ???????????????г?
????м?, ?????????г? ?? ???г?л????м ???л??????
м????ц [9]. ???дл??????? ? ?????? м???д ???д?л????? ?? ????м???? ?????л???, ??ч?? ? ???????
м????ц? ???ц??ль??г? ??д?, д??????ю??? ???л???ч????? (??ч???, ??????ч???? ???л???ч?????) ??ш???? ??д?ч? ?????, ???д?л???ь ??ш???? д? ????г?, ??????? ????ч??? ???ль??м ?ш????м ??????? ??м??????, ??????????? дл? ????г?ц?????г?
????м????. ??? ?????ч????? ???л???ц?? ????ль????л?? ????м??? JNSGyro-4 [11]. ???????м????
??????л?, ч?? ?????????? ?ч?? ???????ц??????
м????ц? ??????? ??м?????? ?????л??? ?м??ьш??ь
??г??ш????ь ????д?л???? ???????ц?? ???м????
?? 30 %, ??????ч???? ?? ???л?ч???? ??ч??л???ль??ю ?л??????ь ???г??мм?, ??????ю??? ? ???ль??м ???м??? ? ?????л?ю??? г????????????? ????д?л??ь ???????ц?ю 20 ???/?.
1. ќ—Ќќ¬Ќџ≈ ќЅќ«Ќј„≈Ќ»я
» —“–” “”–ј ƒјЋ№Ќќћ≈–Ќџ’ »«ћ≈–≈Ќ»…
Xrk, Yrk, Zrk ? д???????? ????д????? ???????
????м???? r ? k-ю ????? ? г??ц?????ч????? ?????м? ????д???? WGS-84;
s
s
s
X k , Y k , Z k ? д???????? ????д????? ????????
s ? м?м??? ???м???, ???дш?????ю??? k-? ?????
?? ???м? ??????????????? ??г??л? ?? ???????? д?
????м???? (? ?????м? WGS-84);
s
1
s
n kr T
? rk = ||Xr ? X ||, ?rk = ( ? rk , ..., ? rk ) ? R
n rk
;
c ? ???????ь ????? ? ?????м?;
sb
f ? ч?????? ??????? ? д???????? b, ????ч?ю??? ???????? s. ?л? ?????м GPS ? GALILEO ч?????? ?? ??????? ?? ??м??? ????????, дл? ?????м?
GLONASS ??????м???ь ч?????? ?? ??м??? ч???????? л????? ???????? ????д?л??? ? д???м???? [3];
sb
sb
? = c/f ? дл??? ??л?? ???????, ????ч?ю???
???????? s ? д???????? b;
?rk ? ?д??г ш??л ???м??? м??д? ?????м?? GPS
? ?????????? ш??л?? ????м???? r;
s
? k ? ?д??г ш??л ???м??? м??д? ?????м?? GPS
??д?м ??ль?????ь?? ?л?д?ю??м? ??????ч????м?, ???????м? ? ??????????? ????г?ц?? ? ??ч??л???ль??? м???д?? л??????? ?лг???? (??льш??
ч???ь ???????л??м?? ? ????ь? ??????? ? ??????ч???? ш????? ???????? ? ?????д???? ?д??ь ??ль??
дл? ?д?????? ч?????л?):
??????? ??д??? s ????ль?????? дл? ??????ч????
??м??? ????????;
?????? ??д??? r ????ль?????? дл? ??м???ц??
?????? ????г?ц?????? ????м?????, ?????л??????? ?? ????д?м ??л?, r = 0, ..., m ? 1; m l 3;
?????? ??д??? k ????ль?????? дл? ??м???ц??
м?м????? ???м???, ???????м?? ?????м?;
nrk ? ч??л? ?????????, ????д?????? ? ???????
????м????м r ? м?м??? ???м??? k;
??????? ??д??? b ????ль?????? дл? ??????ч???? ч???????? д?????????, b = 1, 2 (L1, L2);
? ?????????? ш??л?? ???????? s.
?????????? д? ????????? ??м??????? ? д???
ш??л?? ? ??д???? ? ???????. ??д???? ш??л? д???
??г??ш????ь ????д?? ?????ль??? м????? ? ?? ???м? ??? ??????? ш??л? д??? м?лл?м??????ю ??г??ш????ь, ?? ??д????? ??? ???????м?ю ??????ю
???д?????ч????ь, ?????м?ю??ю ?????????? ?
ц?л?ч??л????? ???ч????, ???л?ч??? дл? ???д?г?
????????. ??л?? ??ч??, ??????? ???д?????ч?????
????????? д? ????????????? ???ч??? ????, ??????? ??????? ? ???м????м ?????м ?л?????? ? ???л?? ??????? ??????д???????, ??л?ю????? ч???ью
???г??мм??г? ??????ч???? ????г?ц?????г? ????м????. ??л?? ??д????? ????????? ???????????
????г?ц?????? ??м?????? ? ????????? ?ш????
???????, ?????м??, ? ?????? [1]. ? д???л????? ?
???д????м ??????ч????м ????д?л?м ?л?д?ю???
??л?ч???:
Rd ? d-м????? ???????????? ???л?д??? ???????????? ? ???м?? ||x||;
??м??л T ????ч??? м????ч??? ????????????????, ??????? ?ч???ю??? ???л?ц?м?;
? rk ? ??д???? ??м?????? ?????????? м??д?
????м????м r ? ????????м s, ??м??????? ? м?-
Rd?m ? ???????????? ??????????????ч??? м????ц ???м??? d ? m;
= ( ? rk , ..., ? rk )T ? R
||x||F =
2
x 11
+ ... +
2
x dm
=
T
tr ( x x ) ? м????ч???
???м? ?????????? м????ц? x ? R
T
d?m
, tr(?)? ?л?д
м????ц?, ||x||F, W = tr ( x Wx ) , гд? W ? ??л?????ль?? ????д?л????? ??????? м????ц?;
58
sb
b
м??? ???м??? k ? ч???????м д???????? b, ? rk =
n rk b
1b
n rk
;
sb
? rk ? ???л?г?ч??м ??????м ????д?л????? ??м?????? ????д?д?ль????? ? ??????? ш??л? ??м?b
1b
n rk b
?????, ? rk = ( ? rk , ..., ? rk )T ? R
n rk
.
? ?ч???м ???д????? ??????ч???? ?????????,
???????ю??? ??м??????? ????д?д?ль????? ? ?o-
CONTROL SCIENCES є 5 ? 2010
pb0510.fm Page 59 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
д???? ? ??????? ш??л?? ? ??????????м? д? ?????????, ?м?ю? ??д:
s1
s
s
s
s1
s2
s
s
s
s2
? rk = ? rk + c(?rk + ? k ) + I rk + ? rk ,
? rk = ? rk + c(?rk + ? k ) + ? I rk + ? rk ,
s1
s1
s
s
s
s1
s1
? ? rk = ? rk + c(?rk + ? k ) ? I rk + ? N r
s2
s2
s
s
s
s2
2s 2
s1
+ ? rk ,
s2
? ? rk = ? rk + c(?rk + ? k ) ? ? I rk + ? N r
1s
(1)
s2
+ ? rk ,
s
гд? ? = ( f /f ) , I rk ? ??д????? ??г??л? ????????
s ? ????????? ??мл? ? ?????м ч???????м д??????sb
??, N r
? ц?л?ч??л????? ???ч???? ??????? ??b
1b
nk b T
?д?????ч??????, N r = ( N r , ..., N r
s2
? rk ,
s1
? rk ,
) ?Z
nk
s1
, ? rk ,
s2
? rk
? ?ш???? ??д???? ? ??????? ??м??????, ??люч?ю??? ? ???? ш?м???ю ??????л?ю??ю ? ?ш???? м??г?л?ч??????.
? ?????м? ????????? (1), ???????м?? ?????м??
????????? ??????????? ????г?ц??, ?? ???????
?ш???? ??л?????? ????????, ????????? ? ?ш?????
???м???д??? д?????. ?д???? ??? ?ш???? м????
?ч????ь ?д???????? дл? ?л???? ?????л???????
????м????? ?, ?????м?, ??ч???ю??? ??? ??????
????????? ??м??????, ??????????? ? ?д??м? ? ??м? ?? ???????? ? д??м ???л?ч??м ?л???? ?????л??????м ???????м.
2. —¬я«јЌЌџ≈ —»—“≈ћџ ќќ–ƒ»Ќј“,
ќ“Ќќ—»“≈Ћ№Ќјя Ќј¬»√ј÷»я
» «јƒј„ј ќѕ–≈ƒ≈Ћ≈Ќ»я ќ–»≈Ќ“ј÷»»
??????ч?м ???????, ??????л????? ?? ??л?,
??м??л?м? A0, A1, ..., Am ? 1. ????ь x, y, z ? ????????? ? ??л?м д???????? ???м??г?ль??? ?????м? ????д????. ????ь ????????? ?????? ???д????л?? ?
T
????????? ?????м? ????д???? l = (x, y, z) . ???? ??
?????? ? г??ц?????ч????? ?????м? ????д???? ?м?T
?? ???д????л???? L = (X, Y, Z ) .
??д?м ?????ь ????г???ль??ю м????ц? ???????? Q, ??????д???ю ????д????? ?д??г? ? ??г? ??
??????? ?? ?д??? ?????м? ? д??г?ю. ??л?? ??ч??,
L = Ql. ??м????м ??ч?л? ????????? ?????м? ????д???? ? ??ч?? ?????л?????? ??????? A0. ???д?м
? ????м??????? м?????????? ??????? Ar?A0, ???????м?? ????? ???????м? ??????? л????. ????ь
T
lr = (xr , yr , zr) ? ?????? ??????? л???? Ar?A0 ?
????????? ?????м? ????д????, Lrk = (Xrk ? X0k,
T
Yrk ? Y0k, Zrk ? Z0k) ? ??? ?? ?????? ? г??ц?????ч????? ?????м? ????д???? ? м?м??? ???м??? k, l
ѕ – ќ Ѕ Ћ ≈ ћ џ ” ѕ – ј ¬ Ћ ≈ Ќ » я є 5 ? 2010
3 ? (m ? 1)
? м????ц?, ???л?ц?м? ???????
? Lk ? R
??л?ю??? ??????? lr ? Lrk.
??????ч?м ч???? ? ???????? ?????? ????????
м??д? ??л?ч???м?, ????????м??? ? ??????? Ar ,
r = 1, ..., m ? 1, ? ??л?ч???м?, ????????м??? ? ??????? A0. ??????????? ??д????? ?д??????? дл?
?л???? ?????л??????? ??????, ? ?????м? ??д???
s
r ??? ??л?ч???? I rk ? ?????????? (1) м???? ???ь
??????. ???м???? ???????? ? ? ?????????м (1) ?
?????м?? ?? ???м????, ч?? ??л?ч???, ?? ??д??????? ??д??? r, ??? ?????? ???????? ??ч???ю?,
??л?ч?м
s1
s
s1
s2
s
s2
? ? rk = ? ? rk + c??rk + ? ? rk ,
? ? rk = ? ? rk + c??rk + ? ? rk ,
(2)
s1
s
s1
s1
1
1s
? ? rk = ------- ? ? rk + f ??rk + ? N r + ? ? rk ,
s1
?
s2
s2
s2
1 ? s + f 2s
? ? rk = ------ ? rk
??rk + ? N r + ? ? rk . (3)
s2
?
??д?ч? ????????ль??? ????г?ц?? ??????? ? ????д?л???? ??л?????? ??????? Ar ????????ль??
??????? A0, ?. ?. ? ????д?л???? ??????? Ar?A0 ? ????ль???????м ????????? (2) ? (3), ???????м??
?????????м? ????????ль??? ????г?ц??. ? г?????
????????? (2) ????ль??ю??? ?д?????ч???, ?? ????ч??? ??д???? ??м??????. ? г????? ?????????
(3) ????ль??ю??? ???д?????ч???, ?? ??ч??? ??????? ??м??????. ????д?????? ???л?????? ш?м??
?3
??????? ??м?????? ?м?ю? ????д?? 10 м ? ??
???м?, ??? ??д???? ??м?????? ?м?ю? ????д??????
?1
1
???л?????? ????д?? 10 ч10 м. ??????? ???дs1
s2
?????ч????? ? N r ? ? N r , ??????????ю??? ??
?????? г????? ????????? (3), ?????м?ю? ц?л?ч??л????? ???ч????. ???ч?? ???ц?д??? ?????ш????
??????? ???д?????ч?????? ???д?л?ю? ?? ?ц????
???????????? ???ч???? ???д?????ч??????, ?????
???л?чш?? ц?л?ч??л????? ????????м?ц?? ? ???????? ?????ль????? ?????ш???? ???д?????ч??????. ??? ???л???ц?? м???д?, ????????г? ? д?????
??????, ????ль????л??ь ??ч??л???ль??? ???м? ???????????г? ?ц???????? ???????????? ?ц???? ??????? ???д?????ч??????, ????????? ? ?????? [12].
??л?ч??л????? ?????ш???? ??????? ???д?????ч?????? ???????? ?? ц?л?ч??л????? ???д????ч???
????м???ц?? ? ??м??ью м???д??, ????????? ?
??????? [6, 7]. ? ??м??? д????? ????ь? ?ч????м, ч??
???ц?д??? ?????ш???? ??????? ???д?????ч??????
s1
??? ????л???? ? ???ч???? ? N r
s2
? ? Nr
????????.
59
pb0510.fm Page 60 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
????ь ????д????? X0k, Y0k, Z0k ??????? A0 ? м?м??? ???м??? k ??? ???????? ?? ??ш???? ??д?ч? ?
???м??ьш?? ???д????? ??????? д??? ?????? ????г?ц?????? ????????? (1) ??? r = 0. ???????м ??д?ч? ????д?л???? ???????? Lrk ? ??м??ью ??ш????
??д?ч? ? ???м??ьш?? ???д????? ??????? ????????? (3). ?д?? ?????ц?? м???д? ?????? ? ?ью????
[5] дл? ?????м? ??л??????? ????????? ??????л????? ??ш???ю ??д?ч? ? ???м??ьш?? ???д?????
дл? л?????????????? ?????м?. ????ш?м ????????? (3), л?????????????? ? ??????????? ??ч?? X0k,
Y0k, Z0k ? м?м??? ???м??? k. ??????ч?м:
sb
? rk
=
sb
(? ? rk
?
1b
b
? rk = ( ? rk , ...,
sb
sb
? N r )? ,
n rk b T
? rk )
? R
Jrk ? R
n rk „ 3
?rk ? R
n rk
2n rk
X 1k ? X 0k
X 2k ? X 0k
Y 1k ? Y 0k
L? k =
Z 1k ? Z 0k
Y 2k ? Y 0k
?? 1k
=
60
Z 2k ? Z 0k
?? 2k
n
(5)
гд? ( ? ? )2 ? ????????? д???????? ?ш???? ??????
n
2n
V = diag(W, W) ? R .
? (m ? 1),
Jk C
? R
2n k „ 4
..
. X ( m ? 1 )k ? X 0k
..
. Y ( m ? 1 )k ? Y 0k =
..
. Z ( m ? 1 )k ? Z 0k
.. ??
( m ? 1 )k
.
( ?? 1k, ..., ?? ( m ? 1 )k )
1
W = diag(( ? ? )?2, ..., ( ? ? )?2),
L? k
Jk C
Lk
?ш???? ??м??????. ?л? ??????? м????ц?
2
||Ak L? k ? ?k|| F, V ? min,
??????м? м????ц? Jrk ??л?ю??? ??????? ??????л?ю??? ????????? л???? ??????????? ???????? s = 1, ..., nrk ?? ??????? Ar. ?л? ??????, ?????л??????? ?? ?????????? ?????ль??? м?????
д??г ?? д??г?, л???? ??????????? ?? ????????,
?????л??????? ?? ?????????? ?20 000 ?м ?? ???,
??????ч???? ??лл???????. ???д??л???м, ч??
ч??л? ? ?????? ?????????, ???люд??м?? ???????м? Ar, r = 0, ..., m ? 1, ?д???????. ??? ???м ???д??л?????? ????л??ю??? ??л???? nrk ? nk, Jrk ? Jk,
гд? nk ? ????? дл? ???? ?????? ч??л? ???люд??м?? ?????????, ? Jk ? ????? дл? ???? ?????? м????ц? ????? ??????-????ц?? ?rk. ??л??, ??????nk
sb
гд? ??????? ?л?г??м?? ? ? rk , ????ч?ю??? ??
д?ч? м???м???ц?? ????ш????? ??мм? ???д?????
??????? ????????? ????????ль??? ????г?ц?? (4)
,
.
T
(4)
????????? ??????? ??м?????? ? ? rk , ???????м ??-
? м????ц? ????? ??????-????ц??
ч?м: C = (c, ..., c) ? R , Ak =
Ak L? k = ?k,
sb
n rk
? ?1 ?
2n rk
rk?
? R
,
?rk = ?
? 2?
?
? rk ?
?k = (?1k, ..., ?(m ? 1)k) ? R
? ?ч???м ???д????? ??????ч???? ?????????
????????ль??? ????г?ц??, л?????????????? ? ??????????? ??л?????? X0k, Y0k, Z0k ??????? A0, ?????м?ю? ??д
.
,
(6)
???????ц?? ????????? ?????м? ????д???? ????????ль?? г??ц?????ч????? ?????м? ????д???? ?
м?м??? ???м??? k ????д?л????? м????ц?? Qk. ?л?
????????? д?л?? ??????м ??д??? ??????м???? ??
???м??? k ? ????????? (6). ????м??? ????д?л???? L = Ql, ??????ш?м ??д?ч? (6) ??? ??д?ч? ????д?л???? ????г???ль??? м????ц? Q, д?????л?ю??? м???м?м ? ??д?ч? ? ???м??ьш?? ???д?????
2
||A L? ? ?|| F, V ? min, L? =
Ql
( ?? 1, ..., ?? m ? 1 )
L?
T
Q Q = I.
,
(7)
??????ш?м ??д?ч? (7) ? ??д?
2
min ||A L? ? ?|| F, V =
L?
?
2
min
= min ?
||A L? ? ?|| F, V
Q ? ?? 1, ?? 2, ..., ?? ( m ? 1 )
?
?,
?
T
Q Q = I.
J
??????ч?м H =
? R
2n ? 3
(8)
, D =
C
? R2n.
J
C
???? Ђ??????????ї м???м?м ? ????????? (8), ??л?ч?м
2
||HQ l ? ?|| F, W ? min,
Q
QTQ = I.
(9)
1
T
W = V ? ---------------VDD V.
T
D VD
CONTROL SCIENCES є 5 ? 2010
pb0510.fm Page 61 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
??м???м, ч?? ??????? м????ц? W ?????д??? ?
?м??? д????? ???г? 1. ?????ц? V ????д?л????? ?????????м? (5) ? (6) ? ???????? ????????? ш?м??
??????? ??м??????. ????ль??? ????д?л???? ???м? ??????????, ??????ш?м ??д?ч? (9) ? ??д?
||HQ l ? ?||
T
2
F, W
T
T
= tr(HQ l ? ?) W (HQ l ? ?) =
T
T
T
T
= (tr( l Q H W HQ l ) ? 2tr( l Q H W ?) +
T
+ tr(? W ?) ? min,
????ь H
??ц?
+
QTQ = I.
(10)
? ????ш????? ????д????????? м??-
T
T
T
T
T
T
??л??, tr( l Q l *) = tr(l * Q l ) = tr( l l * Q ). ????T
??ч?м B = l l * ? ??????ш?м ??д?ч? (13) ? ??д?
tr(BQ) ? max,
T
Q Q = I.
(14)
???д?????? м????ц? B ? ??д? ???г?л????г?
T
T
???л?????? B = P ?T, P P = T T = I, ? ? д??г???ль??? м????ц?, ??л?ч?м ??ш???? ??д?ч? (14) ?
T T
??д? Q = T P .
????м ??????м, ??м??? ??????? м????ц? W
? ??д? (12),
????м?г???ль??? ??????? м????ц?? W
?????л??? ????? ???л???ч????? ??ш???? ??д?ч? (11).
H + = (HT W H)?1HT W .
T
???д??л?г?????, ч?? n l 4 ? м????ц? H W H ?
? R3 ? 3 ???????д???, д??? ?????м?? ?? ???м????
+
?????д??????ь м????ц? W . ??????ч?м l * = H ?
??ш???? ??д?ч? ? ???м??ьш?? ???д?????
||H l ? ?||
2
F, W
3„3
??ль???ш?? ?????????????? ????????? (10)
?????д?? ? ?л?д?ю??? ??д?ч?:
2
T
T
T
T
Q Q = I. (11)
??д?ч? ???д????ч??? ????м???ц?? (11) ???
???д????ч??? ?г????ч????? ??ш????? ??????????м? м???д?м?, ?????????м? ?? м???д? ?ью???? [5,
13]. ?? ?л?д??м д??г?? ???м?. ? ??ч????? ?????г?
ш?г? ????д?л?м д??г?ю ????м?г???ль??ю ??????ю
м????ц?, д??????ю??ю ???л???ч????? ??ш????
??д?ч? (11). ???д???? ????м?г???ль??? ???????
м????ц? ?????л??? ???????ь?? ?? ???д????ч??г?
?л?г??м?г? ? ?????ль?????ь?? ????????м ??ч??л???ль??м ????м?м (?м., ?????м??, ?????? [9]).
????м ??ш???? ????м????ч???? ???д?л?????? д?
??ш???? ??д?ч? (11) ??? ????д??? ??????? м????ц? W .
3. ќѕ–≈ƒ≈Ћ≈Ќ»≈ ¬—ѕќћќ√ј“≈Ћ№Ќќ…
¬≈—ќ¬ќ… ћј“–»÷џ
????м????м ????м?г???ль??ю ??????ю м????ц?
? = H +TH +.
W
(12)
+
?????м?? ?? ???м???? ???д????? H H = I ?
????г???ль????ь м????ц? Q, ??????ш?м ??д?ч?
(11) ? ??д?
T
T
tr( l Q l *) ? max,
T
Q Q = I.
ѕ – ќ Ѕ Ћ ≈ ћ џ ” ѕ – ј ¬ Ћ ≈ Ќ » я є 5 ? 2010
+T
+
H ,
(15)
гд? ????м??? ? ?????м??? ???ч???? ?? ???????л?
[0, 1]. ??ч???? ?? ???ч???? ? = 0, дл? ??????г? ??ш???? ??д?ч? (11) ??л?ч???, ???д?м ?г? ????м????ч????? ???д?л????? д? ???ч???? ? = 1, дл? ??????г? W(1) = W . ??д?????? м????ц? (15) ? ????????? (10), ??л?ч?м
T
T T
||HQ l ? ?|| F, W = tr( l Q H W HQ l ) ?
? 2tr( l Q H W Hl *) ? min,
???д?м ? ????м??????? ??????ю м????ц?, ????????ю ?? ????м????,
W(?) = ?W + (1 ? ?)H
? min .
l?R
4. ѕј–јћ≈“–»„≈— ќ≈ –ј—Ў»–≈Ќ»≈
јЌјЋ»“»„≈— ќ√ќ –≈Ў≈Ќ»я
(13)
T
T
T
tr[ l Q (?H W H + (1 ? ?)I )Q l ] ?
T
? 2tr[ l QT(?HTW H + (1 ? ?)I )l *] ? min,
QTQ = I.
T
??????ч?м M = H W ? I. ??гд? ???л?д??? ??д?ч? м???м???ц?? ?????м??? ??д
T T
1
? --- tr( l Q MQ l ) ? ?tr(BMQ) ? tr(BQ) ? min,
2
T
Q Q = I.
(16)
????ь Q(?) ? ??ш???? ??д?ч? (16). ???д?л??м
Q(?) ?? ????м???? ?? ? = 0 д? ? = 1.
????ь ? ? м????ц? м??????л?? ??г?????, ????ч?ю??? ?г????ч????м QTQ = I ? ??д?ч? (16).
??? ??мм????ч?? ? ??л? ???ц????? ?г????ч????.
????ц?? ??г????? ?????м??? ??д
T T
1
L(Q, ?, ?) = ? --- tr( l Q MQ l ) ? ?tr(BMQ) ?
2
3
? tr(BQ) +
3
3
? ? ?ij ? (qkiqkj ? ?ij),
i=1 j=1
(17)
k=1
? 1, i = j,
?ij = ?
? 0, i ? j.
61
pb0510.fm Page 62 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
????ь ?(?) ? м????ц? м??????л?? ??г?????,
Ј
????ч?ю??? ??ш???ю Q(?), ? Q (?) ? ?Ј (?) ? ??л-
??? ???????д??? м????ц Q(?) ? ?(?) ?? ????м???? ?. ??????дл??? ?л?д?ю??? ??????д????.
????? 1. ??????? ?????????? ????????, ?????????? ??????? Q(0), ???????????? ????????
T
?(0) = 1
--- P?P . ?
2
??????м дл? ????????? ??м??л ??????м???? ??
Ј
????м???? (?) ? ?????????? Q(?), ?(?), Q (?) ?
?Ј (?).
????? 2. (a) ?????? ???????????
Ј
(18)
Q = QS,
??? ??????? S = ?S' ????????????????.
(b) ???????????????? ??????? S ? ????????????
??????? ?Ј ????????????? ????????? ?????????
T
T
?Q MQSll + 2S? + 2 ?Ј =
T
T
T
T
= Q MB ? Q MQll . ?
(19)
????????ль???? л?мм ?????д??? ? ???л??????.
??????ч?м
T
T
T
T
? = Q MQ, R = Y0 Y 0 , ? = Q MB ?
T
T
Q MQY0 Y 0 .
???????????????? ????????? (19) ? ??ч?????? ?? ??г? ????ль???? д???
??SR + ?RS? + 2S? + 2?S = ? ? ?T.
(20)
????????? (20) ????????ль?? ??????мм????ч??? м????ц? S л?????? ? м???? ???ь ??????????
??? ?????м? ???? л??????? ????????? ????????ль?? ???? ???д??г???ль??? ?л?м????? S. ???л? ????д?л???? м????ц? S ????????? (18) ????д?л???
Ј
м????ц? Q . ??????ц, ????????? (19) ??ш????? ????????ль?? м????ц? ?Ј .
Ј
??л??? ???????д??? м????ц Q ? ?Ј , ??ч??л????? ??? ????????м ???ч???? ????м???? ?, ????д?л?ю? ??????л???? ??м?????? Q(?) ? ???????????ю???
м????ц? м??????л?? ??г????? ?(?). ?л? ?????????л???? ???? ?????????? Q(?), ?(?) ??? ??м?????? ????м???? ? ?? 0 д? 1 ?????ль???м?? м???д?м л?м????
??л???. ????ь ?? ? ш?г ??м?????? ????м????. ??ч???? ?? ???ч???? Q(0) ? ?(0), ??ч??л??м
??м???м, ч?? ????????? (?1) (?м. ???л??????),
Ј
????д?л?ю??? ??????л???? ??м?????? Q ? ?Ј ,
м???? ????м???????ь?? ??? ????????? ?ью????????? ?????ц?? дл? ??????-????ц?? ?L(Q, ?, ?)/?qst
T
??? ?г????ч???? Q Q = 1.
??ч??л???ль??? ???????м???? ????????ю?,
ч?? м????ц? Q(0), ??ч??л????? ? ??ч?ль??? ??ч??
a = 0, д??? ????ш?? ????л?????? ? ??ш???ю Q(1).
?????м? д???-ч?????? ?????ц?? ???ч?? д??????ч??
дл? ??л?ч???? ??ч??г? ??ш???? (?? = 0,25...0,5).
5. –≈«”Ћ№“ј“џ Ќј“”–Ќџ’ »—ѕџ“јЌ»…
??ль ????????? ??????л? ? ???????? ????????г? ?лг????м? ? ?????ч????? ? ????м???ч????? ??л????? ? ?ц???? ??ч????? ????д?л???? ???????ц??.
??ч????ь ????д?л???? ???????ц?? ??????? ??
????м??г? ??л?????? ?????? ? ?????????? м??д?
??м?. ?????? ??????? ЂMarantї [11] ??л? ??????л??? ?? ???ш? ????м???л? Ђ????ї (???. 1). ? ?????м? ????д????, ??ч?л? ??????? ?????д??? ? ???????? A0, ??ь x ??????л??? ?д?ль ц?????ль???
??? м?ш??? ? ??лл??????? ??????? l1 = A1 ? A0,
??ь y ????г???ль?? ??? x ? ??????л??? ??????? ?
?л??????? ???ш?, ??ь z д???л???? ?????? д?? д?
?????? ??????, ????д????? ??????, ?????д?ю??? ? ????д?????м? ??????? л???? lr = Ar ? A0,
????? (0; 0,766; 0,030), (?0,491; 0,019; 0,001),
(0,488; ?0,020; ?0,007).
????ль????л??
????г?ц??????
????м???
JNSGyro-4 [11]. ?лг????м ??????л ? ???ль??м ???м??? ?? ч?????? 10 ?ц. ??л? ?????д??? ?????ч????? ? ????м???ч????? ?????????. ?????ч????? ????????? д??? ???д????л???? ?? ?ш????? ????д?л???? ???????ц??. ?л???ль????ь ????? ???????л?
2400 ???ч????. ????????л??ь ???д?????д????ч????? ???л?????? ?гл??, ??ч??л????? ? ????ль???????м ????м?г???ль??? ??????? м????ц? (12) ? ??????? м????ц? (5), ????ч?ю??? ???ль??м ?ш??s
??м ??????? ш?м??. ? ???м?л? (5) ???ч???? ? ?
??м???л??ь ? д???????? ?? 0,02 д? 0,07 ц??л? ? ??-
Ј
Q((k + 1)??) = Q(k??) + Q (k??)??,
?((k + 1)??) = ?(k??) + ?Ј (k??)??
дл? k = 0, 1, 2, ... д? ??? ???, ???? k?? д????г???
Ј
???ч???? 1. ?????ц? Q (k??) ? ?Ј (k??) ??ч??л?ю??? ? ???????????? ? ??л??????? ???м??.
62
???. 1. ???????????? ?????? ?? ????? ??????????
CONTROL SCIENCES є 5 ? 2010
pb0510.fm Page 63 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
?????????? д??????? ????м???л? ? ????д?????? Ђд?лг??? ? ш?????ї ? ??ч???? ????м???ч????г? ????? ???д????л??? ?? ???. 2, ?? ???. 3 ? г?????? ??м?????? ???м??? ? ?????. ??м?????? ???г??? ????? ш?м???? ???????? ? ?? ???д????л?ю?
????????. ???м?? ??м??????? ? д???????? ?? 0 д?
360∞. ??л?????????? ??м?????? ????? ??????? ???л???м ????м???л? ??? ?????????.
«ј Ћё„≈Ќ»≈
???. 2. ?????????? ???????? ?????????? ? ??????? ??????????????? ????? (?? ???? ???????? ??????? ? ?????? ? ???????
????, ?? ? ???????, ?????? ? ??? ?????????? ????? ?????
???????)
????м???? ?? ?гл? ?????ш???? ????????. ???д?????д????ч????? ???л?????? ?ш???? ???г???, ????? ? ???м??? ????? 0,22, 0,17 ? 0,08∞ ??????????????. ??? ????ль??????? м????ц? (12) ?ш????
?????л??ь ???м???? ?? 30 % ??льш?. ???? ????ль??? ???д??м ? ??д?????д??? ц?л????????????ь
???м?????? ???????г? ? ?????? м???д?. ?г? ??ч??л???ль??? ????????????ь ??д?????д????? ???м??????ью ???л???ц?? ? ???ль??м ???м???. ?????л???????? ???г??мм? ??????л?, ч?? ??ч??л???ль??? м???д, ?????????? ?? ???д?л????? ??
????м???? ? д?ю??? 30 %-? ?м??ьш???? ??г??ш?????, ???л?ч???л ??ч??л???ль??ю ???д??м????ь
?? ??л?? ч?м ?? 40 %. ???????м ????ч????м ?ш???? ??? ?????????? ??л ??л?? ш?м ? м??г?л?ч?????ь, ????????? ?????????м ??д????г??л? ??
???ш? ????м???л? ? ????м?м ?????????г? ??г??л? ???????? ???????? ???????.
?????? м???д ????д?л???? ????????ль??? ???????ц?? ????д?г? ??л?. ????, ? д????????ль?????,
??ш????? ??д?ч? ???????ц?? ????д?г? ??л? ????????ль?? л???ль??г? г????????, ??д?ч? ????д?л????
????????ль??? ???????ц?? л?г?? ??ш????? д????л????м ??д??????л?, ????д?ю??г? ?????м?ц?ю
?? ???????ц?? м??д? ?????ль??м? ??л?м?. ?????д????? ???????? ????????? ??д?????д?ю? ????????????????ь ???дл??????г? м???д?. ? ??л?ч?? ??
д??г??, ????? ????????? м???д??, ???дл???????
м???д ?????л??? ?????ль?? ?ч???ь ???ль??ю ????????? ???????ц?????? ?????? м??д? ?ш????м? ??????? ??м?????? ??? ??????? ??ч??л???ль???
?????????????.
ѕ–»Ћќ∆≈Ќ»≈
????????ль????
л ? м м ? 1. ??л???? ???ц???L ( Q, ?, ? )
???????? ??ч?? (Q, ?) ????ц?? (17) -----------------------------= 0,
?q st
?=0
qst ? ?л?м???? м????ц? Q, ?????м??? ??д
?L ( Q, ?, ? )----------------------------?q st
+
= ?bts +
?=0
?
? ?it ? q
j?t
st
? (q q ? ? ) +
si st
it
? ?it ? q
i?t
(qstqsj ? ?tj) + ?tt
st
?
(q q ? 1) =
? q st st st
= ?bts + 2 ? ?itqsi = 0
i
???. 3. ??????? ????????? ??????? (?) ? ????? (?) ? ??????? ??????????????? ?????
ѕ – ќ Ѕ Ћ ≈ ћ џ ” ѕ – ј ¬ Ћ ≈ Ќ » я є 5 ? 2010
63
pb0510.fm Page 64 Monday, September 27, 2010 9:15 AM
”ѕ–ј¬Ћ≈Ќ»≈ ѕќƒ¬»∆Ќџћ» ќЅЏ≈ “јћ» » Ќј¬»√ј÷»я
T
?л? ?B + 2Q?(0) = 0. ?????м?? ?? ???м???? ???л?????? B = P ?V ? ????????? (17), ??л?ч??м ?(0) =
1 T T
T
= --- Q B = P ?P .
2
? ? ? ? ? ? ? ? л ь ? ? ? ? л ? м м ? 2. ??????д???? (?)
?л?д??? ???????д??????? ?? ????г???ль????? м????ц?
Q(?) ? ??????? ??????ль??г? ???????????? ? г????? ????г???ль??? м????ц SO(3) ? ??ч?? I, ????????г? ?? ??????мм????ч??? м????ц.
????????ц????????
??л????
???ц??????????
?L ( Q, ?, ? ) = 0 ?? ????м???? ? д??? ?л?д?ю??? ??л?-----------------------------?q st
??? (?л?д?ю??? ????? ?? ?????м? ? ??????? ????ц??)
2
?L
- qЈ gh
? ?------------------q ?q
g, h
st
+
gh
+
?
? ?Ј gh ? q ? (qkg qkh ? ?gh) +
st k
g, h
1
T T
?
--- tr(l Q MQl) ? tr(BQ) = 0.
? q st 2
(?1)
????ь {?}st ????ч??? ???????????ю??? ?л?м??? м????ц?. ??гд? ???л?м?????? д???????ц???????? ? ????????? (?1) д???
2
?L
T
-------------------- = ?msg{ll }th + 2?th?sg,
?q st ?q gh
?
? ?Ј gh ? q ? (qkgqkh ? ?gh) = 2{Q ?Ј }st,
g, h
st k
(?2)
1
T T
T
T
?
--- tr(l Q MQl) ? tr(BQ) = {MQll ? B }st. (?3)
? q st 2
??м????ц?? ??л???? (?1) ? ?????????м? (?2) ?
(?3) д???
Ј T
Ј
T
T
?MQ ll + 2Q ? + 2 ?Ј = MB ? MQll .
Ј
??д???????? Q = QS ? ???л?д??? ????????? ? ?мT
??????? ?л??? ?? м????ц? Q д??? ????????? (19).
Ћ»“≈–ј“”–ј
1. Leick ?. GPS Satellite Surveying. ? New York: John Wiley &
Sons, 1995.
2. GPS Interface Control Document. ? URL: http://www.navcen.uscg.gov/pubs/gps/icd200/ (д??? ?????????: 27.05.2010).
3. ???????????? ??????ль??? д???м???. GLONASS. ?
URL: http://www.glonass-ianc.rsa.ru/i/glonass/ICD-2002r.pdf
(д??? ?????????: 27.05.2010).
4. GALILEO Interface Control Document. ? URL: http://www.
gsa.europa.eu/go/galileo/os-sis-icd (д??? ?????????:
27.05.2010).
5. ???? ?., ?????? ?., ???? ?. ??????ч????? ????м???ц??. ?
?.: ???, 1985.
6. Lenstra A.K., Lenstra H.W., Lova?sz L. Factoring Polynomials
with Rational Coefficients // Math. Annalen. ? 1982. ?
Vol. 261. ? P. 515?534.
7. ???????? ?. ???????? ? ц?л?ч??л????? ???г??мм????????. ? ?.: ???, 1991.
8. Wahba G. A Least Squares Estimate of Spacecraft Attitude //
SIAM Review. ? 1965. ? Vol. 7, N 3.
9. Crassidis J.L., Landis Markley F., Cheng Y. Survey of Nonlinear Attitude Estimation Methods // J. Guidance, Control, and
Dynamics. ? 2007. ? Vol. 30, N 1. ? P. 12?28.
10. ???????? ?.?., ?????? ?.?. ???л?д?????? м???д?? ??ш???? ??д?ч? ???????ц?? ? ????ль???????м ???????????
?????м // ????г?ц?? ? г?????????. ? 1999. ? ? 2 (25). ?
?. 30?55.
11. http://www.javad.com/jns (д??? ?????????: 27.05.2010).
12. Rapoport L., Barabanov I., Khvalkov A., et al. OCTOPUS:
Multi antennae GPS/ GLONASS RTK System // ION Meeting Proceedings, Nashville, Tennesse, Sept. 14?17, 1999.
13. Gill P.E., Golub G.H., Murray W., Saunders M.A. Methods for
Modifying Matrix Factorizations // Math. of Computations. ?
1977. ? Vol. 28. ? P. 505?535.
?????? ???????????? ? ?????????? ?????? ???????????
?.?. ??????????.
???????? ??? ????????? ? д-? ???.-м??. ????, ??? л???????????,
???????? ????л?м ?????л???? ?м. ?.?. ???????????? ???,
г. ??????, (495) 334-86-60, [email protected]
«а достигнутые трудовые успехи и многолетнюю плодотворную работу
академик –оссийской академии наук, советник –оссийской академии наук,
член редколлегии нашего журнала —танислав ¬асильевич ≈мель¤нов ”казом
ѕрезидента –оссийской ‘едерации награждЄн ќрденом ѕочЄта.
—ердечно поздравл¤ем —танислава ¬асильевича с высокой наградой и
желаем ему дальнейших творческих успехов!
–едколлеги¤ и редакци¤
журнала Ђѕроблемы управлени¤ї
64
CONTROL SCIENCES є 5 ? 2010
Документ
Категория
Без категории
Просмотров
6
Размер файла
773 Кб
Теги
метод, определение, ориентации, относительные
1/--страниц
Пожаловаться на содержимое документа