# Navigation for Indoor Robot: Straight Line Movement via Navigator

Authors: Chaozheng Zhu; Ming He; Pan Chen; Kang Sun; Jinglei Wang; Qian Huang
Journal: Mathematical Problems in Engineering
 (2018)

---

## Abstract

Due to the need of zigzag overlay strategy, long-term linear motion is essential for sweep robot. However, the existing indoor sweep robot navigation algorithm has many problems; for instance, algorithm with high complexity demands high hardware performance and is incapable of working at night. To overcome those problems, in this paper, a new method for indoor robot Straight Line Movement via Navigator (SLMN) is proposed to ensure long linear motion of robot with an acceptable error threshold and realize multiroom navigation. Firstly, in a short time, robot runs a suitable distance when it is covered by navigator’s ultrasonic sensor. We can obtain a triangle with twice the distance between navigator and robot and the distance of robot motion. The forward angle of the robot can be conveniently obtained by the trigonometric functions. Comparing the robot’s current angle with expected angle, the robot could correct itself and realize the indoor linear navigation. Secondly, discovering dozens of the magnitude gaps between the distance of robot run and the distance between navigator and robot, we propose an optimized method using approximate scaling which increases efficiency by nearly 70.8%. Finally, to realize multiroom navigation, we introduce the conception of the depth-first search stack and a unique encode rule on rooms and navigators. It is proved by extensive quantitative evaluations that the proposed method realizes indoor full coverage at a lower cost than other state-of-the-art indoor vision navigation schemes, such as ORB-SLAM.


---

## Body

## 1. Introduction

Traditional low cost sweep robot used the random mode of sweeping, with small coverage area, low cleaning efficiency, and easy collision. Adopting the shape of Z full coverage algorithm [1] can effectively reduce the disadvantage of random sweeping, but robot must run linearly if the full coverage algorithm can work steadily [2, 3]. It could work normally based on an inertial measurement unit (IMU) [4, 5], but with the data from both sensor and PID correcting its direction, it is not enough to realize its indoor localization. Due to accumulated errors of sensors, wheel mechanical inaccuracy, and the slide problem, sweeping robot would drift away, so researchers start the indoor navigation system [6] research.Rodas et al. proposed an optimal Bayesian fusion and localization algorithm based on Bluetooth positioning data [7]. However, Bluetooth localization error is still serious, deployment cost is huge, and the result is not satisfactory. Lin et al. proposed a fingerprint localization system based on Wi-Fi [8], which is a sample of signal intensity, and preconstruct a radio map, use the information of Wi-Fi cover, and read data from accelerometer reliably. However, a great quantity initial work should be finished before using it, and the effect is not very well. Fuentes-Pacheco et al. proposed vSLAM (visual Simultaneous Localization and Mapping) [9–13]; this scheme has lower deployment cost and high localization accuracy features, and also its visualization is prefect. However, when the light had changed, the result fluctuated greatly. With introducing a Kinect (a depth camera) [14], the cost of full system would rise. In [15], Binu proposed to use the naturally occurring magnetic fields inside the concrete buildings with simplified magnetic map [16] matching algorithm. Due to the existence of sensor accumulation error problems, the navigation’s accuracy is insufficient. Furthermore, Wolfgang Hess et al. propose a 2D SLAM system that combines scan-to-submap matching with loop closure detection and graph optimization, making its source codes open-source [17]. However, equally it needs a LIDAR, which costs much money.Because of the characteristics of light moving straight forward, the infrared ray [18] wavelength is long and mostly would not scatter in the air. An infrared ray is sent by opening a small hole in the front of the navigator, so that it can be considered as a line in terms of the ultrasonic (see Figure 1). The infrared communication module consists of transmitter and receiver. The transmitter is located on the navigator, and the receiver is located on the sweeping robot. A low scattering infrared ray is sent by the transmitter, after being received by the sweeping robot, and it can be regarded as a baseline in the room (we usually put the navigator approximately on the middle of the room’s edge) [19].Figure 1
Infrared ray and ultrasonic wave emission diagram of navigator. From theoretical analysis, the analytic method of deviation angle alpha.Similarly, the ultrasonic module is divided into the transmitter and the receiver. The transmitter is located on the navigator, and the receiver is located on the sweeping robot. Measuring the time of running between the navigator and the sweeping robot and multiplying by the velocity of ultrasonic in the air, we can work out the distance between them. While meeting the infrared baseline, the sweeping robot would start to correct itself. And because the infrared ray has the reflection characteristic, when the infrared ray radiates to the ceiling, the reflection will occur, so that the full room would be covered [20].In a word, to realize indoor linear navigation, the deployment cost based on Wi-Fi [21, 22] and Bluetooth is huge, and vSLAM cannot work normally in the evening. Therefore, when considering the cost problem with guaranteeing high coverage percentages, we propose to use a navigator (consisting of infrared and ultrasonic) to help localization and navigation. It mainly carries out the shape of Z strategy based on motor encoder and PID controller local optimum [23]. At the same time, infrared ray and ultrasonic are used to realize auxiliary localization and correct itself to make global optimum [18]. Comparing to existing algorithms, extensive quantitative evaluations shows that the proposed method realizes indoor full coverage at a lower cost than other state-of-the-art indoor vision navigation schemes, such as ORB-SLAM [11, 24] (see Figure 2).Figure 2
The development route of indoor navigation.The remainder of this paper is organized as follows. Section2 provides an overview of the robot hardware platform. Section 3 explains the proposed localization and navigation algorithm. Finally, Section 4 provides experimental results and comparisons.


