第一章 移动机器人的运动模型
- 解：由题意得
R = 20 m 、 L = 2 m t a n α = L R = 0.1 α ≈ 6 ∘ R = 20m、L = 2m \\ tan\alpha=\frac{L}{R}=0.1 \\ \alpha\approx6^{\circ} R=20m、L=2mtanα=RL=0.1α≈6∘ - 解：由题意得
V y = 1 m / s R = 20 m / s W = 1 m V y = ψ ˙ R ψ ˙ = V y R = 0.05 s − 1 V l e f t = ψ ˙ ( R − W / 2 ) = 0.975 m / s V r i g h t = ψ ˙ ( R + W / 2 ) = 1.025 m / s V_{y} = 1m/s \\ R=20m/s \\ W=1m\\ V_{y}=\dot\psi R\\ \dot\psi=\frac{V_{y}}{R}=0.05s^{-1}\\ V_{left}=\dot\psi(R-W/2)=0.975m/s\\ V_{right}=\dot\psi(R+W/2)=1.025m/s Vy=1m/sR=20m/sW=1mVy=ψ˙Rψ˙=RVy=0.05s−1Vleft=ψ˙(R−W/2)=0.975m/sVright=ψ˙(R+W/2)=1.025m/s
第四章 机器人导航
2、解：由题意得
假设地球的半径R为6378127m
x
=
R
c
o
s
(
l
a
t
)
c
o
s
(
l
o
n
g
)
=
6378127
c
o
s
4
2
∘
c
o
s
(
−
8
5
∘
)
=
413107.071
m
y
=
R
c
o
s
(
l
a
t
)
s
i
n
(
l
o
n
g
)
=
6378127
c
o
s
4
2
∘
s
i
n
(
−
8
5
∘
)
=
−
4721835.432
m
z
=
R
s
i
n
(
l
a
t
)
=
6378127
s
i
n
4
2
∘
=
4267799.987
m
x=Rcos(lat)cos(long)=6378127cos42^{\circ}cos(-85^{\circ})=413107.071m\\ y=Rcos(lat)sin(long)=6378127cos42^{\circ}sin(-85^{\circ})=-4721835.432m\\ z=Rsin(lat)=6378127sin42^{\circ}=4267799.987m
x=Rcos(lat)cos(long)=6378127cos42∘cos(−85∘)=413107.071my=Rcos(lat)sin(long)=6378127cos42∘sin(−85∘)=−4721835.432mz=Rsin(lat)=6378127sin42∘=4267799.987m
5、解：由题意得
起始分带为东经
18
0
∘
180^{\circ}
180∘(或西经
18
0
∘
180^{\circ}
180∘)开始，自西向东算起，每隔
6
∘
6^{\circ}
6∘分带，第一带的中央子午线为
−
17
7
∘
-177^{\circ}
−177∘或
18
3
∘
183^{\circ}
183∘。假设地球半径为6378137m。
l
o
n
g
=
8
5
∘
l
a
t
=
2
8
∘
i
=
i
n
t
(
18
0
∘
+
8
5
∘
6
)
+
1
=
45
l
o
n
g
0
=
−
177
+
(
45
−
1
)
∗
6
=
8
7
∘
l
o
n
g
−
l
o
n
g
0
=
−
2
∘
N
o
r
t
h
i
n
g
≈
R
(
l
a
t
)
(
π
/
18
0
∘
)
=
3116945.742
m
E
a
s
t
≈
R
(
l
o
n
g
−
l
o
n
g
0
)
(
π
/
18
0
∘
)
c
o
s
(
2
8
∘
)
+
500000
=
303421.45
m
long=85^{\circ} \quad lat=28^{\circ}\\ i=int(\frac{180^{\circ}+85^{\circ}}{6})+1=45\\ long_{0}=-177+(45-1)*6=87^{\circ}\\ long-long_{0}=-2^{\circ}\\ Northing\approx R(lat)(\pi/180^{\circ})=3116945.742m\\ East\approx R(long-long_{0})(\pi/180^{\circ})cos(28^{\circ})+500000=303421.45m
long=85∘lat=28∘i=int(6180∘+85∘)+1=45long0=−177+(45−1)∗6=87∘long−long0=−2∘Northing≈R(lat)(π/180∘)=3116945.742mEast≈R(long−long0)(π/180∘)cos(28∘)+500000=303421.45m
第六章 遥感
1、解：由题意得
[
X
T
Y
T
Z
T
]
E
a
r
t
h
=
[
X
V
E
a
r
t
h
R
V
E
Y
V
E
a
r
t
h
Z
V
E
a
r
t
h
0
0
0
1
]
[
X
T
Y
T
Z
T
]
V
e
h
\left[ \begin{matrix} X_{T} \\ Y_{T} \\ Z_{T} \end{matrix} \right]_{Earth} =\left[ \begin{matrix} & & & X_{{V}_{Earth}}\\ & R_{VE} & & Y_{{V}_{Earth}} \\ & & & Z_{{V}_{Earth}}\\ 0 & 0 & 0 &1 \end{matrix} \right] \left[ \begin{matrix} X_{T} \\ Y_{T} \\ Z_{T} \end{matrix} \right]_{Veh}
⎣⎡XTYTZT⎦⎤Earth=⎣⎢⎢⎡0RVE00XVEarthYVEarthZVEarth1⎦⎥⎥⎤⎣⎡XTYTZT⎦⎤Veh 其中：
[
X
T
Y
T
Z
T
]
V
e
h
\left[ \begin{matrix} X_{T} \\ Y_{T} \\ Z_{T} \end{matrix} \right]_{Veh}
⎣⎡XTYTZT⎦⎤Veh 表示目标相对于机器人坐标 ；
同时：
[
X
T
Y
T
Z
T
]
E
a
r
t
h
\left[ \begin{matrix} X_{T} \\ Y_{T} \\ Z_{T} \end{matrix} \right]_{Earth}
⎣⎡XTYTZT⎦⎤Earth 表示目标相对于大地坐标系坐标
R
V
E
R_{VE}
RVE为旋转矩阵如下：
[
c
o
s
ψ
V
E
c
o
s
ϕ
V
E
−
s
i
n
ψ
V
E
s
i
n
θ
V
E
s
i
n
ϕ
V
E
−
s
i
n
ψ
V
E
c
o
s
θ
V
E
c
o
s
ψ
V
E
s
i
n
ϕ
V
E
+
s
i
n
ψ
V
E
s
i
n
θ
V
E
c
o
s
ϕ
V
E
s
i
n
ψ
V
E
c
o
s
ϕ
V
E
−
c
o
s
ψ
V
E
s
i
n
θ
V
E
s
i
n
ϕ
V
E
c
o
s
ψ
V
E
c
o
s
θ
V
E
s
i
n
ψ
V
E
s
i
n
ϕ
V
E
−
c
o
s
ψ
V
E
s
i
n
θ
V
E
c
o
s
ϕ
V
E
−
c
o
s
θ
V
E
s
i
n
ϕ
V
E
s
i
n
θ
V
E
c
o
s
θ
V
E
c
o
s
ϕ
V
E
]
\left[ \begin{matrix} cos\psi_{VE}cos\phi_{VE}-sin\psi_{VE}sin\theta_{VE}sin\phi_{VE} & -sin\psi_{VE}cos\theta_{VE} & cos\psi_{VE}sin\phi_{VE}+sin\psi_{VE}sin\theta_{VE} cos\phi_{VE}\\ sin\psi_{VE}cos\phi_{VE}-cos\psi_{VE}sin\theta_{VE}sin\phi_{VE}& cos\psi_{VE}cos\theta_{VE} & sin\psi_{VE}sin\phi_{VE}-cos\psi_{VE}sin\theta_{VE} cos\phi_{VE} \\ -cos\theta_{VE}sin\phi_{VE} & sin\theta_{VE} & cos\theta_{VE}cos\phi_{VE} \end{matrix} \right]
⎣⎡cosψVEcosϕVE−sinψVEsinθVEsinϕVEsinψVEcosϕVE−cosψVEsinθVEsinϕVE−cosθVEsinϕVE−sinψVEcosθVEcosψVEcosθVEsinθVEcosψVEsinϕVE+sinψVEsinθVEcosϕVEsinψVEsinϕVE−cosψVEsinθVEcosϕVEcosθVEcosϕVE⎦⎤
其中：偏航角yaw：
ψ
V
E
\psi_{VE}
ψVE，俯仰角pitch：
θ
V
E
\theta_{VE}
θVE，滚转角roll：
ϕ
V
E
\phi_{VE}
ϕVE，代入数据得：
[
X
T
Y
T
Z
T
1
]
E
a
r
t
h
=
[
0.79
−
0.6
0.108
102
0.60
0.80
−
0.03
59
−
0.07
0.09
0.99
1
0
0
0
1
]
[
3
15
−
2.2
1
]
V
e
h
=
[
95.1324
72.866
−
0.038
1
]
\left[ \begin{matrix} X_{T} \\ Y_{T} \\ Z_{T} \\ 1 \end{matrix} \right]_{Earth} =\left[ \begin{matrix} 0.79& -0.6 &0.108 & 102\\ 0.60&0.80 & -0.03& 59 \\ -0.07& 0.09&0.99 & 1\\ 0 & 0 & 0 &1 \end{matrix} \right] \left[ \begin{matrix} 3 \\ 15 \\ -2.2 \\ 1\end{matrix} \right]_{Veh} =\left[ \begin{matrix} 95.1324\\72.866\\-0.038\\1 \end{matrix} \right]
⎣⎢⎢⎡XTYTZT1⎦⎥⎥⎤Earth=⎣⎢⎢⎡0.790.60−0.070−0.60.800.0900.108−0.030.9901025911⎦⎥⎥⎤⎣⎢⎢⎡315−2.21⎦⎥⎥⎤Veh=⎣⎢⎢⎡95.132472.866−0.0381⎦⎥⎥⎤
2、同理，将偏航角、俯仰角、滚转角代入旋转矩阵得：
[
X
T
Y
T
Z
T
1
]
E
a
r
t
h
=
[
0.766
−
0.64
0
120
0.64
0.766
0
40
0
0
1
0
0
0
0
1
]
[
−
7
10
2
1
]
V
e
h
=
[
108.238
43.18
2
1
]
\left[ \begin{matrix} X_{T} \\ Y_{T} \\ Z_{T} \\ 1 \end{matrix} \right]_{Earth} =\left[ \begin{matrix} 0.766& -0.64 &0 & 120\\ 0.64&0.766 & 0& 40 \\ 0& 0&1 & 0\\ 0 & 0 & 0 &1 \end{matrix} \right] \left[ \begin{matrix} -7\\ 10\\ 2\\ 1\end{matrix} \right]_{Veh} =\left[ \begin{matrix} 108.238\\43.18\\2\\1 \end{matrix} \right]
⎣⎢⎢⎡XTYTZT1⎦⎥⎥⎤Earth=⎣⎢⎢⎡0.7660.6400−0.640.7660000101204001⎦⎥⎥⎤⎣⎢⎢⎡−71021⎦⎥⎥⎤Veh=⎣⎢⎢⎡108.23843.1821⎦⎥⎥⎤
Δ
ψ
=
ψ
d
e
s
−
ψ
c
u
r
r
e
n
t
=
−
a
r
c
t
a
n
(
x
d
e
s
−
x
y
d
e
s
−
y
)
−
ψ
c
u
r
r
e
n
t
=
−
a
r
c
t
a
n
(
108.238
−
120
43.18
−
40
)
−
40
=
34.8
7
。
\Delta\psi=\psi_{des}-\psi_{current}=-arctan(\frac{x_{des}-x}{y_{des}-y})-\psi_{current}=-arctan(\frac{108.238-120}{43.18-40})-40=34.87^{。}
Δψ=ψdes−ψcurrent=−arctan(ydes−yxdes−x)−ψcurrent=−arctan(43.18−40108.238−120)−40=34.87。，即偏航角在原来的基础上逆时针旋转34.87
Δ
θ
=
a
r
c
t
a
n
(
z
d
e
s
−
z
(
x
d
e
s
−
x
)
2
+
(
y
d
e
s
−
y
)
2
)
=
9.3
\Delta\theta=arctan(\frac{z_{des}-z}{\sqrt{(x_{des}-x)^2+(y_{des}-y)^2}})=9.3
Δθ=arctan((xdes−x)2+(ydes−y)2zdes−z)=9.3，即滚转角逆时针旋转9.3
第八章 障碍物映射及其在机器人导航中的应用
1、解：由题意知
该问题的解为一组x、y值，并满足
(
x
+
10
)
2
+
(
y
−
20
)
2
=
2
8
2
(x+10)^2+(y-20)^2=28^2
(x+10)2+(y−20)2=282
且·
(
x
−
18
)
2
+
(
y
−
29
)
2
=
3
9
2
(x-18)^2+(y-29)^2=39^2
(x−18)2+(y−29)2=392两式相减得：
56
x
+
18
y
=
−
72
56x+18y=-72
56x+18y=−72再带入到第一个式子得：
x=0.6、y=-6或者x=-16.47、y=47.24
569
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
