Page 172 - Introduction to Autonomous Mobile Robots

Perception

∑ 2 ∑ ( ( r) 2 157
S = w d = w ρ cos θ – α) – . (4.72)
i
i i
i
i
It can be shown that the solution to equation (4.70) in the weighted least-squares sense 3
is
2
 ∑ w ρ sin 2θ – -------- ∑ ∑ w w ρ ρ cos sin θ 
2
θ
1  i i i Σw i i j i j i j 

α = ---atan -------------------------------------------------------------------------------------------------------------------- (4.73)
2  ∑ 2 1 ∑ ∑ ( θ ) 
 w ρ cos 2θ – -------- w w ρ ρ cos θ + j 
i
i
j i j
i
i i
Σw
i
∑ w ρ cos ( θ – α)
i i
i
r = ---------------------------------------------- (4.74)
∑ w i
4
In practice, equation (4.73) uses the four-quadrant arc tangent (atan2) .
Let us demonstrate equations (4.73) and (4.74) with a concrete example. The seventeen
measurements ρ θ,( ) in table 4.2 have been taken with a laser range sensor installed on a
i i
mobile robot. We assume that the uncertainties of all measurements are equal, uncorrelated,
and that the robot was static during the measurement process.
Direct application of the above solution equations yields the line defined by α = 37.36
and r = 0.4 . This line represents the best fit in a least-squares sense and is shown visually
in figure 4.37.
Propagation of uncertainty during line extraction. Returning to the subject of section
4.2.3, we would like to understand how the uncertainties of specific range sensor measure-
ments propagate to govern the uncertainty of the extracted line. In other words, how does
uncertainty in ρ i and θ i propagate in equations (4.73) and (4.74) to affect the uncertainty
α
r
of and ?

3. We follow here the notation of [14] and distinguish a weighted least-squares problem if C X is
diagonal (input errors are mutually independent) and a generalized least-squares problem if C is
X
nondiagonal.
⁄
4. Atan2 computes tan ( xy) – 1 but uses the signs of both x and y to determine the quadrant in which
(
(
,
,
–
the resulting angles lies. For example atan 22 – 2) = – 135° , whereas atan 2 2 2) = – 45° , a dis-
tinction which would be lost with a single-argument arc tangent function.