## 2. Robotic Platform

In order to verify the navigation algorithm, this paper designs a double navigation platform between sweeping robot and navigator. The sweeping robot adopts Cortex-M3 architecture 32 bit microcontroller stm32 as the main control chip, which is produced by STMicroelectronics. The navigator adopts super low power msp430 as the main control chip, which is produced by the United States TI. A sweeping robot consists of two wheels controlled by deceleration motors, equipped with photoelectric encoders to measure the speed of the two wheels, and uses six-axis gyroscope MPU6050 to calculate its posture [25]. Additionally, the sweeping robot and navigator are equipped with infrared transceiver module to communicate with each other at the top [20]. In flank, an infrared sensor and ultrasonic sensor are used to obtain the distance between them as shown in Figure 3.Figure 3
The infrared relies on ceiling reflection to cover full room communication.


## 3. Localization and Navigation of Indoor Robot

The navigator is located at the edge of the room, and it regularly sends an infrared ray signals without scattering. When the sweeping robot detects infrared ray signal, calculates the distancea
→, and records the wheels roll over the distance b
→, the ultrasonic wave starts to emit regular frequency. With the ultrasonic wave, we could measure c
→ again. As a result, by solving a
→
,
b
→
,
c
→ which form a vector triangle, we can obtain the real forward angle and correct the sweeping robot direction by comparison to the theoretical value as shown in Figure 4.Figure 4
From theoretical analysis, the analytic method of deviation angleα.


### 3.1. Ensure Running in a Straight Line

