In global planning, the map information is provided by Google map which is already known, while in local planning, the environment and traffic cannot be known in advance. As such, the AR needs to pre-build a map of the current environment by the captured image (as shown in Figure 5).

Recently, lots of efficiency path planning algorithms on local dynamic environment has been established, such as Ant Colony algorithm, Particle Swarm algorithm and Bacterial Foraging Optimization. Each of these methods has its own pros and cons. Among these, Bacterial Foraging Optimization is relatively efficient and has the most applicable scope. Also, BFO reduces the cost of computing power by avoiding most of the computation complexity, which makes it fit in small sized robots. The BFO working flowchart is shown as Figure 6.

The initial location of AR, the destination and the position of obstacles will be input as parameters. The final output will be a shortest pathway without crashing with any obstacles.

Step 1: ( S = 1 , 2 , ⋯ , n ) virtual particles are generated according to the detected positions of obstacles with radius R around the AR’s position. Each particle position would be defined as P s ( t ) at any given time t, the position after a small period of time is calculated as P s ( t + d t ) = P s ( t ) + R ( Δ ( t ) / ‖ Δ ( t ) ‖ )

J obstacle = H obstacle ∗ exp ( − W obstacle ‖ θ i ( t ) − P 0 ( t ) ‖ 2 ) (1)

If ‖ P 0 ( t ) − C ( t ) ‖ 2 ≤ β

J obstacle = 0 (2)

where Δ t and ‖ Δ t ‖ is defined as a unit length of a random vector used to defined the moving direction of the particle and magnitude of it, respectively.

Step 2: During this step, our AR has met with the moving obstacles, a repellant Gaussian cost function is assigned to each position of detected obstacle to find out the best particle: where Hobstacle and Wobstacle are the constants defining height and weight of the repellant, P 0 ( t ) is the position of the obstacle detected by sensors, β is the sensor range.

J goal = − H goal ∗ exp ( − W goal ( ‖ θ i ( t ) − P G ( t ) ‖ 2 ) ) (3)

J = J obstacle + J goal (4)

Variables are defined in the same manner as (3). The total cost function is calculated by (4).

Step 3 & 4.

The best particle is decided by the distance error and cos function error to the target, which is calculated by (5) and the cost function error is calculated by (6). where d s ( t ) = ‖ P s ( t ) − P G ( t ) ‖ 2 .

e s d ( t ) = d s ( t + d t ) − d s ( t ) (5)

e s J ( t ) = J ( P s ( t + d t ) ) − J ( P s ( t ) ) (6)

Step 5 & Step 6:

At step 5, all particles are sort in ascending order of cost error, such that the particle with the lowest distance error will be in the top, which is the best particle we try to find.

Step 7:

After the best particle has been found, add the particle to the desired path and replaced the current position of AR with the new position. Continue repeating this process till the convergence condition is satisfied. (Simulation result is shown as Figure 7).

The template is designed so that author affiliations are not repeated each time for multiple authors of the same affiliation. Please keep your affiliations as succinct as possible (for example, do NOT post your job titles, positions, academic degrees, zip codes, names of building/street/district/province/state, etc.). This template was designed for two affiliations.

1) For author/s of only one affiliation: To change the default, adjust the template as follows.

• Selection: Highlight all author and affiliation lines.

• Change number of columns: Select the Columns icon from the MS Word Standard toolbar and then select “1 Column” from the selection palette.

• Deletion: Delete the author and affiliation lines for the second affiliation.

2) For author/s of more than two affiliations: To change the default, adjust the template as follows.

• Selection: Highlight all author and affiliation lines.

• Change number of columns: Select the “Columns” icon from the MS Word Standard toolbar and then select “1 Column” from the selection palette.

• Highlight author and affiliation lines of affiliation 1 and copy this selection.

• Formatting: Insert one hard return immediately after the last character of the last affiliation line. Then paste down the copy of affiliation 1. Repeat as necessary for each additional affiliation.

LiDAR is essential for the realization of automatic obstacle avoidance, which is the core mission of our AR. The algorithms we use focus on solving sensor error problems and improving algorithm accuracy. The scanning areas of the laser are divided into emergency, accurate and fuzzy. If the MR detects an obstacle in these layers, the corresponding algorithm can be used to perform the obstacle avoidance behavior.

The AR is modeled as a kinematic unicycle and has three degrees of freedom, x r , y r , and θ r , which moves on the xy-plane and rotates about z-axis with an angular velocity ω in Figure 8. The kinematics of the wheeled robot is depicted by (7).

[ x ˙ r y ˙ r θ ˙ ] = [ cos ( θ r ) 0 sin ( θ r ) 0 0 1 ] [ v ω ] (7)

