Hybrid Navigation System Based Autonomous Positioning and Path Planning for Mobile Robots

Abstract

Positioning and navigation technology is a new trend of research in mobile robot area. Existing researches focus on the indoor industrial problems, while many application fields are in the outdoor environment, which put forward higher requirements for sensor selection and navigation scheme. In this paper, a complete hybrid navigation system for a class of mobile robots with load tasks and docking tasks is presented. The work can realize large-range autonomous positioning and path planning for mobile robots in unstructured scenarios. The autonomous positioning is achieved by adopting suitable guidance methods to meet different application requirements and accuracy requirements in conditions of different distances. Based on the Bezier curve, a path planning scheme is proposed and a motion controller is designed to make the mobile robot follow the target path. The Kalman filter is established to process the guidance signals and control outputs of the motion controller. Finally, the autonomous positioning and docking experiment are carried out. The results of the research verify the effectiveness of the hybrid navigation, which can be used in autonomous warehousing logistics and multi-mobile robot system.

1 Introduction

In recent years, mobile robots have been studied a lot and widely implemented in industrial automation and smart factories [1, 2]. Mobile robots have superior capabilities over humans such as low cost, tireless work, robustness and high precision. With the development of applications of warehouse logistics and shelf handling, higher requirements have been presented for the autonomous mobility performance of mobile robots [3,4,5]. Navigation systems guide a robot to go to the desired location safely, which involves constant mobile robots’ position and attitude estimation and correction, path planning and following. The common navigation sensors types used for autonomous positioning are radio frequency identification (RFID), laser, infrared, magnetic field and so on [6,7,8,9]. From a practical point of view, it is important to use these relatively cheap navigation sensors. Many navigation schemes have been rapidly developed depending on a particular environment [3]. The widespread use of sweeping robots, automatic warehousing logistics and intelligent shelves handling is a good example. However, the operating time of RFID is limited and the process consumes a great deal of power [10]. Lasers are expensive or only one-dimensional distance information can be provided. In addition, the error of laser and infrared sensors is large when it is exposed to the direct sunlight [11]. Magnetic field guidance needs to lay magnetic stripe in advance and the mobile robot can only move along the fixed path [12, 13]. In recent times, multi-sensor fusion methods have been adopted for self-positioning.to further improve positioning accuracy. A navigation control system and strategy that incorporates multiple sensors including CCD camera, RFID sensor and sonar are proposed [14]. The inertial guidance method based on multiple sensors, which consist of the encoder, gyro, acceleration sensor, ultrasonic and infrared sensor is adopted to establish a fusion system [15]. The pose estimation of a mobile robot based on the fusion of an inertial sensor and vision is presented in Ref. [16]. However, the fusion methods can work well in the indoor structured environment, while cannot expand the application occasions. In the outdoor lighting environment, the camera has the disadvantages of illumination change and distortion just like the laser and infrared sensor.

This paper focuses on the problem of navigation of the mobile robot with load tasks or docking tasks from the initial unknown position to the docking module. The docking mode is outer envelope to adapt to the heavy load on the module. A hybrid navigation system with Kalman filter is proposed to meet different application requirements and precision requirements in different distance. For the long distance, the global positioning system (GPS) and Beidou navigation satellite system (BDS) are mostly used to estimate absolute position [17], while its positioning error is large than 5 m. Then in the medium distance, ultrasonic sensor is implemented because it is not affected by the environment and has accuracy and stability. The ultrasonic signal transmitters and receivers are installed in the docking module and the mobile robot respectively. Because of the maximum beam angle, the performance is limited in the short distance [18, 19]. Therefore, the magnetic sensor with a magnetic stripe is introduced. The disadvantage that the magnetic navigation can only move along a fixed path is solved through installing the magnetic stripe reversely under the docking mechanism. The final position is determined by a photoelectric switch.

On the basis of the hybrid navigation system, the Bezier curve is utilized to plan the suitable path of the mobile robot. Compared with other geometric elements curves for generating a path, such as polynomials, cubic splines, straight lines, $\beta$-spline, etc. [20,21,22]. The tangent vectors at the end of Bezier curve are exactly the input and output directions of the mobile robot, which is fit for the docking problem [23, 24]. The mobile robot satisfied the non-holonomic constraints of pure rolling [25]. To satisfy motion and real-time constraints, an iterative path optimization method is proposed. However, due to the data measured by the sensors exists deviation, which will lead to inaccurate position calculation and even mutation, the Kalman filter is used to generate corresponding estimates and filtering error covariances. And the navigation control system is constructed to achieve fusion control mode of autonomous positioning, path planning and motion control [28,29,30].

