A Fusion Navigation System of Security Robot Based on Millimeter Wave Radar and Inertial Sensor

1. Introduction

Security robots are intelligent agents that perform dangerous tasks on behalf of humans [1,2] in adverse environments such as smog and dust. To enable the autonomous movement of the robot, the position of the robot must be obtained and it movement be controlled in real time; therefore, navigation technology is a key technology of the robot [3,4]. Laser-based and vision-based navigation are common methods for mobile robot navigation [5,6]. For instance, the university of Luxembourg [7] utilized Lidar to construct and optimize in real-time a three layered Situational Graph that includes a robot tracking layer where the robot poses are registered, a metric-semantic layer with features such as planar walls and novel topological layer constraining the planar walls using higher-level features such as corridors and rooms. This graph can be used for robot navigation. Zou et al. [8] analyzed the characteristics and performance of different Lidar simultaneous localization and mappings and summarized their application to indoor navigation. Although cartographic navigation accuracy is high, over time, cartographers will consume more and more memory to store newly constructed maps. Another study [9] proposed a maples visual navigation system for biped humanoid robots. In this method, information is extracted from color images using deep reinforcement learning to derive motion commands. In addition, a robot navigation system using a Kinect sensor and the concept of transfer learning based on convolutional neural networks was proposed [10]. These studies indicate that laser and visual navigation technologies are widely used and have high precision.

However, in case of working environments such as smog or dust, achieving a precise laser positioning focus point is difficult due to the disturbance of air density [11]. In a dust environment, the environmental information cannot be fully collected using visual sensors, thus hindering the application of a visual navigation method [12]. To realize effective navigation of robots in smog and dust environments, a sensor with strong anti-interference ability needs to be studied. MMW, with a frequency band of 30-300 GHz, have characteristics of both infrared and microwaves, thus possessing the characteristics of strong anti-interference ability, low power consumption, strong penetration, and high Doppler frequency [13]. Therefore, application of MMW radar in smog and dust environments has navigation of robots in the smog environment [14]. The results indicated that the resolution of detection targets using MMW is better than that using optical sensors. Laser navigation cannot be used for robots in a mine environment according to the analysis in [15]; hence, in that study, the technology of simultaneous location and mapping using MMW was proposed to realize autonomous navigation of robots. The triangulation method based on frequency-modulated MMW radar was first proposed in [16]. By scanning the robot with an MMW radar, the distribution information of point clouds was obtained; further, a cut-clustering method was used to effectively reduce the ranging error and localization error caused by the volume of the robot. Based on that method, the security robot navigation system was designed, which was experimentally confirmed to realize autonomous operation of the robot in smog and dust environments, with a localization error of approximately 0.11 m.

While performing dangerous tasks, the robot is usually required to reach the target point as accurately as possible [17]. For example, the robot needs to be accurately moved to a specific position during the task of cleaning of dangerous and flammable objects [18]. One of the effective ways to improve the working efficiency of the robot is to improve its localization accuracy. Combining MMW radar and other sensors is an effective method to improve navigation accuracy [19]. Inertial and other navigation methods are commonly combined with MMW. For example, Li et al. [20] proposed an architecture based on the recurrent convolutional neural network, which combines laser and inertial data to achieve high-precision positioning of the robot. An integrated autonomous relative navigation method based on vision and inertial measurement unit (IMU) data fusion was proposed in [21]. The experimental results showed that this method has high accuracy and robustness. A navigation system based on low-cost vision and inertial fusion and using the extended Kalman filter (EKF) algorithm was proposed in [22]; experiments revealed a navigation accuracy of approximately 0.3 m. In general, inertial sensors are not affected by external environment and have strong anti-interference ability and good navigation accuracy within a short time [23]. Therefore, inertial sensors can also be used in smog and dust environments. A navigation method combining frequency-modulated continuous-wave (FMCW) radar and an inertial odometer was proposed in [24]. FMCW radar is fixed on the carrier, and its navigation error increases with the increase of the robot’s moving distance. In [25,26], a depth fusion method of MMW attitude estimation and other sensors was proposed to accurately estimate the motion trajectory of the carrier; in this method also, the MMW radar is fixed to the carrier.

Fixing the MMW radar on the carrier may increase the navigation error gradually with the moving distance. In this study, the MMW radar is placed in the carrier motion space, the position of the robot is measured by the triangular positioning method, and the positioning accuracy is improved by incorporating inertial sensor information. In this paper, based on inertial navigation theory and frequency-modulated MMW navigation theory, the state equation and the measurement equation of the robot are constructed. Then, the Kalman been researched. A comparative study was conducted on the filtering algorithm is designed, and the fusion navigation method is proposed. Autonomous navigation of the robot in smog and dust environments is achieved with high accuracy. This paper’s contributions are summarized as follows:

1) In order to obtain the approximate geometric center of the target under the sparse point cloud, based on the point cloud information of the millimeter wave radar, an approximate center method based on the sparse point cloud is proposed.