Traditionally, a sweeping robot can stably execute straight line because two wheels can keep themselves running with the same velocity. Because the electronic characteristics of two wheels may not be completely consistent, in the same PWM duty rate, the velocities of two motors are not exactly the same. If there is long-term running, the error is accumulated continually, and the sweeping robot would move forward one side. Therefore, it is necessity to adjust the velocity of the two wheels in real time so that the velocity of two wheels becomes the same, and robot can keep itself linearly running for a long time without any consideration to the other obstacles.In this paper, we installed two 26-hole photoelectric tachometer encoders at the back of the motors. By measuring the number of pulsesN generated by the photoelectric encoder in unit time T and before that measuring the circumference of the wheelL, the formula is given as follows.(1)
V
=
N
/
26
∗
L
TTheoretically, after obtaining the forward speed ofV, the velocity of the wheel can be adjusted by the PID control algorithm in the field of classical automatic control. However, considering the approximate errors produced on the formula transformation, we directly added the number of pulsesN generated by the photoelectric encoder to the PID formula [26], instead of calculating the forward speed ofV each wheel. Generally, there are two kinds of PID, as shown below.(2)
u
n
=
K
P
e
n
+
T
T
I
∑
i
=
0
n
e
i
+
T
D
T
e
n
-
e
n
-
1
+
u
0Incremental PID control algorithm:(3)
∆
u
n
=
u
n
-
u
n
-
1
=
K
P
e
n
-
e
n
-
1
+
K
P
T
T
I
e
n
+
K
P
T
D
T
e
n
-
2
e
n
-
1
+
e
n
-
2
(4)
e
n
=
r
t
-
c
t
(5)
c
t
=
Nwhereu
(
n
) is the output value of PID and r
(
t
) is the desired value. N is the photoelectric encoder current measurement value, and e
(
n
) is the deviation value. In this paper, DC motor is used for robotic motion, by controlling the duty rate of PWM to control the velocity, so according to the incremental PID control algorithm, the recursion formula is(6)
u
n
=
u
n
-
1
+
∆
u
n
=
u
n
-
1
+
K
P
1
+
T
T
I
+
T
D
T
e
n
-
K
P
1
+
2
T
D
T
e
n
-
1
-
K
P
e
n
-
2
=
u
n
-
1
+
b
0
e
n
+
b
1
e
n
-
1
+
b
2
e
n
-
2
(7)
b
0
=
K
P
1
+
T
T
I
+
T
D
T
(8)
b
1
=
-
K
P
1
+
2
T
D
T
(9)
b
2
=
-
K
Pwhereu
(
n
) is currently the value of PWM, which supplies motor; additionally, u
(
n
-
1
) is the last value of PWM. e
(
n
), e
(
n
-
1
), e
(
n
-
2
) are the offsets between expected velocity and real velocity by current, by last time, and by the time before last time.T is the sample time, usually a fixed value; K
P,T
I,T
D are the parameters which will be optimized. The algorithm’s pseudocode is as in Algorithm 1.Algorithm 1:The implementation of PID algorithm.
1
Do
2
if
n=1 or 2
3  calculate the offsets of right and left wheel
e
R
(
n
)
,
e
L
(
n
)
4  calculate the current PWM value of right and left wheel
u
R
(
n
)
,
u
L
(
n
)
5  substitute n=1 or 2 into equation(6), obtain
u
(
1
)
,
u
(
2
)
,
e
(
1
)
,
e
(
2
)
6
if
n
>
=3
7  substitute
u
(
n
-
1
)
,
e
(
n
)
,
e
(
n
-
1
)
,
e
(
n
-
2
)
into equation(6)
8
While
(if work signal =
true
)
9
return
u
(
1
)
,
u
(
2
)
,
e
(
1
)
,
e
(
2
)
,
u
(
n
)Among them,u
(
1
)
,
u
(
2
)
,
e
(
1
)
,
e
(
2
) are the PID output values and offsets of the left and right wheel, respectively. Using PID algorithm, the mechanical errors can be eliminated, and the robot can ensure that it is straight as much as possible.


### 3.2. Positioning and Navigation Algorithm

#### 3.2.1. Angle and Position Correction

