Chapter 2 System Configuration And Navigation Principles
2.4 Vehicle Guidance Principle And Proposed Process
In the navigation process, the vehicle monitors the objects and doors consecutively by using the map created from learned data. The vehicle moves to a fixed position and stops to check the existence of the monitored objects. As soon as an unusual situation is occurred, a warning is issued. After checking the objects at a location, the vehicle moves to the next position where there are monitored objects or doors, and then continues checking the existence of the monitored objects or whether the door is opened. An illustration of the vehicle navigation process is shown in Figure 2.4.
In order to patrol along the learning path, the vehicle moves along each node in the map one by one. To move from a node to another, a line following technique is used to reduce navigation errors.
When the vehicle moves to a node where there is a nearby monitored object, the computer detects the object by using the position of the learned object. As soon as the computer detects an object from the image, its features are compared with those of the learned objects. If the features are incorrect, a warning is issued. Of course, when the computer can not detect the object, the same warning is also issued. Because the vehicle might navigate far away from the original path gradually, we use a
vision-based technique to reduce accumulative errors for navigation. When the vehicle detects an object and consider it as a learned object, we use its position recorded in the learning process to correct the position of the vehicle for reducing accumulative errors.
Start navigation
Vehicle patrolling
Figure 2.4 A flowchart of proposed navigation process
Although the door is considered an object, the method of recognizing the door End navigation
Door identification Object detection
Whether correct Whether closed
Whether unchecked objects
No Yes
A warning is announced
Yes No
Yes
Correct the vehicle position
No
Navigating unfinished path
situation is different from the method of object matching. After the vehicle moves to a suitable position recorded in the learning process, the computer checks the door situation to see if it is opened or closed. After the vehicle patrols the entire route and checks all objects and doors, a security patrolling process is finished.
Chapter 3
Location Mapping Calibration and Mechanic Error Correction
3.1 Introduction
When a vehicle navigates in indoor environments, the position and distance are important data for the navigation. By using these data, the vehicle can arrive at suitable positions to search monitored objects or doors and navigate along a learned path. In order to get position data and distance values, a camera and an odometer are used in this study, which are equipped on the vehicle.
At first, we utilize an image captured by the camera to obtain the relative position between an object and the vehicle. We use a 2D mapping method to achieve this goal. The detailed process is described in Section 3.2.
The odometer provides the positions of the vehicle in the environment. It records the coordinates and the direction angles of the vehicle when the vehicle navigates.
Possible mechanic errors might cause the real positions of the vehicle to be unequal to the expected values provided by the odometer. Hence, in Section 3.3, we will propose a correction method to reduce the mechanic error so that the vehicle can navigate more stably.
Before describing the above-mentioned methods, we first introduce some coordinates system and the definition of the direction angle of the vehicle for use in this study. We introduce the coordinate systems in Section 3.1.1 and the definition of
the direction angle of the vehicle in the space is described in Section 3.1.2. By using the direction angle, a transformation function between the coordinate systems is described in Section 3.1.3.
3.1.1 Coordinate Systems
Three coordinate systems are utilized in this study to describe the locations of the vehicle and objects. The coordinate systems are shown in Figure 3.1. The definitions of these systems are stated in the following.
u v
I
Room
x y
G
(a) (b)
Vy
Vx
V
Vx
Vy
V
(c)
Figure 3.1 Three coordinate systems in this study. (a) ICS (b) GCS (c) VCS.
(1) The image coordinate system (ICS): denoted as u−v. The u−v plane is coincident with the image plane and the origin I of the ICS is placed at the center of the
image plane.
(2) The global coordinate system (GCS): denoted as x-y. The x-axis and the y-axis are defined to lie to on the ground, and the origin G of the global coordinate system is a pre-defined point on the ground. In this study, we define G as the starting position of the navigation.
(3) The vehicle coordinate system (VCS): denoted as Vx-Vy. The Vx-Vy plane is coincident with the ground. The Vx-axis is parallel to two wheels of the vehicle.
The Vy-axis is parallel to the body of the vehicle. And the origin V is placed at the middle of the line segment that connects the two contact points of the two driving wheels with the ground.
3.1.2 The Direction Angle
The direction angle of the vehicle is defined in the global coordinate system, the x-y plane. The angle denoted as θ represents the rotation degree of the vehicle in the global system and plays a very important role in the coordinate transformations.
θ is the angle between the positive direction of the x-axis and the front of the vehicle. The direction angle θ is set to be zero at the beginning of navigation. The range of θ is between 0 and π if θ is in the first and second quadrants, as illustrated in Figure 3.2(a) and (b). It is between 0 and −π if θ is in the third and forth quadrants, as illustrated in Figure 3.2(c) and (d).
3.1.3 Coordinate Transformation
By using the direction angle and the coordinates of the vehicle in the GCS, we can describe the coordinate transformation between the vehicle coordinate system and the global coordinate system by the following equations.
V cosx Vy sin p x= × θ − × θ+ x
V sinx Vy cos p y= × θ + × θ+y
where xp and yp are the coordinates of the vehicle in the GCS. It is shown in Figure 3.3.
θ
x y
θ
0
π/ 2
π/ 2
− πor−π
(a) (b)
θ
x
θ 0
π/2
π /2
− π or−π
(c) (d) Figure 3.2 Definition of the direction angle (a)(b) θ is positive. (c)(d) θ is negative.
x y
GCS
θ
Vx
Vy
Figure 3.3 The coordinate transformation between the GCS and VCS.
3.2 Location mapping Calibration
The camera is the only sensor of the vehicle to gather the features of the environment, and the techniques of learning and guidance are based on visual perception. Location mapping calibration and image analysis techniques are indispensable in this study. A real location data acquisition method by Location mapping calibration and image mapping is proposed to obtain the relative positions of the vehicle and the surrounding environment precisely.
We use a point set P = {P00, P01, …,Pmn} whose coordinates are known in advance in the VCS attached on the floor, and their corresponding point set V = {V00, V01, …,Vmn} appearing in the image, to compute the VCS coordinates of a set of pixels. The detailed process is described in the following algorithms.
Algorithm 3.1. Real location data acquisition by image taking and mapping.
Input: An image I, as shown in Figure 3.5(a).
Output: A point set V = {V00, V01, …,Vmn} and another point set P = {P00, P01, …,Pmn} Steps:
Step 1. Attach some straight lines on the floor, as shown in Figure 3.4. Set the length between every two vertical lines to be 30 cm and the length between every two horizontal lines to be 30 cm, too, where every rectangle is a 30×30 square. There exists a distance Div equal to 80cm in the front of the vehicle, which the camera can not view.
Step 2. For those tessellated regions shown in the image which are not surrounded by the lines, extend the lines such that every quadrilateral is complete. Mark red points on the intersections of the lines, as shown in Figure 3.5(b).
Step 3. Record the coordinates of each red point Pij(uij, vij) in the ICS and group all such points into the set P.
Step 4. Measure relative coordinates Vij(xij, yij) manually in the VCS of the point Pij
and group such points into the set V.
Div
Figure 3.4 An illustrated of attaching the lines on the floor.
(a)
(b)
Figure 3.5 A method of finding image coordinates of tessellated points in the grabbed image. (a) A grabbed image with tessellated points. (b) The tessellated points marked by red points.
We now have known the VCS coordinates of red points. As for the VCS coordinates of the other pixels, we use an interpolation method to obtain them, as described in the following algorithm.
Algorithm 3.2. Interpolation for computing lateral distances.
Input: A point I(u,v) in the ICS, a point set P, and another point set V.
Output: The coordinates of I in the VCS.
Steps:
Step 1. Compute the varibales a and b of the line equation y = ax + b for lines L0, L1, L2, and L3 by using ICS coordinates of four endpoints, Pij, P(i+1)j, Pi(j+1), and
P(i+1)(j+1) in the following ways, as illustrated in Figure 3.6.
Step 2. Decide whether the point I is in the region surrounded by four endpoints, Pij, P(i+1)j, Pi(j+1), P(i+1)(j+1) by substituting (u, v) for (x, y) of the line equation in Step 3. If the inequalities (3.3) and (3.4) are satisfied, the point I is in this region.
Else, repeat Step 2 to check the next region.
Step 4. By using a horizontal line equation y = v which passes the point I, we obtain two intersections H(uh, vh) and K(uk, vk) as shown in Figure 3.6(a).
Step 5. By using a vertical line equation x = u which passes the point I, we obtain two intersections S(us, vs) and T(ut, vt).
Where i is serial number of the horizontal lines and j is serial number of the vertical lines.
Although the image is distorted slightly, we consider every line as a straight line.
Every region is not rectangular as appearing in the image; therefore, we use the
Pi(j+1) Figure 3.6 An illustration of the interpolation method. (a) A region contains the point I
in the ICS. (b) The projection of the region in (a) onto a floor region.
3.3 Proposed Mechanical Error Correction Method
There exist mechanic errors when the vehicle navigates back and forth, no matter how people control the vehicle or how the vehicle navigates automatically.
Whenever the control instruction is moving ahead or backing off, the vehicle will move away from the original straight path gradually and the coordinates and the direction angle of the vehicle provided by the odometer will be different from the actual position and angle of the vehicle.
A method is proposed for solving the above problem. A mathematical model is set up for correcting mechanic errors. In Section 3.3.1, we will describe the concept of the method briefly. Utilizing this mathematical model; we can adjust the coordinates and the direction angle of the vehicle provided by the odometer, as described in Section 3.3.2. Finally, we hope the vehicle can move along straight path automatically;
hence, we propose a method to solve this problem, as described in Section 3.3.3.
Because there exists a similar problem when the vehicle backs off, we use the same concept to correct navigation errors. It is described in Section 3.3.4.
3.3.1 Brief Description of Proposed Error Correction Model
Due to mechanic errors, a straight path of the vehicle navigation will become a curve path. A detailed situation is illustrated in Figure 3.7. In Figure 3.7(a), the vehicle starts moving and the expected path is a straight line in advance. After moving a distance, the vehicle moves along path d instead of the expected path c. The same situation occurs when the vehicle backs off. In Figure 3.7(b), the vehicle backs along path f instead of path e.
j j
Expected path Actual path
Expected path Actual path
(a) (b) Figure 3.7 Illustrations of navigation path deviation. (a) Moving ahead. (b) Backing
off.
Besides the last problem, the coordinates and direction angle provided by the odometer do not show the actual position of the vehicle in the GCS due to the mechanic error. As shown in Figure 3.8, the vehicle moves from the starting position P0 and arrives at position Pc finally. But the coordinates provided by the odometer are
position. Actually, because the vehicle moves along the curve path, the vehicle has rotated for an angle θ.
Because the path of the vehicle is a curve, we compute a curve equation to represent the path. We measure the deviation distances in advance to build a mathematical equation as a correction model such that the vehicle can modify navigation errors dynamically. The concept is described as follows.
At first, we get some the vertical deviation distance values measured manually when the vehicle navigates forward a distance. According to the values, a second-order equation y = ax2 + bx + c is obtained by a curve fitting technique.
Among the equation, x is the distance between the starting position P0 and the position P1, and y is the deviation distance from the expected path. Moreover, In Section 3.3.2, we will describe how to get the coefficients a, b, and c of the equation. This correction equation always accompanies the vehicle whether the vehicle is controlled by a user or navigates automatically.
To correct the direction angle of the vehicle, we use the first derivative equation to compute the rotation angle. It is like a concept in physics mentioning that when the object is in a curvilinear motion, by using a tangent line, the direction of the vehicle can be computed. Based on such a principle, we can find a tangent line L1 as shown in Figure 3.8. We compute the slope of line L1 by using the first derivative of y = ax2 + bx + c. The process of modifying the values of the vehicle position is shown in Figure 3.9 and a brief computing process is described as follows.
A distance xd is gathered by computing the length between P0 and P1. Substituting xd into the equation y = ax2 + bx + c, we can get the deviation value yd. By using xd and the slope dy 2
dx = ax b+ , we can get an deviation angle θ the vehicle had turned in the GCS. Hence, by using coordinate transformation techniques, we can
use xd, yd and θ to get actual position Pc. The details are described in Section 3.3.3.
Every time the vehicle starts moving, the above method is carried out. As long as the vehicle stops moving, the correct values of xd, yd and θ must be computed again in the next moving circle.
yd
Figure 3.8 An illustration the correction model.
Compute the navigation distance xd
Compute the deviation
¬ Correct the direction angle ¬¬
¬¬
Correct the coordinates of the odometer
End correction
Figure 3.9 Flowchart of correction process.
3.3.2 Curve Fitting for Navigation Path
In Section 3.3.1, we have described the concept of the correction model.
According to the discussion in the last section, to build the second-order curve equation is the most basic requirement. All correction computation is based on this
curve equation, so we describe how we compute the coefficients of the equation first in this section. A detailed process is described in the following algorithm.
Algorithm 3.3. Curve fitting for navigation path.
Input: A navigation path.
Output: An equation y = ax2 + bx + c.
Steps:
Step 1. Measure a deviation distance ydv every 0.5m when the vehicle navigates, as shown in Figure 3.10.
Figure 3.10 An illustration of recording the coordinates.
Step 2. Record the deviation distance ydv when the vehicle navigates a distance x, and denote the data as P1j(xj, y1,j) where xj is navigation distance computed between the starting position and the new position provided by the odometer and y1,j is a deviation distance. We record thirteen data when the vehicle moves a 6m distance. Finally, we group the data into a point set P1 = {P1,0, P1,1, …, P1,12}.
Step 3. Repeat Step 1 two times and group the data into two sets P2 and P3.
Step 4. There are three deviation values at each distance xj. Compute a mean set P in the following way;
3 3 3 3
,0 ,1 ,2 ,12
1 1 1 1
1 , , , ...,
3 i i i i i i i i
P P P P P
= = = =
⎧ ⎫
= ⎨ ⎬
⎩
∑ ∑ ∑ ∑
⎭. (3.7)where the data in the set P are denoted as P x yj( ,j j).
Step 5. By using the least square method, we compute the coefficients of the optimum curve y=ax2+bx+c according to x and j y , as shown in j
Figure 3.11 A figure of the curve.
Because the deviations are slightly different in each navigation path, we measure each deviation three times and use their means to compute the curve equation. In this study, a, b, and c are computed to be a = 0.00008, b = −0.0089, and c = 0.5828. The distance unit we use is cm.
3.3.3 Coordinates And Angle Correction
After computing the path curve equation, the main goal is to adjust the odometer values of a navigating vehicle. Utilizing the curve equation; we can conjecture that the vehicle has shifted how much distance from a straight line when it is still to move a certain distance. A detailed description of the adjustment is described as follows.
In Figure 3.12, the vehicle starts moving from point toward the goal point in the GCS. The direction of the vehicle in the starting position is θ.
Due to mechanic errors, the final position the vehicle arrives at is point P1′(x1′,y1′). Although the position of the vehicle is P′1 and the angle has become θ +α, the odometer shows that the position and angle are P1 and θ . The point P′1 and angle α is the most important data we want to modify the error of the odometer. We compute the distance D as follows:
2
Utilizing D and the curve equation , the deviation distance is derived to be as follows:
c
Then we use the coordinate transformation technique to get the point as follows:
Figure 3.12 An illustration of correction detailed.
In the following, we want to get the rotation angle α , as shown in Figure 3.13.
Finally, we correct the coordinates of the vehicle to be and correct the angle to be θ +α.
Figure 3.13 An illustration of computing correct angle.
3.3.4 Straight Navigation Technique
Because the vehicle always navigates along a curve path, in this section, we propose a technique to control the vehicle to navigate along a straight line automatically. The most important concept behind our technique is that we control the vehicle to return to the original straight path, as illustrated in Figure 3.14. The path of the vehicle is shown as a red line in Figure 3.14.
Assume that the vehicle starts from point , and plans to navigate along path . According to the curve equation
P0
y=ax2+bx c+
L0 , we can get a distance dy when
the vehicle has moved a distance . We use these data to compute a moving distance and a rotation angle such that the vehicle can turn toward path and go back to this path. Although we can correct the values of the odometer at any time, it is impossible to do this in the practical operation. Hence, we correct the mechanic error when the vehicle stops moving.
Dt
L0
P′1
When the vehicle goes to point , the vehicle stops and corrects the values of the odometer. To utilize the tangent line, we compute the rotational angle α . Point which is on the path is the goal for the vehicle to go back to path
. We compute the distance
2( , )2 2
Figure 3.14 An illustration of the correction path
The vehicle should turn 2α toward path and go through the distance L0
P P1 2′ ′ back to , as the path shown in Figure 3.3.4.1. Unfortunately, the vehicle does not arrive at point
L0 L2
P′2 because of the curve path. If the vehicle goes toward point P′2, the path will be like the blue line shown in Figure 3.15 and the terminal will be Pt finally, as shown in Figure 3.15. Hence, the vehicle has to turn an angle of φ such that it can navigate along the curve and arrive at P′2 finally. As
L1
shown in Figure 3.15, the planed path is and the expected goal is , such that the vehicle can arrive at
2( , )2 2
Figure 3.15 Computation of corrected path
Summarizing the above description, we describe the correction process as follows:
Step 1. Start the vehicle from point . According to value of the odometer, the vehicle stops at the point
P0
P′1 after going through a distance of Dt. Step 2. Correct the coordinates and the angle of the vehicle.
Step 3. Compute the turn angle φ by using Equation (3.15) and navigate a distance P P1 2′ ′ .
Step 4. Turn the vehicle for the angle of 2α φ+ toward the straight path and let it navigate for a distance of
L0
P P1 2′ ′ to arrive at point P′2 and stop.
Step 5. Correct the coordinates and the angle of the vehicle again.
Step 6. Turn the vehicle such that the direction of the vehicle is the same as the
Step 6. Turn the vehicle such that the direction of the vehicle is the same as the