2) In order to solve the navigation problem of security robots in the smoke and dim environment, a navigation algorithm based on millimeter wave and inertial information fusion is constructed.

3) The hardware platform of the security robot based on millimeter wave and inertial navigation fusion is designed.

2. Design of Navigation System

Prior to establishing the mathematical model of the fusion navigation system, it is necessary to study the basic principles of the MMW navigation system and inertial navigation system (INS).

2.1. MMW Navigation System

The basic principle of the MMW navigation system is based on the distance between the MMW radars and the robot, obtained using triangulation to calculate and control the position of the robot.

After normalizing the signal amplitude of a millimeter wave radar, the transmitted signal and the echo signal reflected back at the distance d are:

$$\left\{\begin{array}{c}{X}_{\mathrm{trans}}\left(t\right)=\cos \left(2\pi f_{0}t+\pi \mu t^2\right)\\ X_{\mathrm{rec}}\left(t\right)=\cos \left[2\pi f_0\left(t-\Delta \tau \right)+\pi \mu {\left(t-\Delta \tau \right)}^2\right]\end{array}\right.$$

(1)

In the formula, $f_0$is the starting frequency of millimeter wave radar frequency modulated continuous wave. $\mu$ is the modulation slope.$\Delta t=2d/c$is the time delay. $c$ is the speed of light.$d$ is the distance between the measured object and the MMW radar. The complex signal $X_{mix}\left(t\right)$ obtained by mixing the received signal with the transmitted signal is:

$$\begin{array}{l}{X}_{\mathrm{mix}}\left(t\right)=\exp [{\displaystyle \frac{j\left(4\pi \mu dt\right)}{c}}+\\ j\left({\displaystyle \frac{2d}{c}}\right)\left(2\pi f_0-{\displaystyle \frac{2\pi \mu d}{c}}\right)]\end{array}$$

(2)

The instantaneous frequency $f_i$ of the signal is:

$$f_i={\displaystyle \frac{1}{2\pi }}{\displaystyle \frac{\mathrm{d}[\frac{4\pi \mu dt}{c}+\frac{2d}{c}(2\pi f_0-\frac{2\pi \mu d}{c})]}{\mathrm{d}t}}={\displaystyle \frac{2\mu d}{c}}$$

(3)

According to Formula (3), the instantaneous frequency is proportional to the distance. By sampling the echo signal and using fast Fourier transform processing, the target distance corresponding to the peak position of $f_i$ is:

$$d=f_{i}c{T}_{\mathrm{m}}/2\Delta F$$

(4)

where $\Delta F$ is the bandwidth of the modulated signal, and $T_{\mathrm{m}}$ is the modulation period.

For the $i$-th signal source received by the 1st array element of the radar M array elements, it can be obtained through Nyquist sampling:

$$\begin{array}{l}\begin{array}{cc}{X}_l\left(t\right)={\displaystyle \sum _{i=1}^N{A}_T{A}_R{S}_i\left(t-{\tau }_{li}\right)}+N_l\left(t\right)& \end{array}\\ l=1,2,\cdots ,M\begin{array}{cc} & i=1,2,\cdots ,N\end{array}\end{array}$$

(5)

Where $X_l\left(t\right)$ is the ADC sampling frequency of the $l$-th signal of ${\rm X}\left(t\right)$ .$A_T{A}_R$ is the gain of the first array element to the $i$-th signal, and $N_l\left(t\right)$ is the additive noise of the first array element. $S_i\left(t-{\tau }_{li}\right)$refers to the signal from the $i$-th signal to the lth array element under the delay of ${\tau }_{li}$. The expression of delay is:

$${\tau }_{ki}=\left(d_k\sin {\theta }_i\right)/c\begin{array}{cc} & k=1,2,\cdots ,l,M\end{array}$$

(6)

Where $d_k$ is the position of the array element, and ${\theta }_i$ is the azimuth of a signal source and the radar. According to Formulas (3) and (4), the ${\displaystyle \sum _{i=1}^N{A}_T{A}_R{S}_i\left(t-{\tau }_{ki}\right)}$ of M array elements is simplified as follows:

$${\displaystyle \sum _{i=1}^N{A}_T{A}_R{S}_i\left(t-{\tau }_{ki}\right)}\approx {\displaystyle \sum _{i=1}^N{A}_T{A}_R{S}_i\left(t\right)\exp \left(-j{\omega }_0{\tau }_{ki}\right)}$$

(7)

Where ${\omega }_0$is the frequency of the received signal. Let the $\exp \left(-j{\omega }_0{\tau }_{ki}\right)$ of Formula (5) be transformed to obtain:

$$\begin{array}{l}\alpha \left({\theta }_i\right)=\exp \left(-j{\omega }_0{\tau }_{Mi}\right)=\\ \exp \left(-j{\omega }_0{x}_k\sin {\theta }_i/c\right)\end{array}$$

(8)

Utilize Capon algorithm:

$$P\left(\theta \right)=1/\left[{\alpha }^H\left(\theta \right)R^{-1}\alpha \left(\theta \right)\right]$$

(9)

Where $R={\rm X}\left(t\right)\cdot {{\rm X}}^H\left(t\right)/L$, and L is the number of snapshots. $P\left(\theta \right)$is the set spectral range, which is from positive 90 degrees to negative 90 degrees. Search the spectral peak of one dimension of $P\left(\theta \right)$, and the abscissa corresponding to the peak is the azimuth $\theta$, so as to obtain the azimuth of the signal.

Based on the expression to measure the quality of radar received signal:

$$\mathrm{SNR}={\displaystyle \frac{\sigma P_t{G}_{\mathrm{TX}}{G}_{\mathrm{RX}}{\lambda }^2{T}_{\mathrm{meas}}}{{\left(4\pi \right)}^3{d}^4{K}_{m}TF}}$$

(10)

Where SNR is the signal-to-noise ratio. $\sigma$is the scattering cross-section. $P_t$is the radar output power. $G_{\mathrm{TX}}$and $G_{\mathrm{RX}}$are the antenna receiving and transmitting power. $\lambda$ is the signal wavelength. $T_{\mathrm{meas}}$ is the pulse modulation time. $d$ is the distance between the radar and the target. $T$ is the temperature. $F$ is the radar internal noise coefficient, and $K_{\mathrm{m}}$ is the antenna noise coefficient.

According to Formula (10), the signal-to-noise ratio of the processed signal is inversely proportional to the fourth power of the distance and is proportional to the radar cross-section. Therefore, the number of target point clouds will be unevenly distributed due to the distance between the reflector and the radar. If the mean value of these point clouds is directly processed, the geometric center distance between the radar and the target will be offset, as shown by the yellow dots in Figure 1:

For this kind of situation, this paper proposes an approximate center method under a sparse point cloud. The set of radial distance between the point cloud and radar is:

$$\mathit{d}=\left[d_1,d_2,\cdots ,d_M\right]$$

(11)

The set of angle between the point cloud and radar is:

$$\mathit{\theta }=\left[{\theta }_1,{\theta }_2,\cdots ,{\theta }_M\right]$$

(12)

The set of point cloud between radar and robot is:

$$p\left(\mathit{d},\mathit{\theta }\right)=\left[p_1\left(d_1,{\theta }_1\right),p_2\left(d_2,{\theta }_2\right),\cdots ,p_M\left(d_M,{\theta }_M\right)\right]$$

(13)

Assume that the maximum angle range of the radar is $\varpi$,and the maximum detection range is $\mathcal{l}$, and divide Figure 1:

$$\left\{\begin{array}{l}{\varpi }_{m-1}-{\varpi }_1=\left(m-1\right)\Delta \varpi \\ {\mathcal{l}}_{e-1}-{\mathcal{l}}_1=\left(e-1\right)\Delta \mathcal{l}\end{array}\right.$$

(14)

Where ${\varpi }_1$ is the starting angle scale value. ${\varpi }_{M-1}$ is the ending angle scale value. $m-1$ is the division multiple, and $\Delta \varpi$ is the angle division interval. ${\mathcal{l}}_1$ is the starting distance scale. ${\mathcal{l}}_{M-1}$ is the ending distance scale value. $\Delta \mathcal{l}$ is the distance division interval, and $e-1$ is the division multiple. Its composition range is:

$$K_{me}=\left(m-1\right)\times \left(e-1\right)$$

(15)

According to the distribution of in, it can be divided into G parts, and its expression is:

$$\left\{\begin{array}{c}G\left(M_m,D_d\right)=\left[G_1\left(d,\theta \right),G_2\left(d,\theta \right),\cdots ,G_m\left(d,\theta \right)\right]\\ G_1\left(d,\theta \right)\cup G_2\left(d,\theta \right)\cup \cdots \cup G_m\left(d,\theta \right)=p\left(d,\theta \right)\\ G\left(M_m,D_d\right)\subseteq K_{me}\left(m,e\right)\end{array}\right.$$

(16)

Where $G_1\left(d,\theta \right),G_2\left(d,\theta \right),\cdots ,G_m\left(d,\theta \right)$ represents all subsets in the $G\left(M_m,D_d\right)$ region, and the set elements of Formula 10 are distributed in the G region. After calculating the mean value of each subset of the G region, we can get:

$$d_p={\displaystyle \frac{E\left[G_1\left(d,\theta \right)\right]+E\left[G_2\left(d,\theta \right)\right]+\cdots +E\left[G_m\left(d,\theta \right)\right]}{G}}$$

(17)

According to Formula 17, the approximate geometric center distance between the radar and the target can be obtained.

Then, three radars are arranged to form the scheme of triangulation, as shown in Figure 2.

As shown in the Figure 2, three radars are placed at points A, B, and C in the navigation coordinate system, and their coordinates are $\left(x_{\mathrm{mA}},y_{\mathrm{mA}},z_{\mathrm{mA}}\right)$, $\left(x_{\mathrm{mB}},y_{\mathrm{mB}},z_{\mathrm{mB}}\right)$, $\left(x_{\mathrm{mC}},y_{\mathrm{mC}},z_{\mathrm{mC}}\right)$, respectively. The distances from points A, B, and C to the robot are $d_A$,$d_B$, and $d_C$ respectively. R $\left(x_{\mathrm{mR}},y_{\mathrm{mR}},z_{\mathrm{mR}}\right)$ based on the basic principles of triangulation can be obtained.

2.2. Strapdown Inertial Navigation System

In the strapdown INS, the gyroscope and accelerometer are fixed on the robot, with their axes consistent with the carrier body coordinate system, the x-axis is aligned with the direction of the robot’s movement; the direction of the z-axis is perpendicular to the ground, and the direction of the y-axis is determined by the right-hand rule. Using the gyroscope, the attitude matrix of the robot is obtained by measuring the angular motion information. The acceleration of the robot in the navigation coordin, ate system can be expressed as:

$$\left[\begin{array}{c}{a}_{\mathrm{nx}}\\ a_{\mathrm{ny}}\\ a_{\mathrm{nz}}\end{array}\right]=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left[\begin{array}{c}{a}_{\mathrm{bx}}\\ a_{\mathrm{by}}\\ a_{\mathrm{bz}}\end{array}\right]-\left[\begin{array}{c}0\\ 0\\ \mathrm{g}\end{array}\right]$$

(18)

Where$C_{11}=\cos \beta \cos \psi$, $C_{12}=\cos \beta \sin \psi$, $C_{13}=-\sin \beta$, $C_{21}=\sin \gamma \sin \beta \cos \psi -\cos \gamma \sin \psi$, $C_{22}=\sin \gamma \sin \beta \cos \psi +\cos \gamma \cos \psi$, $C_{23}=\sin \gamma \cos \beta$, $C_{31}=\cos \gamma \sin \beta \cos \psi +\sin \gamma \sin \psi$, $C_{32}=\cos \gamma \sin \beta \sin \psi -\sin \gamma \cos \psi$, $C_{33}=\cos \gamma \cos \beta$, g is the gravity acceleration, $\gamma$, $\beta$, and $\psi$ are the rotation angles of the robot along the x, y, and z axes, respectively. ${\left[\begin{array}{ccc}{a}_{\mathrm{bx}}& a_{\mathrm{by}}& a_{\mathrm{bz}}\end{array}\right]}^T$ is the acceleration of the robot in the carrier body coordinate system, while ${\left[\begin{array}{ccc}{a}_{\mathrm{nx}}& a_{\mathrm{ny}}& a_{\mathrm{nz}}\end{array}\right]}^T$ is the acceleration of the robot in the navigation coordinate system. To combine the two navigation systems, the inertial navigation coordinate system is consistent with the MMW navigation coordinate system.

The velocity of the robot can be obtained by integrating ${\left[\begin{array}{ccc}{a}_{\mathrm{nx}}& a_{\mathrm{ny}}& a_{\mathrm{nz}}\end{array}\right]}^T$, and the position of the robot can be obtained by integrating the velocity. Due to the noise in ${\left[\begin{array}{ccc}{a}_{\mathrm{bx}}& a_{\mathrm{by}}& a_{\mathrm{bz}}\end{array}\right]}^T$, the velocity and position data of the robot are reliable for a short time, and the errors in the data are divergent with increasing time.

3. Fusion Navigation System

An MMW navigation system can work in smog and dust environments and has the characteristics of nondivergence of positioning errors. A strapdown INS has good short-term accuracy, can be integrated with the MMW navigation system to improve the positioning accuracy of the robot, and can provide localization data for the system in a short time when the MMW signal is blocked.

To realize the fusion of the MMW and the INS, it is necessary to study the state equation and measurement equation, thereby constructing a mathematical model of the fusion system.

3.1. State Equation

In the strapdown INS, $T$ is the sampling time, $\Delta v$ is the velocity variation, which is detected using accelerometer.

indicates the difference in velocity at the (k−1)-th and k-th moments. From k-1 to k, the average value of acceleration ${\stackrel{\_}{a}}_{n\left(\mathrm{k}-1\right)}$ in navigation coordinate is:

$${\stackrel{\_}{a}}_{n\left(\mathrm{k}-1\right)}={\displaystyle \frac{\Delta v_k}{T}}$$

(19)

${\stackrel{\_}{a}}_{\mathrm{nx}\left(\mathrm{k}-1\right)}$, ${\stackrel{\_}{a}}_{\mathrm{ny}\left(\mathrm{k}-1\right)}$ and ${\stackrel{\_}{a}}_{\mathrm{nz}\left(\mathrm{k}-1\right)}$ represent the average acceleration of the robot along the $n_x$, $n_y$ and axes respectively. The position of the robot can be expressed as:

$$\left\{\begin{array}{c}{x}_{R\left(k\right)}=x_{R\left(\mathrm{k}-1\right)}+{\displaystyle \frac{1}{2}}{T}^2{\stackrel{\_}{a}}_{\mathrm{nx}\left(\mathrm{k}-1\right)}\\ y_{R\left(k\right)}=y_{R\left(\mathrm{k}-1\right)}+{\displaystyle \frac{1}{2}}{T}^2{\stackrel{\_}{a}}_{\mathrm{ny}\left(\mathrm{k}-1\right)}\\ z_{R\left(k\right)}=z_{R\left(\mathrm{k}-1\right)}+{\displaystyle \frac{1}{2}}{T}^2{\stackrel{\_}{a}}_{\mathrm{nz}\left(\mathrm{k}-1\right)}\end{array}\right.$$

(20)

Where $x_{R\left(k\right)}$, $y_{R\left(k\right)}$ and $z_{R\left(k\right)}$ represent the position of the robot along the $n_x$, $n_y$ and $n_z$ axes at time k. $x_{R\left(\mathrm{k}-1\right)}$, $y_{R\left(\mathrm{k}-1\right)}$ and $z_{R\left(\mathrm{k}-1\right)}$ represent the position of the robot along the $n_x$, $n_y$ and $n_z$ axes at k-1 time. These average accelerations are consisted of true values and noises. Let ${\stackrel{\_}{a}}_{\mathrm{nx}\left(\mathrm{k}-1\right)}$, ${\stackrel{\_}{a}}_{\mathrm{ny}\left(\mathrm{k}-1\right)}$ and ${\stackrel{\_}{a}}_{\mathrm{nz}\left(\mathrm{k}-1\right)}$ represent the true values of the average accelerations, Equation (18) can be expressed as:

$$\left[\begin{array}{c}{x}_{R\left(k\right)}\\ y_{R\left(k\right)}\\ z_{R\left(k\right)}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{R\left(\mathrm{k}-1\right)}\\ y_{R\left(\mathrm{k}-1\right)}\\ z_{R\left(\mathrm{k}-1\right)}\end{array}\right]+\left[\begin{array}{ccc}{\displaystyle \frac{T^2}{2}}& 0& 0\\ 0& {\displaystyle \frac{T^2}{2}}& 0\\ 0& 0& {\displaystyle \frac{T^2}{2}}\end{array}\right]\left[\begin{array}{c}{{\stackrel{\_}{a}}^{\text{'}}}_{\mathrm{nx}\left(\mathrm{k}-1\right)}\\ {{\stackrel{\_}{a}}^{\text{'}}}_{\mathrm{ny}\left(\mathrm{k}-1\right)}\\ {{\stackrel{\_}{a}}^{\text{'}}}_{\mathrm{nz}\left(\mathrm{k}-1\right)}\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{w}_{x\left(\mathrm{k}-1\right)}\\ w_{y\left(\mathrm{k}-1\right)}\\ w_{z\left(\mathrm{k}-1\right)}\end{array}\right]$$

(21)

Where $w_{x\left(\mathrm{k}-1\right)}$, $w_{y\left(\mathrm{k}-1\right)}$ and $w_{z\left(\mathrm{k}-1\right)}$ represent the noise of the accelerometer along the $n_x$, $n_y$ and $n_z$ axes from k-1 to k.

In the navigation system, the position coordinate of the robot is an important state quantity. $R_k={\left[\begin{array}{ccc}{x}_{R\left(k\right)}& y_{R\left(k\right)}& z_{R\left(k\right)}\end{array}\right]}^T$ is a state quantity at k, and $R_{\mathrm{k}-1}={\left[\begin{array}{ccc}{x}_{R\left(\mathrm{k}-1\right)}& y_{R\left(\mathrm{k}-1\right)}& z_{R\left(\mathrm{k}-1\right)}\end{array}\right]}^T$ is a state quantity at k−1. The discrete-time state equation is shown in the following equation:

$$R_k=\Phi R_{\mathrm{k}-1}+\Lambda {\stackrel{\_}{V}}_{\mathrm{k}-1}+\Upsilon w_{\mathrm{k}-1}$$

(22)

Where $\Phi =\mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]$ is the state transition matrix. $\Lambda =diag\left[\begin{array}{ccc}\Delta T& \Delta T& \Delta T\end{array}\right]$ is the control transition matrix. $\Upsilon =\mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]$ is the noise coefficient matrix. $w_{k-1}={\left[\begin{array}{ccc}{w}_{x\left(\mathrm{k}-1\right)}& w_{y\left(\mathrm{k}-1\right)}& w_{z\left(\mathrm{k}-1\right)}\end{array}\right]}^T$ is the state noise.

3.2. Measurement Equation

In addition, the position of the robot is obtained by the MMW navigation system. Because the navigation system consists of noise interference the position of the robot at k can be given as follows:

$$\left[\begin{array}{c}{x}_{\mathrm{mR}\left(k\right)}\\ y_{\mathrm{mR}\left(k\right)}\\ z_{\mathrm{mR}\left(k\right)}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{R\left(k\right)}\\ y_{R\left(k\right)}\\ z_{R\left(k\right)}\end{array}\right]+\left[\begin{array}{c}{\upsilon }_{x\left(k\right)}\\ {\upsilon }_{y\left(k\right)}\\ {\upsilon }_{z\left(k\right)}\end{array}\right]$$

(23)

Where ${\left[\begin{array}{ccc}{x}_{\mathrm{mR}\left(k\right)}& y_{\mathrm{mR}\left(k\right)}& z_{\mathrm{mR}\left(k\right)}\end{array}\right]}^T$ is the only observed quantity. Therefore, the measurement equation of the system is:

$$Z_k=H{R}_k+{\upsilon }_k$$

(24)

Where $H=\mathrm{diag}\left[\begin{array}{ccc}1& 1& 1\end{array}\right]$ is the measurement matrix, and ${\upsilon }_k={\left[\begin{array}{ccc}{\upsilon }_{x\left(k\right)}& {\upsilon }_{y\left(k\right)}& {\upsilon }_{z\left(k\right)}\end{array}\right]}^T$ is the measurement noise.

3.3. Noise

In Equation (20), $w_{\mathrm{k}-1}$ is related to the noise of the accelerometer. Without considering environmental vibration and large temperature changes, the noise of the accelerometer can be expressed as [27]:

$${\mathsf{\delta }\mathrm{a}=\mathsf{\Delta }}_a+C_a+{\eta }_a$$

(25)

Where ${\Delta }_a$ represents the zero drift of the accelerometer, $C_a$ represents the fixed error caused by the installation of the accelerometer and other factors, and represents the measurement noise of the accelerometer, which is Gaussian white noise. The compensation method is usually used to approximately eliminate ${\Delta }_a$ and $C_a$ In this case, the noise of the accelerometer is mainly random white noise and can be derived as follows:

$$a_n\approx {\eta }_a$$

(26)

Therefore, $w_{\mathrm{k}-1}$ in Equation (20) is an approximate white noise variable, and its mathematical expectation is as follows:

$$E\left(w_{x\left(\mathrm{k}-1\right)}\right)=E\left(w_{x\left(\mathrm{k}-1\right)}\right)=E\left(w_{x\left(\mathrm{k}-1\right)}\right)\approx 0$$

(27)

In Equation (25), $E$ represents the mathematical expectation of random variables; then the variance matrix $w_{\mathrm{k}-1}$ of $Q_{\mathrm{k}-1}$ is:

$$Q_{\mathrm{k}-1}=\left[\begin{array}{ccc}D\left(w_{x\left(\mathrm{k}-1\right)}\right)& 0& 0\\ 0& D\left(w_{y\left(\mathrm{k}-1\right)}\right)& 0\\ 0& 0& D\left(w_{z\left(\mathrm{k}-1\right)}\right)\end{array}\right]$$

(28)

where represents the variance of random variables.

In Equation (22), ${\upsilon }_k$ is the measurement error of the MMW navigation system. It has the characteristics of Gaussian white noise [16], and its mathematical expectation is as follows:

$$E\left({\upsilon }_{x\left(k\right)}\right)=E\left({\upsilon }_{y\left(k\right)}\right)=E\left({\upsilon }_{z\left(k\right)}\right)=0$$

(29)

The measurement noise covariance matrix $G_k$ of the system is as foliows:

$$G_k=\left[\begin{array}{ccc}D\left({\upsilon }_{x\left(k\right)}\right)& 0& 0\\ 0& D\left({\upsilon }_{y\left(k\right)}\right)& 0\\ 0& 0& D\left({\upsilon }_{z\left(k\right)}\right)\end{array}\right]$$

(30)

3.4. Filtering Algorithm

The measurement equation is established according to the millimeter wave navigation system, the state equation is established according to the motion state of the system, and the position information of the robot is fused using the Kalman filter algorithm. The algorithm and information interaction are shown in Figure 3:

Algorithm Integrated navigation using Kalman filter approach

1: At k, the predicted position coordinate $R_{\mathrm{k}/\mathrm{k}-1}$ of the robot can be deduced as follows: $R_{\mathrm{k}/\mathrm{k}-1}=\Phi R_{\mathrm{k}-1}+\Gamma {\stackrel{\_}{V}}_k$

2: The mean square error of the estimate is: $P_{\mathrm{k}/\mathrm{k}-1}=\Phi P_{\mathrm{k}-1}{\Phi }^T+Q$, $P_{\mathrm{k}-1}$ is the best estimation error covariance at k−1.

3: The Kalman filter gain can be obtained: $K_k=P_{\mathrm{k}/\mathrm{k}-1}{H}^T{\left(H{P}_{\mathrm{k}/\mathrm{k}-1}{H}^T+G\right)}^{-1}$

4: The optimal estimation position coordinate of the robot can be obtained as follows: $R_{\mathrm{k}/k}=R_{\mathrm{k}/\mathrm{k}-1}+K_k\left(Z_k-H_k{R}_{\mathrm{k}/\mathrm{k}-1}\right)$

5: The mean square error between the optimal estimated position coordinate and the true position coordinate is given by $P_k=\left(I-K_k{H}_k\right)P_{\mathrm{k}/\mathrm{k}-1}$

4. Experiment and Analysis

To verify the effectiveness of the proposed fusion navigation system based on MMW radar and the INS, an experimental device of the system is designed.

4.1. Experimental Equipment

Experimental setup of the proposed fusion navigation system is shown in Figure 4.

In Figure 4, the navigation coordinate system $O_n{X}_n{Y}_n{Z}_n$ is constructed on a 4 m × 4 m green carpet. The X, Y, and Z axes are orthogonal to each other at point O. Three MMV radars A, B, and C are placed on three tripods, and their position coordinates are $\left(-0.5,3,1\right)$, $\left(2,-1,1\right)$, and $\left(2.5,5.5,1\right)$ respectively. Three high-speed data differential signal lines with USB 3.0 are connected between the radars and a server, and communication between the server and the robot is achieved by a wireless module. The motion controller of the robot is an STM32 development board.

In the experimental scheme, IWR1642 radars (Texas Instruments, the United States) are used in the MMW navigation system; each radar has a working frequency band of 77–81 GHz, as shown in Figure 5.

An MTi-30 strapdown inertial sensor (Xsens, the Netherlands) is used for the INS, as shown in Figure 6.

In the design scheme, transmission and processing of information are as shown in Figure 7.

According to the figure, the robot position is output by the MMW radar and the INS. However, the output signal contains noise; therefore, the Kalman filter algorithm is used in the fusion navigation system to estimate the optimal position of the robot. Then, this information is transmitted to the motion controller. Closed-loop control is performed in the controller according to current and the target position coordinate.

4.2. Experimental Parameters

Parameters of the MMW navigation system

The elevation angle of the radar is set to 10°.The radar’s minimum signal-to-noise ratio threshold is set to 45. The radar communication baud rate is set to 921600. The range of the radar’s horizontal opening angle is −60° to 60°, and the range of the pitching angle is −19° to 19°. The noise covariance matrix of the MMW navigation system is:

$$G_k=\mathrm{diag}\left[\begin{array}{ccc}{\left(0.11\right)}^2& {\left(0.11\right)}^2& 0\end{array}\right]$$

(31)

Parameters of the INS

The standard full range of the gyroscope is 450°/s, and the bias stability in operation is 10°/h. The accelerometer is 200 m2/s, and the bias stability in operation is 15 µg. The sampling frequency of MTi-30 is 50 Hz. MTi-30 outputs the attitude angle and velocity variation. In the experiment, the zero drift of the MTi-30 accelerometer was collected by making MTi-30 static for 10 min; was then compensated. Based on the zero bias stability characteristics of gyroscopes and accelerometers, an INS error model is established to obtain the value of Q.

Parameters of the fusion navigation system

Control period of the system: The two systems are fused per 0.2 s. The initial position $R_0={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T$ The initial covariance matrix $P_0={\left[0\right]}_{3\times 3}$Parameters of robot motion control

The length, width, and height of the four-wheel driving mobile robot are 45, 42, and 48 cm, respectively. Total mass is approximately 5.8 kg, and the maximum movement speed is approximately 1.2 m/s. The four motors are the MD36N planetary gear motor. The core control chip of the robot is STM32F103RCT6. The software platform of the robot is the Free-RTOS operating system.

Parameters of robot motion control

The communication baud rate of the wireless module is 921600. The communication protocol of the wireless module is the UDP protocol.

Environment parameters

Smog is created using 20 cigarette cakes; a cigarette cake is about 6.8 cm in diameter, 1 cm in thickness, and 58 g in weight; it is mainly composed of ammonium chloride, flour, and rosin. The indoor light is insufficient, as shown in Figure 8.

4.3. Experimental Results and Analysis

In order to verify the effectiveness of the proposed algorithm, a test experiment of the approximate center method based on sparse point clouds was conducted first. In the experiment, the environment first uses a radar to measure the geometric center of the robot, and compares the two methods. The two methods are direct mean processing and approximate center method for sparse point clouds. The geometric center of the robot is measured with a 1mm accuracy scale and calculated using mathematical formulas. The experimental results are shown in Figure 9:

The blue dots in the image are the point clouds projected by the robot under the millimeter wave coordinate system after being scanned by the millimeter wave radar. The red dot is the reference geometric center point of the robot. The yellow point is the geometric center point obtained by directly processing the mean value of the blue point, and the purple point is the approximate geometric center point of the robot obtained by using the approximate center method of sparse point clouds.

According to Figure 9, it can be intuitively found that the center method based on sparse point clouds is closer to the geometric center of the robot than the direct mean method.

According to the above experimental parameters and methods, experiments were conducted on the fusion navigation of the robot based on the MMW radar and inertial navigation.

Straight motion paths

The initial coordinates of the robot were (0,1,0) and (0,0,0) for straight navigation paths 1 and 2, which satisfy the expressions $y=0.5x+1$ and $y=2x$ respectively. The target position coordinates were set to (4,3,0) and (2,4,0), respectively. The experimental results are shown in Figure 10 and Figure 11, respectively.

In Figure 10 and Figure 11, the solid line represents the theoretical trajectory of the robot. The long-dashed line represents the actual trajectory of the robot based on the MMW navigation system. The short-dashed line represents the actual trajectory of the robot based on the fusion navigation system. To further study the fusion navigation system error of the robot in the linear navigation path, additional 50 sets of experiments were conducted for each path. The navigation error is defined as the difference between the coordinates of the actual point reached by the robot and those of the target point. The error data are shown in Figure 12 and Figure 13, respectively, for navigation paths 1 and 2.

For the results shown in Figure 10 and Figure 11, the gain matrix and covariance matrix of the system are shown in Figure 14 and Figure 15, respectively.

According to Figure 14 and Figure 15, the initial system gain is 0, and diagonal value of the initial covariance matrix is 0. When the gain reaches 0.778, the diagonal value of the covariance matrix is 0.009. After eight sampling iterations, the system gain is 0.631, the diagonal value of the covariance matrix is 0.006, and the system tends to be stable.

To accurately obtain the error value of the fusion navigation system, 100 sets of experiments were conducted, and the average localization error was obtained. This error is compared with the average localization error of 100 experimental errors of the MMW navigation system, as shown in Table 1.

According to Table 1, the fusion localization error is approximately 0.083 m, which is lower than the error of the MMW navigation system.

Curved motion paths

The initial coordinates of the robot were (0.5,0,0) and (0,0,0), for the curved navigation paths 1 and 2, which satisfy the expressions $y=0.5x^2+0.9x+0.5$ and $y=0.5x^2$, respectively. The target position coordinates were set to (3.4,3.22,0) and (2.8,3.92,0), respectively. The experimental results are shown in Figure 16 and Figure 17, respectively.

In Figure 16 and Figure 17, the solid line represents a theoretical trajectory of the robot. The long-dashed line indicates the actual trajectory of the robot using the MMW navigation system. The short-dashed line indicates the actual trajectory of the robot using the fusion navigation system. To further study the fusion navigation system error of the robot in the curved navigation path, additional 50 sets of experiments were conducted for each path, and the error data are shown in Figure 18 and Figure 19, respectively.

For the experimental results shown in Figure 16 and Figure 17, the gain matrix and covariance matrix of the system are shown in Figure 20 and Figure 21, respectively.

According to Figure 20 and Figure 21, the initial value of the system gain is 0, and the diagonal value of initial covariance matrix is 0; when the gain reaches 0.778, the diagonal value of the covariance matrix is 0.009. After 8 sampling periods, the system gain is 0.631, the diagonal value of the covariance matrix is 0.006, and the system tends to be stable.

To accurately obtain the error value of the fusion navigation system, 100 sets of experiments were conducted, and average the localization error is obtained. This error is compared with the average localization error of 100 experimental errors of the MMW navigation system, as shown in Table 2.

According to Table 2, the fusion localization error is approximately 0.086 m, which is lower than the error of the MMW navigation system.

In the initial stage of the fusion navigation system, the trajectory of the robot is close to that of the MMW navigation system. With the convergence of the gain value of the fusion navigation algorithm, the trajectory of the robot becomes closer to the theoretical trajectory. When the number of iterations of the fusion algorithm is more than eight, the error decreases, resulting in more stable operation of the robot, than that with the MMW navigation system. Finally, the robot reaches the terminal position according to the set navigation path. The average error accuracy of fusion navigation system after a number of experiments is approximately 0.08 m. Therefore, the fusion navigation system based on the MMW radar and INS has smaller error and higher precision than the MMW navigation system.

5. Conclusions

To achieve autonomous operation of a robot in smog and dust environments and to improve its navigation accuracy, a fusion navigation system based on the MMW radar and INS was proposed. Three MMW radars were placed in the motion space of the robot, and the triangulation method was used to realize the navigation of the robot in the smog and dust environment. The experimental results confirmed that the navigation error was close to the error of navigation system with only the MMW radar in the initial stage. After eight iterations of the filtering algorithm, the gain matrix converged gradually and the error of the fusion navigation system decreased, resulting in a stably operating robot. The localization error of the fusion navigation system was approximately 0.08 m, which confirmed better navigation accuracy than the navigation system with only MMW. The difference between this method and satellite navigation is that MMW uses signal frequency processing to calculate the distance between the radar and the robot, which avoids the need to consider the time difference between the transmitted signal and the received signal. Further, when more MMW radar devices are arranged, the robot can operate autonomously in a larger space.