The two wheels of the sweeping robot are driven by two independent motors. Although using PID regulation control can ensure that the two wheels are at the same speed, the robot would shift when running a distance because there may exist slipping and mechanical error. A new method of angle and position correction based on infrared and directional ultrasonic sensors is presented in this paper as shown in Figure5.Figure 5
A diagram of the sweeping robot working Ti (i = 0, …, n).The navigator is placed in the middle of the wall on one side of the room, emitting vertical infrared ray and fan-shaped ultrasonic waves on both sides. The angle of motion of the robot is always parallel to the wall. When the robot runs from top to bottom and from right to left, and each time is in the middle of the room, the robot receives infrared ray and ultrasonic waves, determining its position by combining onboard devices. If the robot discovers deviation from the straight line, it corrects itself in time.Basic work principle: With the infrared ray by the navigator treated as the baseline, we sent an ultrasonic wave with certain angles in particular frequency which can measure the precise position of robot. Based on two times of measurement, we can resolve robot’s deviation angle. Specific steps are as follows (before the algorithm is detailed, note that we briefly define the notation that is used throughout the paper in Table1):Table 1
The model of analytic geometry parameters.
Symbol
Description
Symbol
Description
O
The position of navigator
A
The Expected position of robot
B
The real position of robot
C
The point where circle whose radius is equal to the distance of O to A cuts OB
a
→
The vector of O to A
a
→
The distance of O to A
b
→
The vector of O to B
b
→
The distance of O to B
c
→
The vector of A to B
c
→
The distance of A to B
d
→
The vector of O to C
d
→
The distance of O to C
e
→
The vector of A to C
e
→
The distance of A to CStep 1.
When the robot arrives at the baseline of infrared, after it receives an infrared ray, the distancea
→ between them can be obtained.Step 2.
After the robot leaves the baseline and runs in a short distancee
→, measure the distance between them by ultrasonic wave again.Step 3.
Resolve the horizontal attitude angleα from the relationship of geometry.(10)
α
=
arccos
⁡
α
→
2
+
e
→
2
-
d
→
2
2
e
→
a
→However, as the complexity of the formulaα is expensive and the gap between e
→ and a
→ is more than 1
0
3 orders of magnitude, we propose a geometry algorithm to make the complexity reduction substantial (see Figure 6).Figure 6
Establishing coordinate system for the motion model of robot.FollowO as the center of a circle, the direction of a
→ as x axis forward, and the direction of perpendicular a
→ as y axis forward. Let down ⊙
O trajectory is y=f(x). The slope of the tangent at C is f
′
(
c
) in ⊙
O.The slope of AC: because ofb
→ towards 0 relative to a
→, x
C-x
A=o(x) [27], o(x) is the infinitely small quantity of x.(11)
f
′
x
C
=
lim
x
C
→
x
A
⁡
f
x
C
-
f
x
A
x
C
-
x
Aso, the slope of the tangent at C being f
′
(
c
) in ⊙ O is equal to the slope of AC.In the circle, from the relationship of geometryd
→
· (1,f
′
(
c
))=0, then d
→
·
e
→=0.Similarly,a
→
·
e
→=0, from the relationship of geometry -
a
→
,
c
→=-
a
→
,
e
→+-
e
→
,
c
→.(12)
α
=
9
0
°
+
β
=
9
0
°
+
arcsin
⁡
b
→
-
a
→
c
→As the (10) and (12) are antitrigonometric functions, we can directly consider the internal formula. As we all know, multiplication is nearly 10 times slower than addition and subtraction, and division is nearly 20 times slower than addition and subtraction, when it is an 8-bit CPU. Now, we define the time of addition and subtraction as t, then the multiplication is 10
t, and division is 20
t. For (10), the time of the whole equation is as follows.(13)
10
t
+
10
t
+
10
t
+
2
t
+
20
t
+
10
t
+
10
t
=
72
tFor equation (12), the time of the whole equation is as follows.(14)
t
+
20
t
=
21
tThen, we have the following.(15)
72
t
-
21
t
72
t
∗
100
%
=
70.8
%Compared with (10), (12) reduces the computational complexity [28] and increases efficiency by nearly 70.8%. At the same time, it satisfies the actual precision requirement under the condition of A
C
^ being smaller. The relationship between relative errors and deviation angle from the original line is shown in Figure 7.Figure 7
The relationship between radius/offsets and approximate angle error.


#### 3.2.2. Navigation Implementation between Multiple Rooms

In order to help a robot to cross different rooms to work continuously, it should be improved autonomously. In this paper, we built topology model for multiple rooms based on the theory of graph optimization [29]. Meanwhile, the depth-first search (DFS) [30] stack is introduced to firstly select the most nearest and inmost room to work [31], which improves the efficiency of DFS. The algorithm’s pseudocode is as in Algorithm 2.Algorithm 2:DFS stack for cleaning work.
1
Define
every room has a unique ID
V
i
(
i
=
1
‚
…
n
)
2 Let S be a stack
3
if
robot receive an infrared ray communication signals
V
1
4  Start work in regular path
5
if
the robot receive another infrared ray communication signals
V
2
6   Then S.push(
V
2
)
7
When
finish working this room
8
While
S is not empty
9
V
i
= S.pop()
10      Enter the ID of room
V
i
11
if
V
i
is not labeled as discovered:
12       Label
V
i
as discovered
13
Goto
line 3
14      Back to initial room, then the work is over
15
Else
16  Stop workIn this algorithm, we define a room asV
i
(
i
=
1
…
n
) by the navigator which has a unique encode signal. We build a stack to store the room information so that the robot can select the most nearest and inmost room to work. When it finishes the current room, then it checks the stack status every time. Lastly, it goes back to the initial room and stops working.


## 3.1. Ensure Running in a Straight Line