The organization of this paper is as follows: the hybrid navigation system is introduced in Section 2.1, and the process of the Kalman filter is discussed in Section 2.2; the path planning method based on the Bezier curve is described in Section 3.1, and the motion controller to realize path following is constructed in Section 3.2; Section 4 builds the navigation control system and the experiment results; finally, Section 5 concludes the paper.

2 Hybrid Navigation System

For the mobile robots with load tasks or docking tasks, they need to move autonomously from initial pose to the target position to achieve docking with the mechanism in the unknown complex environment. This requires the mobile robots to have autonomous positioning and navigation functions. The selection of the navigation mode and sensors are mutually restricted. Many factors need to be considered, including actual application requirements, cost, positioning accuracy, and real-time performance. With the development of electronic technology and sensor technology, there are more and more guidance methods of mobile robots, including infrared, laser, inertia, ultrasonic, vision, magnetic and so on. In order to realize the autonomous positioning in a large range, different sensors should be selected under different working conditions.

According to the distance between the mobile robot and the target position, it can be divided into three situations: long distance (R > 0.5 m), medium distance (0.2 m < R < 5 m) and short distance (R < 0.2 m). Under different distance conditions, there are different application requirements and precision requirements. Because the mobile robots are working in the outdoor environment, they will be exposed to the direct sunlight, so the camera, the laser guidance and the infrared guidance are not applicable. Therefore, this paper proposes the following hybrid navigation system to achieve autonomous positioning in Figure 1.

Figure 1

The schematic of hybrid navigation system

The docking mechanism consists of a lifting platform and a locking mechanism. The lifting platform has four separate retractable outriggers to fit for the uneven road in the outdoor environment. It is shown in Figure 2.

Figure 2

Schematic diagram of the lifting platform

The locking mechanism is mounted on the top of the mobile robot, which has four locking blocks to match the docking area in the lifting platform. The four locking blocks are driven synchronously to align the rotation centers of the mobile robot and the lifting platform. The docking state is shown in Figure 3.

Figure 3

Schematic diagram of the docking state

2.1 Autonomous Positioning

The Beidou navigation satellite system and the gyroscope are used to locate the mobile robot in a long distance. The Beidou system adopts China geodetic coordinate system and has the characteristics of all-weather and global. Based on the position coordinate data of known points in the same coordinate system space, the coordinate values of unknown points are calculated by the method of space distance intersection. Thus, it can locate and navigate moving objects in real-time and transmit relevant information quickly. The gyroscope determines the orientation of the mobile robot and is installed in the middle line of the body. Due to the large motion range and long driving distance, there is no strict limitation on the motion accuracy and trajectory except the signal stability, which can guarantee the mobile robot pass through each task point and ensure safety.

In practice, the positioning accuracy of the Beidou system is about 5 m. Therefore, the ultrasonic sensor positioning method of dual emission and dual receiver is introduced when it is in the medium distance as shown in Figure 4(a). The ultrasonic signal transmitters are fixed at the target position (points A and B). Because the beam angle of the ultrasonic signal is limited, each point has two identical transmitters to increase coverage area, which are represented by the dotted and solid lines in the figure respectively. And the four receivers are installed at the front of the mobile robot to receive ultrasonic signals (points C and D). The ultrasonic frequency is 40 kHz, and the transmission frequency is 20 Hz. When the distance between the two is within the range of 5 m, stable ultrasonic signal transmission can be carried out. At this time, the Beidou system and gyroscope sensor will no longer be worked.

Figure 4

Dual emission + dual receiver ultrasonic sensor system positioning principle diagram

To unify time reference for transmitters and receivers, a 2.4 G signal source is introduced. The signal transmitter is fixed on the middle of the target position as shown in Figure 4(b). Since the signal travels at the speed of light, much faster than the ultrasonic signal, the propagation time of the 2.4 G signal can be ignored. Therefore, the transmitting time and receiving time can be regarded as the same time, which is used as the time reference.

By measuring the transmission delay of the ultrasonic signal, namely signal arrival time t_i (i = 1, 2, 3, 4), the distances s_i (i = 1, 2, 3, 4) between each point are obtained. Then the coordinate position information of the receiving points can be solved according to Eq. (1):