As shown in Figure 8, ω L and ω R are used to represent the angular velocities of the left and the right wheels, respectively. The linear and angular velocity of the robot is given by (8).

[ v ω ] = [ r 2 r 2 r b − r b ] [ ω L ω R ] (8)

where the results of the joint are effected by left and right wheels. Where r is the constant radius of the wheels, and b denotes the constant distance between the two wheels.

We used Livox Mid-40/Mid-100 for obstacles detection. The scanning angle resolution is 0.5˚ and the scanning area is 190˚. The number of its direction vector is 361. In Figure 9, dk is set to the return value of LIDAR in each vector direction, indicating the distance between the obstacle point and MR in any vector

direction. Due to environmental effect and MR actions, there is an error in scanning data. Therefore, it is necessary to filter the raw data to first eliminate all unreliable data and isolated data from the scan results. We use the median filtering method to remove noise and improve the efficiency of the algorithm. The basic principle of the median filter is to replace the point with a median near the point:

1) Median Filtering

Consider the input vector X = { X − N , ⋯ , X − 1 , ⋯ , X N } and the non-negative integer weight, W0. Then, the output Y of the center weighted median filter is given as

Y = { X − N , ⋯ , X − 1 , W 0 ◇ X 0 , ⋯ , X N } (9)

When the weight in (9) is extended to the positive real value, the output is calculated as follows. First, the elements of the input vector X is sorted in ascending order. In the sorted sequence, let the i^th largest element be X ( i ) .

Then, the output is equal to:

X ( 2 N + W θ 2 + 1 ) (10)

where X ( 2 N + W θ 2 + 1 ) means the maximum integer which does not exceed.

2) Mean Shift Clustering Algorithm

f ^ h , K ( x ) = c k , d n h d ∑ i = 1 n k ( ‖ x − x i ‖ 2 h ) (11)

where k ( x ) is the profile of kernel K so that K ( X ) = c k , d k ( ‖ x ‖ 2 ) ; where c k , d is a normalization constant. When the derivative of k ( x ) exits, g ( x ) = − k 0 ( x ) can be used as a profile to define a new kernel G ( x ) = c g , d g ( ‖ x ‖ 2 ) with normalization constant c g , d .

Take the gradient of (11), the following equation can be obtained,

m h , G ( x ) = C ∇ ^ f h , k ( x ) f ^ h , G ( x ) (12)

Since C = 1 / 2 h 2 c , then

m h , G ( x ) = ∑ i = 1 n x i g ( ‖ x − x i h ‖ 2 ) ∑ i = 1 n g ( ‖ x − x i h ‖ 2 ) − x (13)

Therefore, the mean shift vector computed with kernel G at location x is

x ( k + 1 ) = x k + m h , G ( x k ) , k = 1 , 2 , ⋯ (14)

The measurement range of LIDAR is divided into (as shown in Figure 10(a)) emergency obstacle-avoidance layer and accurate obstacle-avoidance layer.

As shown in Figure 10(b), d k is set to return value of LIDAR in every vector direction, referring to the distance between an obstacle point and the AR in any vector direction. d^max represents the maximum effective distance in the scanning area. Δ d ≈ | d k + 1 − d k | is the distance between two adjacent obstacles.

When the MR is in the emergency obstacle avoidance layer, it needs to be quickly and simultaneously reflected to avoid obstacles as soon as possible. Therefore, the emergency obstacle avoidance algorithm should reduce the amount of calculation, increase the responding speed and efficiency, etc. If obstacles are detected within the emergency obstacle avoidance layer, the AR will move in the opposite direction of the obstacle until the unknown obstacle outside the emergency obstacle avoidance area. Accurate obstacle avoidance algorithms will be implemented quickly.

Set the two endpoints of expanded obstacles as A ( x a , y a ) and B ( x b , y b ) . Applying the vector gradient algorithm, we can decide whether the line OG intersects with the line AB at first (as shown in Figure 11).

Line AB: A ′ x + B ′ y + C ′ = 0 (15)

Line OG: A ″ x + B ″ y + C ″ = 0 (16)

1) The distance between point A and line OG is d a = ( A ′ x a + B ′ y a + C ′ ) / A ′ 2 + B ′ 2 .

2) The distance between point B and line OG is d b = ( A ″ x b + B ″ y b + C ″ ) / A ″ 2 + B ″ 2 , AB intersects OG.

If d a ⋅ d b ≤ 0 , AB intersects OG.

If d a ⋅ d b > 0 , AB doesn’t intersect with OG.

If there is no obstacle intersected with expected trajectory, the output point is the current position of the AR, and AR can move along the trajectory as planned;

If there are obstacles intersected with expected trajectory, the distance between the feature points of the current obstacle and the goal can be calculated. The point of shorter distance can be selected as the next visible point of MR. MR can repeat the above procedure until reaching the target.