Traditionally, a sweeping robot can stably execute straight line because two wheels can keep themselves running with the same velocity. Because the electronic characteristics of two wheels may not be completely consistent, in the same PWM duty rate, the velocities of two motors are not exactly the same. If there is long-term running, the error is accumulated continually, and the sweeping robot would move forward one side. Therefore, it is necessity to adjust the velocity of the two wheels in real time so that the velocity of two wheels becomes the same, and robot can keep itself linearly running for a long time without any consideration to the other obstacles.In this paper, we installed two 26-hole photoelectric tachometer encoders at the back of the motors. By measuring the number of pulsesN generated by the photoelectric encoder in unit time T and before that measuring the circumference of the wheelL, the formula is given as follows.(1)
V
=
N
/
26
∗
L
TTheoretically, after obtaining the forward speed ofV, the velocity of the wheel can be adjusted by the PID control algorithm in the field of classical automatic control. However, considering the approximate errors produced on the formula transformation, we directly added the number of pulsesN generated by the photoelectric encoder to the PID formula [26], instead of calculating the forward speed ofV each wheel. Generally, there are two kinds of PID, as shown below.(2)
u
n
=
K
P
e
n
+
T
T
I
∑
i
=
0
n
e
i
+
T
D
T
e
n
-
e
n
-
1
+
u
0Incremental PID control algorithm:(3)
∆
u
n
=
u
n
-
u
n
-
1
=
K
P
e
n
-
e
n
-
1
+
K
P
T
T
I
e
n
+
K
P
T
D
T
e
n
-
2
e
n
-
1
+
e
n
-
2
(4)
e
n
=
r
t
-
c
t
(5)
c
t
=
Nwhereu
(
n
) is the output value of PID and r
(
t
) is the desired value. N is the photoelectric encoder current measurement value, and e
(
n
) is the deviation value. In this paper, DC motor is used for robotic motion, by controlling the duty rate of PWM to control the velocity, so according to the incremental PID control algorithm, the recursion formula is(6)
u
n
=
u
n
-
1
+
∆
u
n
=
u
n
-
1
+
K
P
1
+
T
T
I
+
T
D
T
e
n
-
K
P
1
+
2
T
D
T
e
n
-
1
-
K
P
e
n
-
2
=
u
n
-
1
+
b
0
e
n
+
b
1
e
n
-
1
+
b
2
e
n
-
2
(7)
b
0
=
K
P
1
+
T
T
I
+
T
D
T
(8)
b
1
=
-
K
P
1
+
2
T
D
T
(9)
b
2
=
-
K
Pwhereu
(
n
) is currently the value of PWM, which supplies motor; additionally, u
(
n
-
1
) is the last value of PWM. e
(
n
), e
(
n
-
1
), e
(
n
-
2
) are the offsets between expected velocity and real velocity by current, by last time, and by the time before last time.T is the sample time, usually a fixed value; K
P,T
I,T
D are the parameters which will be optimized. The algorithm’s pseudocode is as in Algorithm 1.Algorithm 1:The implementation of PID algorithm.
1
Do
2
if
n=1 or 2
3  calculate the offsets of right and left wheel
e
R
(
n
)
,
e
L
(
n
)
4  calculate the current PWM value of right and left wheel
u
R
(
n
)
,
u
L
(
n
)
5  substitute n=1 or 2 into equation(6), obtain
u
(
1
)
,
u
(
2
)
,
e
(
1
)
,
e
(
2
)
6
if
n
>
=3
7  substitute
u
(
n
-
1
)
,
e
(
n
)
,
e
(
n
-
1
)
,
e
(
n
-
2
)
into equation(6)
8
While
(if work signal =
true
)
9
return
u
(
1
)
,
u
(
2
)
,
e
(
1
)
,
e
(
2
)
,
u
(
n
)Among them,u
(
1
)
,
u
(
2
)
,
e
(
1
)
,
e
(
2
) are the PID output values and offsets of the left and right wheel, respectively. Using PID algorithm, the mechanical errors can be eliminated, and the robot can ensure that it is straight as much as possible.


## 3.2. Positioning and Navigation Algorithm

### 3.2.1. Angle and Position Correction

