ISSN 0236-3933. Вестник МГТУ им. Н.Э. Баумана. Сер. “Приборостроение”. 2012
76
Поскольку изменение положения системы координат робота опи-
сывается аффинным преобразованием, координаты
i-
й точки простран-
ства в текущий и предыдущий момент времени связаны выражением
,
i
i
C RP T
= +
или
11 12 13
21 22 23
31 32 33
.
C
P
i
i
C
P
i
i
C
P
i
i
X
X
r r r
X
r r r
Y
Y
Y
r r r
Z
Z
Z
⎛ ⎞
⎛ ⎞ Δ
⎛
⎞
⎛ ⎞
⎜ ⎟
⎜ ⎟
⎜
⎟
⎜ ⎟
=
+ Δ
⎜ ⎟
⎜ ⎟
⎜
⎟
⎜ ⎟
⎜ ⎟
⎜ ⎟ ⎜ ⎟
⎜
⎟
⎜ ⎟
⎜ ⎟ Δ⎝ ⎠
⎝
⎠
⎝ ⎠
⎝ ⎠
Выполнив перемножение и сложение матриц, для
i
-
й особой точ-
ки получим
11
12
13
21
22
23
31
32
33
.
C
P
P
P
i
i
i
i
C
P
P
P
i
i
i
i
C
P
P
P
i
i
i
i
X r X r Y r Z X
Y r X r Y r Z Y
Z
r X r Y r Z Z
⎛ ⎞ ⎛
⎞
+
+
+ Δ
⎜ ⎟ ⎜
⎟
=
+
+
+ Δ
⎜ ⎟ ⎜
⎟
⎜ ⎟ ⎜
⎟
⎜ ⎟ ⎜
⎟
+
+
+ Δ
⎝ ⎠ ⎝
⎠
Для нахождения неизвестных решим три системы линейных ал-
гебраических уравнений:
11
12
13
;
, 1,
C
P
P
P
i
i
i
i
X r X r Y r Z X i
N
= + +
+ Δ =
(2)
21
22
23
,
; 1,
C
P
P
P
i
i
i
i
Y r X r Y r Z Y i
N
= +
+
+ Δ =
(3)
31
32
33
, 1, .
C
P
P
P
i
i
i
i
Z r X r Y r Z Z i
N
= +
+
+ Δ =
(4)
Системы линейных алгебраических уравнений (2–4) имеют об-
щий вид:
1
2
3
, 1, .
C
P
P
P
i
i
i
i
x r X r Y r Z t i
N
= + + + Δ =
(5)
Система (5) содержит четыре неизвестных, поэтому для ее реше-
ния необходимо иметь данные по крайней мере о четырех точках. За-
пишем (5) в матричной форме:
,
A y
ω
=
(6)
где
1 1 1
2 2 2
1
1 ;
1
P P P
P P P
P P P
N N N
X Y Z
X Y Z
A
X Y Z
⎛
⎞
⎜
⎟
⎜
⎟
= ⎜
⎟
⎜
⎟
⎜
⎟
⎝
⎠
# # # #
(
)
1 2 3
, , ,
r r r t
=
Δ
ω
;
(
)
1 2
, , ,
.
T
C C
C
N
y x x
x
=
…