$$\begin{gathered} \left\{ {\begin{array}{*{20}l} {x_{1} = \frac{{\left( {x_{b}^{2} - x_{a}^{2} } \right) - \left( {ct_{3} } \right)^{2} + \left( {ct_{1} } \right)^{2} }}{{2\left( {x_{a} - x_{b} } \right)}},} \\ {y_{1} = y_{a} + \sqrt {\left( {ct_{1} } \right)^{2} - \left( {x_{1} - x_{b} } \right)^{2} } ,} \\ \end{array} } \right. \hfill \\ \left\{ {\begin{array}{*{20}l} {x_{2} = \frac{{\left( {x_{b}^{2} - x_{a}^{2} } \right) - \left( {ct_{4} } \right)^{2} + \left( {ct_{2} } \right)^{2} }}{{2\left( {x_{a} - x_{b} } \right)}},} \\ {y_{2} = y_{a} + \sqrt {\left( {ct_{2} } \right)^{2} - \left( {x_{2} - x_{b} } \right)^{2} } ,} \\ \end{array} } \right. \hfill \\ \end{gathered}$$

(1)

and the orientation of the mobile robot can be realized by Eq. (2):

$$\theta = \frac{{\uppi }}{2} + \arctan \frac{{y_{2} - y_{1} }}{{x_{2} - x_{1} }}.$$

(2)

Due to the problems of data loss and instability in ultrasonic signal detection, the positioning error will be large. Therefore, an improved median filtering algorithm is introduced to avoid the situation as shown in Figure 5. First, a queue is set up to record data for the past n times and sort the data from small to large, where n is an odd number for easy calculation. Then the mid-value is taken as the output ultrasonic signal, but if it is equal to zero, the maximum value is the output signal.

Figure 5

Filtering process of the ultrasonic signal

When the distance is less than 0.2 m, the ultrasonic signal is interrupted because of the limit of the beam angle. A stripe expansion device is designed to connect the positioning signal between medium and short distance, as shown in Figure 6. The docking mechanism is higher than the mobile robot, the latter needs to move below the former to meet the motion constraints, and then achieve docking locking. Therefore, the precise positioning mode of magnetic navigation and photoelectric switch is adopted to reach the target position in short distance, as shown in Figure 7. Figure 8 demonstrates the docking process in short distance. When the distance reach 0.2 m in Figure 8(a), the mobile robot stops, waiting for the magnetic stripe to expand (line AF) and trigger the magnetic navigation sensor at point A.

Figure 6

Structure of the stripe expansion device

Figure 7

Docking mechanism in short distance

Figure 8

Magnetic navigation + photoelectric switch system positioning principle diagram

The principle of magnetic guidance sensor is to detect the magnetic field signal of the preset magnetic track to achieve the positioning and navigation. The path of the magnetic stripe is usually fixed. In this paper, the magnetic stripe is installed reversely under the docking mechanism so that it can move with the mechanism, extending the scope of application. When the mobile robot enters the docking module, the magnetic sensor continues working to correct the position error.

Photoelectric switch detected the object by using the occlusion of the detected object to the beam, which has high precision, fast response and strong anti-interference ability, and it is not affected by the magnetic field.

When the photoelectric switch is triggered by the raised marker in the end of the mobile robot, the mobile robot stops moving forward immediately as shown in Figure 8(b).

According to the parameters defined in Figure 8, the relative position relationship between robot and docking mechanism can be obtained by Eq. (3):

$$\begin{gathered} \left\{ {\begin{array}{*{20}l} AD = b/2 = 150,\\ CD = AD\cos \theta = b\cos \theta /2, \\ \end{array} } \right. \hfill \\ \therefore \Delta d = b\left( {1 - \cos \theta } \right)/2, \hfill \\ \end{gathered}$$

(3)

where b represents the half car width, s is the fault tolerance distance, and point O is the center point of the mobile robot. $\theta$ and $\Delta d$ indicate the orientation and position error corrections respectively. The goal of motion control is to make $\theta$ and $\Delta d$ equal to zero to realize the center points coincidence.

2.2 Kalman Filtering

Kalman filter is a recursive linear quadratic estimation. It combines measurement and prediction with statistical noise and error information to estimate the target variables optimally. Its recursive process is more suitable for computers and can guarantee real-time performance. Kalman filter has a wide range of applications, including vehicle and aircraft navigation, signal processing and robot control. In addition, there are extended Kalman filter and unscented Kalman filter in nonlinear stem.

The process of ultrasonic ranging is affected by the errors such as the size of sound velocity, time measurement and Gaussian white noise interference. In this work, the Kalman filter is introduced to improving the filtering efficiency and measurement accuracy, and to optimizing the ranging process.

It can be seen from Eq. (1) that the relationship between distance values and the coordinates is nonlinear, and the extended Kalman filter algorithm is usually used to optimize the nonlinear system. In order to simplify the filtering process and improve the calculation efficiency, the measured four distances information s_i (i = 1, 2, 3, 4) is equivalent transformed to the status information of the midpoint of the mobile robot, as shown in Figure 9. The nonlinear process is transformed into linear process and the standard Kalman filter algorithm is used to realize filtering.

Figure 9

Equivalent transformation diagram

The Kalman filter consists of two processes: prediction and update. In the prediction process, the current prediction value and the corresponding covariance are obtained based on the previous target value and state equation. Based on this, the update is combined with the measurement value to obtain a new target value and covariance to achieve recursion. The filtering process is shown in Figure 10. In addition, the prediction and measurement errors of the system are assumed to be Gaussian white noise.

Figure 10

The Kalman filtering process

The positioning system state vector is expressed as ${\varvec{X}}_{t} = \left( {x_{t} ,v_{xt} ,y_{t} ,v_{yt} } \right)$. $\hat{\user2{X}}_{t}^{ - }$ and $\hat{\user2{X}}_{t}$ are defined as the predicted value resulting from the calculation result of the last iteration and the best estimate generated from the current calculation result respectively. Since it is a recursive calculation, an initial value needs to be set. Generally, the first measurement value is used as a target value, and after that, the prediction value and the estimated covariance can be predicted:

$$\left\{ {\begin{array}{*{20}l} {\hat{\user2{X}}_{t}^{ - } = {\varvec{F}}_{t} \cdot \hat{\user2{X}}_{t - 1} + {\varvec{B}}_{t} {\varvec{u}}_{t} ,} \hfill \\ {{\varvec{P}}_{t}^{ - } = {\varvec{F}}_{t} {\varvec{P}}_{t - 1} {\varvec{F}}_{t}^{\rm T} + {\varvec{Q}},} \hfill \\ \end{array} } \right.$$

(4)

where F_t is the state transition matrix; B_t is the control matrix; u_t is system input; Q is the covariance matrix of the observed noise. The system state equation for the measurement:

$${\varvec{Z}}_{t} = {\varvec{H}} \cdot {\varvec{X}}_{t} + {\varvec{v}}_{t} ,$$

(5)

where ${\varvec{Z}}_{t} = \left[ {\hat{x}_{t} ,\hat{y}_{t} } \right]^{\rm T}$ is the measured value; v_t is measure noise and satisfies the $N\left( {0,{\varvec{R}}} \right)$ distribution; H is the measure matrix.

Then the Kalman gain and the update equation are obtained from Eq. (6).

Combined with the ultrasonic signal, using the acceleration ${\varvec{u}} = \left[ {\begin{array}{*{20}c} {a_{x} } & {a_{y} } \\ \end{array} } \right]^{\rm T}$ as the system input to make predictions and the position and speed change $\left[ {\Delta x_{t} ,\Delta v_{xt} ,\Delta y_{t} ,\Delta v_{yt} } \right]^{\rm T}$ as the observed value, the state equation can be expressed in Eq. (7).

$$\left\{ {\begin{array}{*{20}l} {{\varvec{K}}_{t} = {\varvec{P}}_{t}^{ - } {\varvec{H}}^{\rm T} \left( {{\varvec{HP}}_{t}^{ - } {\varvec{H}}^{\rm T} + {\varvec{R}}} \right)^{ - 1} ,} \hfill \\ {\hat{\user2{X}}_{t} = \hat{\user2{X}}_{t}^{ - } + {\varvec{K}}_{t} \left( {{\varvec{Z}}_{t} - \user2{H\hat{X}}_{t}^{ - } } \right),} \hfill \\ {{\varvec{P}}_{t} = {\text{cov}} \left( {{\varvec{X}}_{t} - \hat{\user2{X}}_{t} } \right) = \left( {{\varvec{I}} - {\varvec{K}}_{t} {\varvec{H}}} \right){\varvec{P}}_{t}^{ - } .} \hfill \\ \end{array} } \right.$$

(6)

Simulation analysis is performed to verify the effectiveness of the Kalman filter. Set motion parameters $v_{x} = 0.5{\text{ m/s}}, \, v_{y} = 0.3{\text{ m/s}}, \, a_{x} = 0.02{\text{ m/s}}^{2} , \, a_{y} = 0.04{\text{ m/s}}^{2}$ from the origin point at time zero and the initial input of the mobile robot ${\varvec{X}}_{0} = \left[ {0.5,0.5,0.8,0.3} \right]^{\rm T}$. Then the initial value of the covariance matrix is determined.

$$\hat{\user2{X}}_{t}^{ - } = \left[ {\begin{array}{*{20}c} 1 & {\Delta t} & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & {\Delta t} \\ 0 & 0 & 0 & 1 \\ \end{array} } \right]_{t} \cdot \hat{\user2{X}}_{t - 1} + \left[ {\begin{array}{*{20}c} {\frac{1}{2}\Delta t^{2} } & 0 \\ {\Delta t} & 0 \\ 0 & {\frac{1}{2}\Delta t^{2} } \\ 0 & {\Delta t} \\ \end{array} } \right]{\varvec{u}}_{t} .$$

(7)

The actual value and filtered value of position and velocity in x, y directions are shown in Figure 11. It can be seen from the figure that the tracking effect is good with large initial error, quickly approaching the real state.

Figure 11

Simulation results of the Kalman filter

3 Path Planning

In Section 2.1, the real-time relative position between the mobile robot and the target position is obtained by the hybrid navigation system. Furthermore, a path planning scheme based on the Bezier curve is proposed to guide the mobile robot and a motion controller is designed to follow the path.

3.1 Bezier Curve Based Path Planning

Bezier curve is a space curve, as shown in Figure 12. Unlike other types of curves such as polynomials or cubic splines, the Bezier curve does not pass through all the data points which define it. The points that define a Bezier curve are called control points. A polygon that can be drawn from these control points is known as Bezier polygons. The Bezier curve is contained within the convex hull that defines the polygon. The turning points are the points at which the slope of the curve changes. Bezier curves have fewer turning points, so it is smoother than a cubic spline. The first and last points on the curve are coincident with the first and last control points. The tangent vector at the end points of the Bezier curve are directly along the first and last span of the polygon. Because the Bezier curve has continuous higher-order derivatives, its radius of curvature changes smoothly from the beginning to the end. The parameter Bezier curve is defined as

$$\begin{aligned} P\left( u \right) =& \left[ {\begin{array}{*{20}c} {x\left( u \right)} \\ {y\left( u \right)} \\ \end{array} } \right] = \left( {1 - u} \right)^{3} \left[ {\begin{array}{*{20}c} {x_{0} } \\ {y_{0} } \\ \end{array} } \right] + 3\left( {1 - u} \right)^{2} u\left[ {\begin{array}{*{20}c} {x_{1} } \\ {y_{1} } \\ \end{array} } \right] \hfill \\ &+ 3\left( {1 - u} \right)u^{2} \left[ {\begin{array}{*{20}c} {x_{2} } \\ {y_{2} } \\ \end{array} } \right] + u^{3} \left[ {\begin{array}{*{20}c} {x_{3} } \\ {y_{3} } \\ \end{array} } \right], \hfill \\ \end{aligned}$$

(8)

Figure 12

Example of the Bezier curve

where u is a parameter and u $\in$[0, 1]. The curvature k(u) at any point on the Bezier curve is expressed in terms of the first and second derivatives with respect to the parameter u as

$$k\left( u \right) = \frac{x^{\prime}\left( u \right)y^{\prime\prime}\left( u \right) - y^{\prime}\left( u \right)x^{\prime\prime}\left( u \right)}{{\left( {x^{{\prime}{2}} \left( u \right) + y^{{\prime}{2}} \left( u \right)} \right)^{\frac{3}{2}} }}.$$

(9)

The estimated position of the mobile robot is used as the first control point, and the position of the target point is used as the fourth control point. The second control point Q is related to the estimated angular direction of the mobile robot and the length of the first side of the defined Bezier polygon d₁. The third control point R can be calculated by the velocity direction when the mobile robot approaches the target point and the length of the third side of the Bezier polygon d₂. The coordinates of the control points Q and R are calculated as

$$Q = \left[ {\begin{array}{*{20}c} {x_{1} } \\ {y_{1} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {x_{0} + d_{1} \cos \theta_{0} } \\ {y_{0} + d_{1} \sin \theta_{0} } \\ \end{array} } \right],$$

(10)

$$R = \left[ {\begin{array}{*{20}c} {x_{2} } \\ {y_{2} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {x_{3} - d_{2} \cos \theta_{3} } \\ {y_{3} - d_{2} \sin \theta_{3} } \\ \end{array} } \right].$$

(11)

Therefore, control points P, Q, R and S can be used to define a smooth path curve, as shown in Figure 12. Note that the Bezier curve does not pass through the intermediate points Q and R, while it passes through the end points P and S at the same time. The property is quite suitable for path planning of the mobile robot with docking task. Like other curve fitting techniques, the accuracy of Bezier paths depends on the accuracy of the pose estimation algorithm.

For a calculated position P and the target point S, lots of Bezier paths can be easily obtained by changing the parameters d₁ and d₂. The length and the radius of the curvature are also various directly with the length of the polygon sides. By fixing d₁ and d₂ and traversing the other respectively, the influence trend on the Bezier curve was explored. Four curves are selected for each parameter and the results are demonstrated in Figures 13 and 14.

Figure 13

Bezier paths change with d₁

Figure 14

Bezier paths change with d₂

The constraint conditions of the planned path include four parts. First is the bounded curvature constraint. The velocity along the path is limited by the radial and tangential acceleration limits of the mobile robot. These acceleration components are calculated as

$$a_{t} = {{{\text{d}}v\left( u \right)} \mathord{\left/ {\vphantom {{{\text{d}}v\left( u \right)} {{\text{d}}t}}} \right. \kern-\nulldelimiterspace} {{\text{d}}t}},a_{r} = v^{2} \left( u \right)/r.$$

(12)

The curvature of the path has an upper bound and it is determined by

$$k_{\max } = 1/r_{\min } = a_{r\max } /v^{2} \left( u \right),$$

(13)

where r is the radius of curvature, and a_t is the tangential acceleration. The second is curvature variation constraint.

Since the mobile robot satisfies the non-holonomic constraints, the smaller the curvature changes, the smaller the cumulative error. Combined with kinematic characteristics and robot size, the curvature should be less than 0.8 according to Eq. (13). Thirdly, the end of the path should keep smooth with small curvature for a better realization of docking, which is positively related to d₂ as shown in Figures 13(a) and 14(a). Finally, the path should be as short as possible under the curvature constraint to reduce the movement time. It is noticed that the length of the Bezier path increases with the increase of d₁ and d₂. The displacement ds is obtained as

$${\text{d}}s\left( u \right) = \sqrt {\left( {\frac{{{\text{d}}x\left( u \right)}}{{{\text{d}}u}}} \right)^{2} + \left( {\frac{{{\text{d}}y\left( u \right)}}{{{\text{d}}u}}} \right)^{2} } {\text{d}}u.$$

(14)

The moving time of the mobile robot from point P to point S can be calculated by integrating dt, which can be expressed as

$$t = \int_{0}^{1} {{\text{d}}t = } \int_{0}^{1} {\frac{{{\text{d}}s\left( u \right)}}{v\left( u \right)}} .$$

(15)

In the medium distance and the short distance, path planning is implemented through iteration to meet real-time requirements and combined with the Kalman filter to update the path. For the calculated coordinate P (x₀, y₀) and target point S (x₃, y₃), the initial input of the iterative algorithm is:

$$d_{1} = \left| {y_{3} - y_{0} } \right|/5,d_{2} = 4d_{1} .$$

(16)

On the basis of Eq. (16), the correction factor is expressed as

$$d_{1} = d_{1} \left( {1 + \sqrt {\left| {k_{0} } \right|} } \right),d_{2} = d_{2} \pm \sqrt {\max (\left| k \right|)} ,$$

(17)

where k₀ represents the curvature in the end point P, and k_max is the maximum curvature of the Bezier curve. The optimized path can be obtained after two iterations.

3.2 Motion Controller Design

The mobile robots are simplified into the two-wheeled model as shown in Figure 15, satisfying the non-holonomic constraints of pure rolling [25]. The velocity of the center point C is denoted by v. The distance between two driven wheels is 2b. The generalized coordinate ($x,y,\theta$) is used to describe the kinematic model, where x, y represents the coordinate of the center point of the tracked vehicle and the parameter $\theta$ represents the angle between the direction of the chassis and the x-axis. Then the two-wheeled kinematic model can be defined as

$$\left\{ {\begin{array}{*{20}l} {\dot{x} = v\cos \theta ,} \\ {\dot{y} = v\sin \theta ,} \\ {\dot{\theta } = \omega ,} \\ \end{array} } \right.$$

(18)

Figure 15

The diagram of path planning

where $v$ and $\omega$ represent the linear velocity and angular velocity respectively, and they are the control inputs of the kinematic model.

A feedback motion controller is designed to follow the Bezier path through the backstepping method [26]. A virtual reference mobile robot from the initial position is set up, which can be described by $\left( {x_{r} ,y_{r} ,\theta_{r} } \right)$, and satisfies the Eq. (18). The reference robot moves along the Bezier curve as the reference path and it can be expressed by

$${\varvec{p}}_{r} = \left( {x_{r} \left( u \right),y_{r} \left( u \right),\arctan \frac{{\dot{y}_{r} \left( u \right)}}{{\dot{x}_{r} \left( u \right)}}} \right).$$

(19)

Then the reference velocity is as Eq. (20):

$$\left\{ \begin{aligned} &\dot{x}_{r} \left( u \right) \cdot \dot{u} = v_{r} \cos \theta_{r} , \\& \dot{y}_{r} \left( u \right) \cdot \dot{u} = v_{r} \sin \theta_{r} , \\ &\frac{{\ddot{y}_{r} \left( u \right)\dot{x}_{r} \left( u \right) - \dot{y}_{r} \left( u \right)\ddot{x}_{r} \left( u \right)}}{{\dot{y}_{r}^{2} \left( u \right) + \dot{x}_{r}^{2} \left( u \right)}} \cdot \dot{u} = \omega_{r} . \\ \end{aligned} \right.$$

(20)

The tracking error of the mobile robot is demonstrated in Figure 15.

And its equation is represented as

$$\left[ {\begin{array}{*{20}c} {x_{e} } \\ {y_{e} } \\ {\theta_{e} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {\cos \theta } & {\sin \theta } & 0 \\ { - \sin \theta } & {\cos \theta } & 0 \\ 0 & 0 & 1 \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {x_{r} - x} \\ {y_{r} - y} \\ {\theta_{r} - \theta } \\ \end{array} } \right].$$

(21)

In summary, the motion controller based on the two-wheeled differential-drive model is to solve $v$ and $\omega$ as the control inputs submitting to Eq. (22).

$$\mathop {\lim }\limits_{t \to \infty } \left\| {\left[ {x_{e} ,y_{e} ,\theta_{e} } \right]^{\rm T} } \right\| = 0.$$

(22)

Using the construction method of the Lyapunov function proposed in Ref. [27] and given input velocity $v > 0$, the motion controller can be written as

$$\left\{ \begin{array}{l} \dot{u}={( v+{{k}_{1}}{{x}_{e}}\cos \theta )}/{\left( \sqrt{{{{\dot{x}}}^{2}}_{r}\left( u \right)+{{{\dot{y}}}^{2}}_{r}\left( u \right)}\cos \theta \right),} \\ \omega = {\left( {{\omega }_{r}}-\frac{\partial \beta }{\partial {{y}_{e}}}\cdot {{v}_{r}}\text{sin}{{\theta }_{e}}-\frac{\partial \beta }{\partial {{v}_{r}}}\cdot {\dot{{v}}_{r}} \right)}/{\left( 1-{{x}_{e}}\frac{\partial \beta }{\partial {{y}_{e}}} \right)}+{{k}_{2}}\sin \frac{{{{\bar{\theta }}}_{e}}}{2}, \end{array} \right.$$

(23)

where $k_{1} ,k_{2}$ are positive constant gains. For the desired path by the given parameters, angular velocity $\omega$ and path parameters u are updated according to Eq. (23). Combined with the velocity of two wheels, where $v_{1}$ represents the left and $v_{2}$ is the right. And the control outputs are

$$\left\{ {\begin{array}{*{20}c} {v_{1} = v + b\omega ,} \\ {v_{2} = v - b\omega .} \\ \end{array} } \right.$$

(24)

4 Experiments

Combined with the schematic diagram in Figure 1 and the proposed methods, the complete hybrid navigation control system is constructed in Figure 16. The motion controller calculates the position error through the optimal estimation of the current position and the reference path. The control outputs can be substituted into Eq. (7) of the Kalman filter to get the prediction. The initial position is the starting point in medium distance navigation, namely the point P in the Bezier curve. In short distance, the control process is similar to that in medium distance, including path planning, state feedback and motion controller, which is not repeated in the flow chart.

Figure 16

Flow chart of the proposed navigation control system

After the navigation process and the control algorithm are obtained, the autonomous docking experiment is carried out to verify its effectiveness. The mobile robot starts from a random position away from the docking module. And it moves under the guidance of the BDS until the ultrasonic signal is stably detected. Then the navigation mode is switched to the ultrasonic sensors. The current position is defined as the initial position, and the generalized coordinate of the middle point of the receiver ultrasonic sensors is about (− 0.92 m, − 2.93 m, 94°) according to Eqs. (1) and (2). The docking module is placed at the original point, and the target point in medium distance is (0 m, − 0.25 m). The reference path is planned and optimized based on the initial and target points. The control input velocity of the mobile robot is set as 0.125 m/s.

The measured curves in medium distance are shown in Figures 17, 18, 19, 20, 21. Figure 17 presents the four initial distances between two receivers and two emissions of the ultrasonic sensors. There are some fluctuations along the distances. Then the coordinates of two ultrasonic receivers (points C and D in Figure 2) are calculated and displayed in Figures 18 and 19, where (x_m, y_m) means the middle point. When the time is less than 5 s, because the mobile robot is far from the docking module and the angle deviation is large, the instability of the signal measurement leads to the measurement error. After 20 s, the distance between two points has divergence trend.

Figure 17

Four transmission distances of ultrasonic signals

Figure 18

X-direction coordinates

Figure 19

Y-direction coordinates

Figure 20

Result of the hybrid navigation control

Figure 21

Velocity of two wheels in medium distance

Figure 16 shows the reference path and the actual path guided by the hybrid navigation control system. The mobile robot corrects its orientation and moves towards the reference path, even though there are large initial orientation deviation and continuous measured errors. And finally, the mobile robot achieves asymptotically stable path following, and the position error is less than 1 cm. Figure 17 presents the velocity of two wheels. With the decrease of the following error, the speed of the two wheels tends to be flat and equal.

When the mobile robot arrives the target point, the ultrasonic signal is too close to be detected. The mobile robot stops until the magnetic stripe is fully expanded. At the same time, the navigation method is changed to magnetic field. The magnetic sensor has 16 signal output points and each point can detect magnetic field intensity. Figure 22 shows the point of the maximum magnetic signal, which represents the motion deviation of the mobile robot. The target is to control the maximum signal at point 9. The control input velocity of the mobile robot is set as 0.05 m/s. And the velocity of two wheels in short distance is illustrated in Figure 23. When the magnetic field detection error or the position deviation occurs, there will be some motion correction. However, because of the high precision of motion in the ultrasonic stage, the real path is approximately a straight line in the experiment.

Figure 22

Point of maximum magnetic field signal

Figure 23

Velocity of two wheels in short distance

Since the Beidou navigation system is stable and reliable in long-distance condition, this section mainly shows navigation effects in medium distance and short distance. The process of autonomous positioning and docking is demonstrated in Figure 24. The turning process of the robot is shown in Figure 24(a), then moves guided by ultrasound sensors in Figure 24(b), and the magnetic stripe has expended as the guidance in Figure 24(c). The mobile robot triggers the photoelectric switch, arrives at the terminal and completes the connection with the docking module in Figure 24(d).

Figure 24

The process of navigation and docking experiment

5 Conclusions

The autonomous navigation is one of the fundamental problems in robotics. This paper focuses on the requirements of the mobile robot with loading and docking tasks in the outdoor environment. The hybrid navigation system is proposed to solve the problem. In order to achieve the accuracy of the autonomous positioning of the mobile robot in an outdoor environment, the navigation system adapts different guide sensors in different distances. The position and orientation of the robot can be calculated by combination and using the guide sensors. The path is planned based on the Bezier curve and optimized by an iterative algorithm to satisfy motion and real-time constraints. And a motion controller is designed by the backstepping method to follow the target path stably. The Kalman filter is introduced to reduce the positioning error and constructed the hybrid navigation control system. The effectiveness of the autonomous navigation system of the mobile robot is verified by the navigation and docking experiment. In order to further improve the performance of the proposed navigation system, future work will concentrate on enhancing the system robustness and avoiding obstacle in unstructured scenarios.