The two wheels of the sweeping robot are driven by two independent motors. Although using PID regulation control can ensure that the two wheels are at the same speed, the robot would shift when running a distance because there may exist slipping and mechanical error. A new method of angle and position correction based on infrared and directional ultrasonic sensors is presented in this paper as shown in Figure5.Figure 5
A diagram of the sweeping robot working Ti (i = 0, …, n).The navigator is placed in the middle of the wall on one side of the room, emitting vertical infrared ray and fan-shaped ultrasonic waves on both sides. The angle of motion of the robot is always parallel to the wall. When the robot runs from top to bottom and from right to left, and each time is in the middle of the room, the robot receives infrared ray and ultrasonic waves, determining its position by combining onboard devices. If the robot discovers deviation from the straight line, it corrects itself in time.Basic work principle: With the infrared ray by the navigator treated as the baseline, we sent an ultrasonic wave with certain angles in particular frequency which can measure the precise position of robot. Based on two times of measurement, we can resolve robot’s deviation angle. Specific steps are as follows (before the algorithm is detailed, note that we briefly define the notation that is used throughout the paper in Table1):Table 1
The model of analytic geometry parameters.
Symbol
Description
Symbol
Description
O
The position of navigator
A
The Expected position of robot
B
The real position of robot
C
The point where circle whose radius is equal to the distance of O to A cuts OB
a
→
The vector of O to A
a
→
The distance of O to A
b
→
The vector of O to B
b
→
The distance of O to B
c
→
The vector of A to B
c
→
The distance of A to B
d
→
The vector of O to C
d
→
The distance of O to C
e
→
The vector of A to C
e
→
The distance of A to CStep 1.
When the robot arrives at the baseline of infrared, after it receives an infrared ray, the distancea
→ between them can be obtained.Step 2.
After the robot leaves the baseline and runs in a short distancee
→, measure the distance between them by ultrasonic wave again.Step 3.
Resolve the horizontal attitude angleα from the relationship of geometry.(10)
α
=
arccos
⁡
α
→
2
+
e
→
2
-
d
→
2
2
e
→
a
→However, as the complexity of the formulaα is expensive and the gap between e
→ and a
→ is more than 1
0
3 orders of magnitude, we propose a geometry algorithm to make the complexity reduction substantial (see Figure 6).Figure 6
Establishing coordinate system for the motion model of robot.FollowO as the center of a circle, the direction of a
→ as x axis forward, and the direction of perpendicular a
→ as y axis forward. Let down ⊙
O trajectory is y=f(x). The slope of the tangent at C is f
′
(
c
) in ⊙
O.The slope of AC: because ofb
→ towards 0 relative to a
→, x
C-x
A=o(x) [27], o(x) is the infinitely small quantity of x.(11)
f
′
x
C
=
lim
x
C
→
x
A
⁡
f
x
C
-
f
x
A
x
C
-
x
Aso, the slope of the tangent at C being f
′
(
c
) in ⊙ O is equal to the slope of AC.In the circle, from the relationship of geometryd
→
· (1,f
′
(
c
))=0, then d
→
·
e
→=0.Similarly,a
→
·
e
→=0, from the relationship of geometry -
a
→
,
c
→=-
a
→
,
e
→+-
e
→
,
c
→.(12)
α
=
9
0
°
+
β
=
9
0
°
+
arcsin
⁡
b
→
-
a
→
c
→As the (10) and (12) are antitrigonometric functions, we can directly consider the internal formula. As we all know, multiplication is nearly 10 times slower than addition and subtraction, and division is nearly 20 times slower than addition and subtraction, when it is an 8-bit CPU. Now, we define the time of addition and subtraction as t, then the multiplication is 10
t, and division is 20
t. For (10), the time of the whole equation is as follows.(13)
10
t
+
10
t
+
10
t
+
2
t
+
20
t
+
10
t
+
10
t
=
72
tFor equation (12), the time of the whole equation is as follows.(14)
t
+
20
t
=
21
tThen, we have the following.(15)
72
t
-
21
t
72
t
∗
100
%
=
70.8
%Compared with (10), (12) reduces the computational complexity [28] and increases efficiency by nearly 70.8%. At the same time, it satisfies the actual precision requirement under the condition of A
C
^ being smaller. The relationship between relative errors and deviation angle from the original line is shown in Figure 7.Figure 7
The relationship between radius/offsets and approximate angle error.


### 3.2.2. Navigation Implementation between Multiple Rooms

In order to help a robot to cross different rooms to work continuously, it should be improved autonomously. In this paper, we built topology model for multiple rooms based on the theory of graph optimization [29]. Meanwhile, the depth-first search (DFS) [30] stack is introduced to firstly select the most nearest and inmost room to work [31], which improves the efficiency of DFS. The algorithm’s pseudocode is as in Algorithm 2.Algorithm 2:DFS stack for cleaning work.
1
Define
every room has a unique ID
V
i
(
i
=
1
‚
…
n
)
2 Let S be a stack
3
if
robot receive an infrared ray communication signals
V
1
4  Start work in regular path
5
if
the robot receive another infrared ray communication signals
V
2
6   Then S.push(
V
2
)
7
When
finish working this room
8
While
S is not empty
9
V
i
= S.pop()
10      Enter the ID of room
V
i
11
if
V
i
is not labeled as discovered:
12       Label
V
i
as discovered
13
Goto
line 3
14      Back to initial room, then the work is over
15
Else
16  Stop workIn this algorithm, we define a room asV
i
(
i
=
1
…
n
) by the navigator which has a unique encode signal. We build a stack to store the room information so that the robot can select the most nearest and inmost room to work. When it finishes the current room, then it checks the stack status every time. Lastly, it goes back to the initial room and stops working.


## 3.2.1. Angle and Position Correction

The two wheels of the sweeping robot are driven by two independent motors. Although using PID regulation control can ensure that the two wheels are at the same speed, the robot would shift when running a distance because there may exist slipping and mechanical error. A new method of angle and position correction based on infrared and directional ultrasonic sensors is presented in this paper as shown in Figure5.Figure 5
A diagram of the sweeping robot working Ti (i = 0, …, n).The navigator is placed in the middle of the wall on one side of the room, emitting vertical infrared ray and fan-shaped ultrasonic waves on both sides. The angle of motion of the robot is always parallel to the wall. When the robot runs from top to bottom and from right to left, and each time is in the middle of the room, the robot receives infrared ray and ultrasonic waves, determining its position by combining onboard devices. If the robot discovers deviation from the straight line, it corrects itself in time.Basic work principle: With the infrared ray by the navigator treated as the baseline, we sent an ultrasonic wave with certain angles in particular frequency which can measure the precise position of robot. Based on two times of measurement, we can resolve robot’s deviation angle. Specific steps are as follows (before the algorithm is detailed, note that we briefly define the notation that is used throughout the paper in Table1):Table 1
The model of analytic geometry parameters.
Symbol
Description
Symbol
Description
O
The position of navigator
A
The Expected position of robot
B
The real position of robot
C
The point where circle whose radius is equal to the distance of O to A cuts OB
a
→
The vector of O to A
a
→
The distance of O to A
b
→
The vector of O to B
b
→
The distance of O to B
c
→
The vector of A to B
c
→
The distance of A to B
d
→
The vector of O to C
d
→
The distance of O to C
e
→
The vector of A to C
e
→
The distance of A to CStep 1.
When the robot arrives at the baseline of infrared, after it receives an infrared ray, the distancea
→ between them can be obtained.Step 2.
After the robot leaves the baseline and runs in a short distancee
→, measure the distance between them by ultrasonic wave again.Step 3.
Resolve the horizontal attitude angleα from the relationship of geometry.(10)
α
=
arccos
⁡
α
→
2
+
e
→
2
-
d
→
2
2
e
→
a
→However, as the complexity of the formulaα is expensive and the gap between e
→ and a
→ is more than 1
0
3 orders of magnitude, we propose a geometry algorithm to make the complexity reduction substantial (see Figure 6).Figure 6
Establishing coordinate system for the motion model of robot.FollowO as the center of a circle, the direction of a
→ as x axis forward, and the direction of perpendicular a
→ as y axis forward. Let down ⊙
O trajectory is y=f(x). The slope of the tangent at C is f
′
(
c
) in ⊙
O.The slope of AC: because ofb
→ towards 0 relative to a
→, x
C-x
A=o(x) [27], o(x) is the infinitely small quantity of x.(11)
f
′
x
C
=
lim
x
C
→
x
A
⁡
f
x
C
-
f
x
A
x
C
-
x
Aso, the slope of the tangent at C being f
′
(
c
) in ⊙ O is equal to the slope of AC.In the circle, from the relationship of geometryd
→
· (1,f
′
(
c
))=0, then d
→
·
e
→=0.Similarly,a
→
·
e
→=0, from the relationship of geometry -
a
→
,
c
→=-
a
→
,
e
→+-
e
→
,
c
→.(12)
α
=
9
0
°
+
β
=
9
0
°
+
arcsin
⁡
b
→
-
a
→
c
→As the (10) and (12) are antitrigonometric functions, we can directly consider the internal formula. As we all know, multiplication is nearly 10 times slower than addition and subtraction, and division is nearly 20 times slower than addition and subtraction, when it is an 8-bit CPU. Now, we define the time of addition and subtraction as t, then the multiplication is 10
t, and division is 20
t. For (10), the time of the whole equation is as follows.(13)
10
t
+
10
t
+
10
t
+
2
t
+
20
t
+
10
t
+
10
t
=
72
tFor equation (12), the time of the whole equation is as follows.(14)
t
+
20
t
=
21
tThen, we have the following.(15)
72
t
-
21
t
72
t
∗
100
%
=
70.8
%Compared with (10), (12) reduces the computational complexity [28] and increases efficiency by nearly 70.8%. At the same time, it satisfies the actual precision requirement under the condition of A
C
^ being smaller. The relationship between relative errors and deviation angle from the original line is shown in Figure 7.Figure 7
The relationship between radius/offsets and approximate angle error.


## 3.2.2. Navigation Implementation between Multiple Rooms

In order to help a robot to cross different rooms to work continuously, it should be improved autonomously. In this paper, we built topology model for multiple rooms based on the theory of graph optimization [29]. Meanwhile, the depth-first search (DFS) [30] stack is introduced to firstly select the most nearest and inmost room to work [31], which improves the efficiency of DFS. The algorithm’s pseudocode is as in Algorithm 2.Algorithm 2:DFS stack for cleaning work.
1
Define
every room has a unique ID
V
i
(
i
=
1
‚
…
n
)
2 Let S be a stack
3
if
robot receive an infrared ray communication signals
V
1
4  Start work in regular path
5
if
the robot receive another infrared ray communication signals
V
2
6   Then S.push(
V
2
)
7
When
finish working this room
8
While
S is not empty
9
V
i
= S.pop()
10      Enter the ID of room
V
i
11
if
V
i
is not labeled as discovered:
12       Label
V
i
as discovered
13
Goto
line 3
14      Back to initial room, then the work is over
15
Else
16  Stop workIn this algorithm, we define a room asV
i
(
i
=
1
…
n
) by the navigator which has a unique encode signal. We build a stack to store the room information so that the robot can select the most nearest and inmost room to work. When it finishes the current room, then it checks the stack status every time. Lastly, it goes back to the initial room and stops working.


## 4. Implementation and Experiments

We tested the SLMN algorithm in a10.8
∗
10m real indoor environment, and the dimension of the robot was 0.8
∗
0.4m. Since it is an upgraded TF card, the robot could easily store the data from the electronic motor encoder. The real robot was shown in Figure 8.Figure 8
The model of a real robot.After finishing its work run, the local trajectory of the robot was restored (as the errors are small, it is difficult to discover the trajectory of at a global view), as shown in Figure9. The red and blue lines represent the robot’s true trajectory and ideal trajectory, respectively. Therefore, we can find out that when the robot passed an infrared reference line (y
=
5m) by the navigator, the deviation trajectory would be effectively corrected.Figure 9
The local trajectory of the robot had been restored, a red line is the real trajectory of the robot, and a blue line is the expected trajectory of the robot.By calculating the absolute value, a good view of this deviation could be created. The deviation that appears on[
(
5
+
10.4
k
,
5
+
10.4
(
k
+
1
)
] will be effectively corrected at 5
+
10.4
(
k
+
1
) as shown in Figure 10.Figure 10
The variation trend of the deviation.Occasionally, resources such as memory and computational time are of primary concern, but most often cost and accuracy are what we want to measure. By analyzing several candidate algorithms for the navigation problem [6], we can identify a most efficient one. In this paper, we put the results of comparative analysis with other state-of-the-art algorithms such as vSLAM [11], LIDAR SLAM [17], and inertial navigation [32, 33] as shown in Table 2.Table 2
The result of performance test.
Compared Algorithms
Precise and cost
Mean relative errors in500
m
2(%)
Battery life with12
V
(
h
)
1
A
H
3
A
H
5
A
H
Proposed
6.73
1.7
4.9
7.6
vSLAM
2.32
1.4
4.1
6.3
Radar SLAM
3.17
1.3
3.9
5.9
Inertial navigation
13.6
2.1
6.1
9.8From Table2, it is seen that the two SLAM systems have similar precision, which are the most highest. However, their work time is also the shortest in the same battery capacity, so it needs more power to keep it working. Although inertial navigation system has the lowest cost, its error is too enormous to work normally. As a result, since the development of new energy still faces several uncertain difficulties [34–36], our proposed algorithm will cost less energy in satisfying practical applications. which than the other state-of-the-art algorithms.


## 5. Conclusion

On the one hand, this paper proposed a high efficiency method on indoor sweep robot line navigation. The two measured values between navigator and robot and the distance of robot motion can form a triangle then to solve the forward angle and correct the direction of forward speed with analytic geometry principle. Finally, we implemented the long time line navigation, and based on that, we propose multiroom navigation algorithm. After simulation and reality environment experiments, on the threshold of error, an indoor line navigation method based on the navigator which consists of the infrared and ultrasonic sensors, even in the evening, could resolve the problems on the other existing navigation.On the other hand, we prepare to do more works on building a map based on the high efficiency motion to improve its practicability and integrity, so it can become self-charging and work continuously.



---

*Source: 8419384-2018-09-24.xml*