Intelligent device navigation method and navigation system

Abstract

The present disclosure discloses an intelligent device navigation method and navigation system. The method comprises the following. Construct a plurality of antennas on a network card in the intelligent device into a linear antenna array. By using the linear antenna array, acquire channel state information of a wireless signal, and estimate an angle of arrival (AoA) and a time of flight (ToF) between the wireless signal transmitting device and the intelligent device. Measure inertial parameters of the intelligent device. Perform data fusion of the AoAs, the ToFs and the inertial parameters to estimate a state variable of the intelligent device. Adjust a motion state of the intelligent device with reference to the state variable, thereby achieving autonomous navigation of the intelligent device. The disclosure can estimate the state of the intelligent device by using wireless signals ubiquitous in the surrounding environment in a GPS unreliable environment.

Claims

1. An intelligent device navigation method for achieving autonomous navigation of an intelligent device, comprising: (1) constructing a plurality of antennas on a network card in the intelligent device into a linear antenna array, wherein each of the antenna is an array element; (2) by using the linear antenna array, acquiring channel state information (CSI) of a wireless signal transmitted by a wireless signal transmitting device in an environment, and estimating an angle of arrival (AoA) and a time of flight (ToF) between the wireless signal transmitting device and the intelligent device; measuring inertial parameters of the intelligent device by using an inertial measurement unit; (3) performing data fusion of the AoAs, the ToFs and the inertial parameters by a sliding window estimator to estimate a state variable of the intelligent device; (4) adjusting a motion state of the intelligent device with reference to the state variable, thereby achieving autonomous navigation of the intelligent device, wherein the wireless signal is an orthogonal frequency-division multiplexing (OFDM) signal, and the sliding window estimator is configured to save the latest state variables of the intelligent device and position information of the current observable wireless signal transmitting device(s).

2. The intelligent device navigation method of claim 1, wherein in the step (2), a method of estimating the AoA and the ToF between the wireless signal transmitting device and the intelligent device comprises: (21) superimposing the CSIs of N OFDM signal subcarriers received by the linear antenna array to obtain a measurement signal matrix X′, and performing CSI smoothing on the measurement signal matrix X′ to obtain a signal measurement matrix X whose rank is larger than a number of wireless signal travel paths; (22) according to a steering vector model and the signal measurement matrix X, acquiring an AoA and a ToF corresponding to each travel path by a multiple signal classification (MUSIC) algorithm; and (23) determining a travel path having a minimum ToF as a direct path, and determining an AoA and the ToF corresponding to the direct path as the AoA and the ToF between the wireless signal transmitting device and the intelligent device, wherein the steering vector model is used to describe, by using the AoA and the ToF, a phase offset introduced by the respective array elements.

3. The intelligent device navigation method of claim 2, wherein the steering vector model is: $\stackrel{.fwdarw.}{\alpha }\left(\theta ,\tau \right)={\left[\begin{array}{ccc}\stackrel{\stackrel{\mathrm{element}1}{︷}}{1,{\Omega }_{\tau }^1,{\Omega }_{\tau }^2,\mathrm{.Math.},{\Omega }_{\tau }^{N-1}},& \underset{\underset{\mathrm{element}2}{︸}}{{\Phi }_{\theta },{\Omega }_{\tau }^1{\Phi }_{\theta },{\Omega }_{\tau }^2{\Phi }_{\theta },\mathrm{.Math.},{\Omega }_{\tau }^{N-1}{\Phi }_{\theta }},\mathrm{.Math.},& \stackrel{\stackrel{\mathrm{element}M}{︷}}{{\Phi }_{\theta }^{M-1},\mathrm{.Math.},{\Omega }_{\tau }^{N-1}{\Phi }_{\theta }^{M-1}}\end{array}\right]}^T,$ wherein Φ.sub.θ and Ω.sub.τ are respectively expressed as:
Φ.sub.θ=e.sup.−j2π×d×sin (θ)×f/c; and
Ω.sub.τ=e.sup.−j2π×f.sup.δ.sup.×τ, wherein the superscript represents an exponent of the power operation, θ represents an AoA, τ represents a ToF, {right arrow over (a)}(θ, τ) represents a phase offset caused by the array elements corresponding to a travel path with an AoA of θ and a ToF of τ, Φ.sub.θ represents a phase offset introduced by the array elements corresponding to a travel path with AoA of θ, Ω.sub.τ represents a phase offset introduced by the array elements corresponding to a travel path with ToF of τ, d represents a spacing between two adjacent array elements, f represents a frequency of a transmitted signal, c represents a velocity of light, f.sub.δ represents a frequency spacing between adjacent subcarriers, and M and N respectively represent the number of array elements in the linear antenna array and the number of subcarriers for the transmission of wireless signals.

4. The intelligent device navigation method of claim 1, wherein the step (3) includes the following steps: (31) constructing a measurement information matrix of the inertial measurement unit according to the inertial parameters measured by the inertial measurement unit, and calculating a measurement vector of the inertial measurement unit; (32) constructing a measurement information matrix of wireless signals according to CSIs of the acquired wireless signals, and calculating a measurement vector of the wireless signals; (33) according to the sliding window estimator, the measurement information matrix and the measurement vector of the inertial measurement unit, and the measurement information matrix and the measurement vector of the wireless signals, calculating a current sliding window estimator by minimizing a sum of Mahalanobis norm of all measurement errors; taking the latest state variable in the current sliding window estimator as the estimated state variable.

5. The intelligent device navigation method of claim 4, wherein in the step (31), the measurement vector of the inertial measurement unit is calculated according to an equation below: ${B_{k+1}}_{B_k}^{}=\hspace{1em}\left[\begin{array}{c}{\hat{\alpha }}_{B_{k+1}}^{B_k}\\ {\hat{\beta }}_{B_{k+1}}^{B_k}\\ \hat{0}\end{array}\right]=\hspace{1em}\left[\begin{array}{c}{R}_{B_0}^{B_k}\left(p_{B_{k+1}}^{B_0}-p_{B_k}^{B_0}\right)-v_{B_k}^{B_k}\Delta t+\frac{g^{B_k}\Delta t^2}{2}\\ R_{B_{k+1}}^{B_k}{v}_{B_{k+1}}^{B_{k+1}}-v_{B_k}^{B_k}+g^{B_k}\Delta t\\ R_{B_{k+1}}^{B_k}{g}^{B_{k+1}}-g^{B_k}\end{array}\right]={B_{k+1}}_{B_k}^{}\chi +n_{1_{B_{k+1}}}^{B_k},$ wherein B.sub.0 represents a body frame of the intelligent device when a 0.sup.th AoA is obtained, B.sub.k and B.sub.k+1 respectively represent body frames of the intelligent device when k.sup.th and (k+1).sup.th AoAs are obtained, ${B_{k+1}}_{B_k}^{}$ represents a measurement information matrix of the inertial measurement unit between when the k.sup.th AoA is obtained and when the (k+1).sup.th AoA is obtained, {circumflex over (α)}.sub.B.sub.k+1.sup.B.sup.k and {circumflex over (β)}.sub.B.sub.k+1.sup.B.sup.k are both covariance matrixes calculated by the measurement information matrix ${B_{k+1}}_{B_k}^{},$ R.sub.B.sub.0.sup.B.sup.k represents a transfer matrix of the frame B.sub.k relative to the frame B.sub.0, p.sub.B.sub.k+1.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k+1, Δt represents a time difference between when the k.sup.th AoA is obtained and when the (k+1).sup.th AoA is obtained, R.sub.B.sub.k+1.sup.B.sup.k represents a transfer matrix of the frame B.sub.k relative to the frame B.sub.k+1, v.sub.B.sub.k+1.sup.B.sup.k+1 represents a velocity matrix of the frame B.sub.k+1 relative to the frame B.sub.k+1, g.sup.B.sup.k+1 represents an earth gravity vector in the frame B.sub.k+1, ${B_{k+1}}_{B_k}^{}$ represents a measurement vector of the inertial measurement unit, $n_{1_{B_{k+1}}}^{B_k}$ represents a measurement noise of the inertial measurement unit, and χ represents a full state parameter vector composed of all state variables and position information of the wireless signal transmitting devices in the sliding window estimator.

6. The intelligent device navigation method of claim 4, wherein in the step (32), the measurement vector of the wireless signals is calculated according to an equation below: $l_{B_j}^{}=\hat{0}=\left[\begin{array}{ccc}-1& 0& {\hat{u}}_l^{B_j}\\ 0& -1& {\hat{v}}_l^{B_j}\end{array}\right]R_{B_0}^{B_j}\left(p_{\mathrm{Bk}}^{B_0}-p_{B_j}^{B_0}+d_l{R}_{\mathrm{Bi}}^{B_0}\left[\begin{array}{c}{u}_l^{B_i}\\ v_l^{B_i}\\ 1\end{array}\right]\right)=l_{B_j}^{}\chi +n_{2_l}^{B_j},$ wherein B.sub.0 represents a body frame of the intelligent device when a 0.sup.th AoA is obtained, B.sub.k, B.sub.i and B.sub.j respectively represent body frames of the intelligent device when k.sup.th, i.sup.th and j.sup.th AoAs are obtained, .sub.l.sup.B.sup.j represents a signal matrix measured from a l.sup.th wireless signal transmitting device when the j.sup.th AoA is obtained, û.sub.l.sup.B.sup.j represents the cosine of the measurement value of the AoA, {circumflex over (v)}.sub.l.sup.B.sup.j represents the sine of the measurement value of the AoA, u.sub.l.sup.B.sup.i represents the cosine of the true value of the AoA, v.sub.l.sup.B.sup.i represents the sine of the true value of the AoA, R.sub.B.sub.0.sup.B.sup.j represents a transfer matrix of the frame B.sub.j relative to the frame B.sub.0, p.sub.B.sub.k+1.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k+1, p.sub.B.sub.j.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.j, R.sub.B.sub.i.sup.B.sup.0 represents a transfer matrix of the frame B.sub.0 relative to the frame B.sub.i, d.sub.l represents a distance between the l.sup.th wireless signal transmitting device and the intelligent device, .sub.l.sup.B.sup.j represents a measurement vector measured from the l.sup.th wireless signal transmitting device when the j.sup.th AoA is obtained, n.sub.2.sub.l.sup.B.sup.j represents a measurement noise of the wireless signals, and χ represents a full state parameter vector composed of all state variables and position information of the wireless signal transmitting devices in the sliding window estimator.

7. The intelligent device navigation method of claim 4, wherein in the step (33), the current sliding window estimator is calculated according to an equation below:
(Λ.sub.p+Λ.sub.imu+Λ.sub.WiFi)χ=(b.sub.p+b.sub.imu+b.sub.WiFi)  (1), wherein b.sub.p are Λ.sub.p are both priori information of the intelligent device, Λ.sub.imu and b.sub.imu respectively represent the measurement information matrix and the measurement vector of the inertial measurement unit, Λ.sub.WiFi and b.sub.WiFi respectively represent the measurement information matrix and the measurement vector of the wireless signals, and χ represents a full state parameter vector composed of all state variables and position information of the wireless signal transmitting devices in the sliding window estimator.

8. The intelligent device navigation method of claim 7, wherein the sliding window estimator slides with an increase in the number of obtained AoAs, and when the sliding window estimator slides, the old state variable and the position information of the unobservable wireless signal transmitting device are removed by applying Schur complement to the least square equations corresponding to the equation (1).

9. The intelligent device navigation method of claim 5, wherein the full state parameter vector χ is expressed as: $\chi =\left[x_{B_s}^{B_0},x_{B_{s+1}}^{B_0},\mathrm{.Math.},x_{B_{N_1+s-1}}^{B_0},a_1,a_2,\mathrm{.Math.},a_{N_2}\right],$ wherein N.sub.1 and N.sub.2 respectively represent the total number of state variables and the total number of wireless signal transmitting devices currently observable by the intelligent device included in the sliding window estimator, s represents an integer greater than or equal to 0, B.sub.0 represents the body frame of the intelligent device when the 0.sup.th AoA is obtained, B.sub.k represents the body frame of the intelligent device when the k.sup.th AoA is obtained, x.sub.B.sub.k.sup.B.sup.0=[p.sub.B.sub.k.sup.B.sup.0,v.sub.B.sub.k.sup.B.sup.k,g.sup.B.sup.k] represents an obtained k.sup.th state variable of the intelligent device, p.sub.B.sub.k.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k, v.sub.B.sub.k.sup.B.sup.k represents a velocity matrix of the frame B.sub.k relative to the frame B.sub.k, g.sup.B.sup.k represents an earth gravity vector in the frame B.sub.k, a value of i satisfies 1≤i≤N.sub.2, and a.sub.i represents a position of the i.sup.th wireless signal transmitting device.

10. An intelligent device navigation system for achieving autonomous navigation of an intelligent device, comprising: a wireless positioning module, an inertial measurement unit, a state estimation module and a control module; wherein the wireless positioning module is configured to acquire channel state information (CSI) of a wireless signal transmitted by a wireless signal transmitting device in an environment by using an antenna array and estimate an angle of arrival (AoA) and a time of flight (ToF) between the wireless signal transmitting device and the intelligent device; the inertial measurement unit is configured to measure inertial parameters of the intelligent device; the state estimation module is configured to perform data fusion of the AoAs, the ToFs and the inertial parameters by a sliding window estimator to estimate a state variable of the intelligent device; and the control module is configured to adjust a motion state of the intelligent device with reference to the state variable, thereby achieving autonomous navigation of the intelligent device, wherein the wireless signal is an OFDM signal, the sliding window estimator is configured to save the latest state variables of the intelligent device and position information of the wireless signal transmitting device(s), and the antenna array is a linear antenna array composed by a plurality of antennas on a network card in the intelligent device, each antenna being an array element.

11. The intelligent device navigation method of claim 6, wherein the full state parameter vector χ is expressed as: $\chi =\left[x_{B_s}^{B_0},x_{B_{s+1}}^{B_0},\mathrm{.Math.},x_{B_{N_1+s-1}}^{B_0},a_1,a_2,\mathrm{.Math.},a_{N_2}\right],$ wherein N.sub.1 and N.sub.2 respectively represent the total number of state variables and the total number of wireless signal transmitting devices currently observable by the intelligent device included in the sliding window estimator, s represents an integer greater than or equal to 0, B.sub.0 represents the body frame of the intelligent device when the 0.sup.th AoA is obtained, B.sub.k represents the body frame of the intelligent device when the k.sup.th AoA is obtained, x.sub.B.sub.k.sup.B.sup.0=[p.sub.B.sub.k.sup.B.sup.0,v.sub.B.sub.k.sup.B.sup.k,g.sup.B.sup.k] represents an obtained k.sup.th state variable of the intelligent device, p.sub.B.sub.k.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k, v.sub.B.sub.k.sup.B.sup.k represents a velocity matrix of the frame B.sub.k relative to the frame B.sub.k, g.sup.B.sup.k represents an earth gravity vector in the frame B.sub.k, a value of i satisfies 1≤i≤N.sub.2, and a.sub.i represents a position of the i.sup.th wireless signal transmitting device.

12. The intelligent device navigation method of claim 7, wherein the full state parameter vector χ is expressed as: $\chi =\left[x_{B_s}^{B_0},x_{B_{s+1}}^{B_0},\mathrm{.Math.},x_{B_{N_1+s-1}}^{B_0},a_1,a_2,\mathrm{.Math.},a_{N_2}\right],$ wherein N.sub.1 and N.sub.2 respectively represent the total number of state variables and the total number of wireless signal transmitting devices currently observable by the intelligent device included in the sliding window estimator, s represents an integer greater than or equal to 0, B.sub.0 represents the body frame of the intelligent device when the 0.sup.th AoA is obtained, B.sub.k represents the body frame of the intelligent device when the k.sup.th AoA is obtained, x.sub.B.sub.k.sup.B.sup.0=[p.sub.B.sub.k.sup.B.sup.0,v.sub.B.sub.k.sup.B.sup.k,g.sup.B.sup.k] represents an obtained k.sup.th state variable of the intelligent device, p.sub.B.sub.k.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k, v.sub.B.sub.k.sup.B.sup.k represents a velocity matrix of the frame B.sub.k relative to the frame B.sub.k, g.sup.B.sup.k represents an earth gravity vector in the frame B.sub.k, a value of i satisfies 1≤i≤N.sub.2, and a.sub.i represents a position of the i.sup.th wireless signal transmitting device.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a schematic diagram of an antenna array according to embodiments of the present disclosure.

(2) FIG. 2 is a schematic diagram showing data fusion of AoAs and ToFs between an intelligent device and one or more wireless signal transmitting devices as well as inertial parameters of the intelligent device according to the embodiments of the present disclosure.

(3) FIG. 3 is a schematic diagram of an intelligent device navigation system according to the embodiments of the present disclosure.

(4) FIGS. 4A and 4B are diagrams showing state estimation according to the embodiments of the present disclosure. FIG. 4A shows three-dimensional positions of an intelligent device; and FIG. 4B shows three-dimensional positions of a surrounding router.

DETAILED DESCRIPTION OF THE EMBODIMENTS

(5) For clear understanding of the objectives, features and advantages of the present disclosure, detailed description of the present disclosure will be given below in conjunction with accompanying drawings and specific embodiments. It should be noted that the embodiments described herein are only meant to explain the present disclosure, and not to limit the scope of the present disclosure. Furthermore, the technical features related to the embodiments of the disclosure described below can be mutually combined if they are not found to be mutually exclusive.

(6) The existing navigation systems mainly rely on visual sensing and laser sensing for positioning in GPS unreliable environments. However, visual sensing navigation is computationally complex and susceptible to lighting conditions; and laser sensing equipment is expensive and heavy, and is not suitable for small intelligent device platforms. The key factor for navigation of an intelligent device is positioning, and the accuracy of wireless indoor positioning has reached the decimeter level in the latest advances. Thus, by an inertial measurement unit (IMU) of the intelligent device assisted with wireless positioning technology, accurate autonomous navigation can be achieved even in GPS unreliable environments.

(7) Since wireless signal resources (e.g., WiFi resources) are ubiquitous in indoor locations, and the wireless signal resources can provide reliable information about the environment (e.g., AoA and ToF), a wireless signal transmitting device (AP) that provides wireless signals can be an environmental reference landmark. In the following embodiments, the wireless signal transmitting device in the environment is a router, and the wireless signals are OFDM signals.

(8) An intelligent device navigation method for achieving autonomous navigation of an intelligent device according to the present disclosure comprises the following.

(9) (1) Construct a plurality of antennas on a network card in the intelligent device into a linear antenna array, in which each of the antenna is an array element.

(10) As shown in FIG. 1, in this embodiment, the antenna array includes three antennas, and the distance between adjacent antennas is d.

(11) (2) By using the antenna array, acquire channel state information (CSI) of a wireless signal transmitted by a wireless signal transmitting device in an environment, and estimate an angle of arrival (AoA) and a time of flight (ToF) between the wireless signal transmitting device and the intelligent device; measure inertial parameters of the intelligent device by using an inertial measurement unit.

(12) In an alternative embodiment, the method of estimating the AoA and the ToF between the wireless signal transmitting device and the intelligent device includes the following.

(13) (21) Superimpose the CSIs of N OFDM signal subcarriers received by the antenna array to obtain a measurement signal matrix X′, and perform CSI smoothing on the measurement signal matrix X′ to obtain a signal measurement matrix X whose rank is larger than a number of wireless signal travel paths.

(14) In order to solve for the AoA and the ToF between the wireless signal transmitting device and the intelligent device with a MUSIC algorithm, it is necessary to ensure that the number of columns of the input measurement signal matrix is larger than the number of wireless signal travel paths. In the present embodiment, since the initially obtained measurement signal matrix is a matrix with single column, which does not meet the requirements of the MUSIC algorithm, a CSI smoothing method is adopted to construct a new measurement matrix by performing translation slip with measured values of the first N/2 subcarriers corresponding to the first two array elements as a reference, and the smoothed measurement matrix can have a dimension of up to N×(N+2). Through the CSI smoothing method, the obtained signal measurement matrix X is allowed to have a rank larger than the number of wireless signal travel paths, thereby meeting the requirements of the MUSIC algorithm.

(15) (22) According to a steering vector model and the signal measurement matrix X, acquire an AoA and a ToF corresponding to each travel path by a MUSIC algorithm.

(16) (23) Determine a travel path having a minimum ToF as a direct path, and determine an AoA and the ToF corresponding to the direct path as the AoA and the ToF between the wireless signal transmitting device and the intelligent device.

(17) Among the multiple travel paths, the path corresponding to the minimum ToF is the direct path. Since the sampling clocks of the digital-to-analog converter (DAC) and the analog-to-digital converter (ADC) in the transmitter and the receiver are not synchronized in the standard wireless communication network, the ToF estimated by the MUSIC algorithm here includes a delay caused by the difference in sampling clocks, and is thus not a true transmission time of the signal from the transmitting end to the receiving end. In addition, since the electromagnetic wave travels at the speed of light, and the delay of several tens of nanoseconds can cause the distance error of the meter level, the ToF estimated by the MUSIC algorithm here cannot represent a true signal transmission distance. Even so, since the delay caused by the difference in sampling clocks affects ToFs of all the paths simultaneously, the conclusion that the path corresponding to the minimum ToF is the signal direct path still holds.

(18) The steering vector model is used to describe a phase offset introduced by corresponding array elements by using the AoA and the ToF. In this embodiment, the steering vector model is:

(19) $\stackrel{.fwdarw.}{\alpha }\left(\theta ,\tau \right)={\left[\begin{array}{ccc}\stackrel{\stackrel{\mathrm{element}1}{︷}}{1,{\Omega }_{\tau }^1,{\Omega }_{\tau }^2,\mathrm{.Math.},{\Omega }_{\tau }^{N-1}},& \underset{\underset{\mathrm{element}2}{︸}}{{\Phi }_{\theta },{\Omega }_{\tau }^1{\Phi }_{\theta },{\Omega }_{\tau }^2{\Phi }_{\theta },\mathrm{.Math.},{\Omega }_{\tau }^{N-1}{\Phi }_{\theta }},\mathrm{.Math.},& \stackrel{\stackrel{\mathrm{element}M}{︷}}{{\Phi }_{\theta }^{M-1},\mathrm{.Math.},{\Omega }_{\tau }^{N-1}{\Phi }_{\theta }^{M-1}}\end{array}\right]}^T,$
where Φ.sub.θ and Ω.sub.τ are respectively expressed as:
Φ.sub.θ=e.sup.−j2π×d×sin (θ)×f/c; and
Ω.sub.τ=e.sup.−j2π×f.sup.δ.sup.×τ,
in which the superscript represents an exponent of the power operation, θ represents an AoA, τ represents a ToF, {right arrow over (a)}(θ,τ) represents a phase offset caused by the array elements corresponding to a travel path with an AoA of θ and a ToF of τ, Φ.sub.θ represents a phase offset introduced by the array elements corresponding to a travel path with AoA of θ, Ω.sub.τ represents a phase offset introduced by the array elements corresponding to a travel path with ToF of τ, d represents a spacing between two adjacent array elements, f represents a frequency of a transmitted signal, c represents a velocity of light, f.sub.δ represents a frequency spacing between adjacent subcarriers, and M and N respectively represent the number of array elements in the antenna array and the number of subcarriers for the transmission of wireless signals.

(20) (3) Perform data fusion of the AoAs, the ToFs and the inertial parameters by a sliding window estimator to estimate a state variable of the intelligent device, the sliding window estimator is configured to save the latest state variables of the intelligent device and position information of the current observable wireless signal transmitting device(s).

(21) In an alternative embodiment, the step (3) includes the following steps.

(22) (31) Construct a measurement information matrix of the inertial measurement unit according to the inertial parameters measured by the inertial measurement unit, and calculate a measurement vector of the inertial measurement unit.

(23) In the present embodiment, the measurement vector of the inertial measurement unit is calculated according to an equation below:

(24) 0${B_{k+1}}_{B_k}^{}=\hspace{1em}\left[\begin{array}{c}{\hat{\alpha }}_{B_{k+1}}^{B_k}\\ {\hat{\beta }}_{B_{k+1}}^{B_k}\\ \hat{0}\end{array}\right]=\left[\begin{array}{c}{R}_{B_0}^{B_k}\left(p_{B_{k+1}}^{B_0}-p_{B_k}^{B_0}\right)-v_{B_k}^{B_k}\Delta t+\frac{g^{B_k}\Delta t^2}{2}\\ R_{B_{k+1}}^{B_k}{v}_{B_{k+1}}^{B_{k+1}}-v_{B_k}^{B_k}+g^{B_k}\Delta t\\ R_{B_{k+1}}^{B_k}{g}^{B_{k+1}}-g^{B_k}\end{array}\right]={B_{k+1}}_{B_k}^{}\chi +n_{1_{B_{k+1}}}^{B_k},$
where B.sub.0 represents a body frame of the intelligent device when a 0.sup.th AoA is obtained, B.sub.k and B.sub.k+1 respectively represent body frames of the intelligent device when k.sup.th and (k+1).sup.th AoAs are obtained,

(25) ${B_{k+1}}_{B_k}^{}$
represents a measurement information matrix of the inertial measurement unit between when the k.sup.th AoA is obtained and when the (k+1).sup.th AoA is obtained, {circumflex over (α)}.sub.B.sub.k+1.sup.B.sup.k and {circumflex over (β)}.sub.B.sub.k+1.sup.B.sup.k are both covariance matrixes calculated by the measurement information matrix

(26) ${B_{k+1}}_{B_k}^{},$
R.sub.B.sub.0.sup.B.sup.k represents a transfer matrix of the frame B.sub.k relative to the frame B.sub.0, p.sub.B.sub.k+1.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k+1, Δt represents a time difference between when the k.sup.th AoA is obtained and when the (k+1).sup.th AoA is obtained, R.sub.B.sub.k+1.sup.B.sup.k represents a transfer matrix of the frame B.sub.k relative to the frame B.sub.k+1, v.sub.B.sub.k+1.sup.B.sup.k+1 represents a velocity matrix of the frame B.sub.k+1 relative to the frame B.sub.k+1, g.sup.B.sup.k+1 represents an earth gravity vector in the frame B.sub.k+1,

(27) ${B_{k+1}}_{B_k}^{}$
represents a measurement vector of the inertial measurement unit,

(28) $n_{1_{B_{k+1}}}^{B_k}$
represents a measurement noise of the inertial measurement unit. The equation set of calculating the covariance matrixes {circumflex over (α)}.sub.B.sub.k+1.sup.B.sup.k and {circumflex over (β)}.sub.B.sub.k+1.sup.B.sup.k by the measurement information matrix

(29) ${B_{k+1}}_{B_k}^{}$
is:

(30) $\{\begin{array}{c}{p}_{B_{k+1}}^{B_0}=p_{B_k}^{B_0}+R_{B_k}^{B_0}{v}_{B_k}^{B_k}\Delta t-R_{B_k}^{B_0}{g}^{B_k}\frac{\Delta t^2}{2}+R_{B_k}^{B_0}{\alpha }_{B_{k+1}}^{B_k}\\ v_{B_{k+1}}^{B_{k+1}}=R_{B_k}^{B_{k+1}}{v}_{B_k}^{B_k}-R_{B_k}^{B_{k+1}}{g}^{B_k}\Delta t+R_{B_k}^{B_{k+1}}{\beta }_{B_{k+1}}^{B_k}\\ g^{B_{k+1}}=R_{B_k}^{B_{k+1}}{g}^{B_k}\end{array}.$

(31) (32) Construct a measurement information matrix of wireless signals according to CSIs of the acquired wireless signals, and calculate a measurement vector of the wireless signals.

(32) In the present embodiment, the measurement vector of the wireless signals is calculated according to an equation below:

(33) $l_{B_j}^{}=\hat{0}=\left[\begin{array}{ccc}-1& 0& {\hat{u}}_l^{B_j}\\ 0& -1& {\hat{v}}_l^{B_j}\end{array}\right]R_{B_0}^{B_j}\left(p_{B_k}^{B_0}-p_{B_j}^{B_0}+d_l{R}_{B_i}^{B_0}\left[\begin{array}{c}{u}_l^{B_i}\\ v_l^{B_i}\\ 1\end{array}\right]\right)=l_{B_j}^{}\chi +n_{2_l}^{B_j},$
where B.sub.i and B.sub.j respectively represent body frames of the intelligent device when i.sup.th and j.sup.th AoAs are obtained, .sub.l.sup.B.sup.j represents a signal matrix measured from a l.sup.th wireless signal transmitting device when the j.sup.th AoA is obtained, û.sub.l.sup.B.sup.j represents the cosine of the measurement value of the AoA, {circumflex over (v)}.sub.l.sup.B.sup.j represents the sine of the measurement value of the AoA, u.sub.l.sup.B.sup.i represents the cosine of the true value of the AoA, v.sub.l.sup.B.sup.i represents the sine of the true value of the AoA, R.sub.B.sub.0.sup.B.sup.j represents a transfer matrix of the frame B.sub.j relative to the frame B.sub.0, p.sub.B.sub.k+1.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k+1, p.sub.B.sub.j.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.j, R.sub.B.sub.i.sup.B.sup.0 represents a transfer matrix of the frame B.sub.0 relative to the frame B.sub.i, d.sub.l represents a distance between the l.sup.th wireless signal transmitting device and the intelligent device, .sub.l.sup.B.sup.j represents a measurement vector measured from the l.sup.th wireless signal transmitting device when the j.sup.th AoA is obtained, n.sub.2.sub.l.sup.B.sup.j represents a measurement noise of the wireless signals.

(34) (33) According to the sliding window estimator, the measurement information matrix and the measurement vector of the inertial measurement unit, and the measurement information matrix and the measurement vector of the wireless signals, calculate a current sliding window estimator by minimizing a sum of Mahalanobis norm of all measurement errors; take the latest state variable in the current sliding window estimator as the estimated state variable.

(35) Since the position of the router is unknown, the state estimation essentially solves the problem of simultaneous localization and mapping (SLAM). The state estimation is described as a state estimation problem of assisting IMU with the wireless system, and the wireless positioning module and the IMU measurement data are combined to obtain a maximum likelihood estimation by minimizing the sum of the Mahalanobis norm of all measurement errors. The specific solution model is as follow:

(36) $\underset{\chi }{\min }\left\{\left(b_p-{\Lambda }_p\chi \right)+\underset{k\in D}{.Math.}{.Math.{\hat{z}}_{B_{K+1}}^{B_k}-{\hat{H}}_{B_{k+1}}^{B_k}\chi .Math.}_{P_{B_{k+1}}^{B_k}}^2+\underset{\left(l,j\right)\in C}{.Math.}{.Math.{\hat{z}}_l^{B_j}-{\hat{H}}_l^{B_j}\chi .Math.}_{P_l^{B_j}}^2\right\}.$

(37) In the above solution model, D represents a set of all measurement data of the IMU, C represents a set of all measurement data of the wireless positioning module,

(38) $\underset{k\in D}{.Math.}{.Math.{\hat{z}}_{B_{k+1}}^{B_k}-{\hat{H}}_{B_{k+1}}^{B_k}\chi .Math.}_{P_{B_{k+1}}^{B_k}}^2$
represents a measurement error of the IMU,

(39) 0$\underset{\left(l,j\right)\in C}{.Math.}{.Math.{\hat{z}}_l^{B_j}-{\hat{H}}_l^{B_j}\chi .Math.}_{P_l^{B_j}}^2$
represents a measurement error of the wireless positioning module, and b.sub.p and Λ.sub.p are both priori information of the intelligent device. Since the all the restrictions are linear, in the present embodiment, the above solution model is organized, and finally, the current sliding window estimator may be calculated according to an equation below:
(Λ.sub.p+Λ.sub.imuΛ.sub.WiFi)χ=(b.sub.p+b.sub.imu+b.sub.WiFi)  (1),
where b.sub.p are Λ.sub.p are both priori information of the intelligent device, Λ.sub.imu and b.sub.imu respectively represent the measurement information matrix and the measurement vector of the inertial measurement unit, Λ.sub.WiFi and b.sub.WiFi respectively represent the measurement information matrix and the measurement vector of the wireless signals.

(40) In the present embodiment, the sliding window estimator slides with an increase in the number of obtained AoAs, and when the sliding window estimator slides, the old state variable and the position information of the unobservable wireless signal transmitting device are removed by applying Schur complement to the least square equations corresponding to the equation (1).

(41) In order to constrain the computational complexity, the sliding window estimator only keeps the latest states, removing the old state of the intelligent device and the position information of the unobservable wireless signal transmitting device; in the commonly used method of removing the old state, these parameters are removed from the system equation. However, this will result in the loss of a portion of the information, and using only the measurement information in the sliding window will result in scale inconsistency. When solving the optimization model, by applying the Schur complement to the least squares equations corresponding to the optimization model, the parameters to be removed can be marginalized, thereby avoiding the problem of scale inconsistency due to information loss.

(42) In the above equation, χ represents a full state parameter vector composed of all state variables and position information of the wireless signal transmitting devices in the sliding window estimator, and the full state parameter vector χ may be expressed as:

(43) $\chi =\left[x_{B_s}^{B_0},x_{B_{s+1}}^{B_0},\mathrm{.Math.},x_{B_{N_1+s-1}}^{B_0},a_1,a_2,\mathrm{.Math.},a_{N_2}\right],$
where N.sub.1 and N.sub.2 respectively represent the total number of state variables and the total number of wireless signal transmitting devices currently observable by the intelligent device included in the sliding window estimator, s represents an integer greater than or equal to 0, B.sub.0 represents the body frame of the intelligent device when the 0.sup.th AoA is obtained, B.sub.k represents the body frame of the intelligent device when the k.sup.th AoA is obtained, x.sub.B.sub.k.sup.B.sup.0=[p.sub.B.sub.k.sup.B.sup.0,v.sub.B.sub.k.sup.B.sup.k,g.sup.B.sup.k] represents an obtained k.sup.th state variable of the intelligent device, p.sub.B.sub.k.sup.B.sup.0 represents a position matrix of the frame B.sub.0 relative to the frame B.sub.k, v.sub.B.sub.k.sup.B.sup.k represents a velocity matrix of the frame B.sub.k relative to the frame B.sub.k, g.sup.B.sup.k represents an earth gravity vector in the frame B.sub.k, a value of i satisfies 1≤i≤N.sub.2, and a.sub.i represents a position of the i.sup.th wireless signal transmitting device.

(44) Through achieving state estimation of the intelligent device with a sliding window estimator, the state estimation problem can be solved with stable computation time complexity. Since the data output rate of the IMU in the intelligent device navigation system is higher than the AoA output rate of the wireless positioning module, more than one IMU measurement data is obtained between when the k.sup.th AoA is obtained and when the (k+1).sup.th AoA is obtained. In the intelligent device navigation system, the antenna array has been azimuthally calibrated with the IMU, and a transfer matrix between the body frame of the intelligent device and the frame of the wireless positioning module is known. When the sliding window estimator is used to estimate the state variables of the intelligent device, the operation model of the intelligent device navigation system is shown in FIG. 2.

(45) In addition, since the sliding window estimator contains both state variables of the intelligent device and position information of the wireless signal transmitting devices, position information of the wireless signal transmitting device can be obtained while a state variable of the intelligent device can be obtained by solving the model.

(46) (4) Adjust a motion state of the intelligent device with reference to the state variable, thereby achieving autonomous navigation of the intelligent device.

(47) Since wireless signal resources (e.g., WiFi resources) are ubiquitous in indoor locations, and wireless signal resources can provide reliable information about the environment (e.g., AoA and ToF), the intelligent device navigation method according to the present disclosure utilizes wireless positioning technology to measure the AoA and ToF between the intelligent device and the wireless signal transmitting device in the environment, and to estimate the state variable of the intelligent device in combination with the inertial parameters of the intelligent device, thereby achieving accurate autonomous navigation of the intelligent device.

(48) The present disclosure further provides an intelligent device navigation system for achieving autonomous navigation of an intelligent device. As shown in FIG. 3, the system includes: a wireless positioning module, an inertial measurement unit, a state estimation module and a control module.

(49) The wireless positioning module is configured to acquire CSI of a wireless signal transmitted by a wireless signal transmitting device in an environment by using an antenna array and estimate an AoA and a ToF between the wireless signal transmitting device and the intelligent device.

(50) The inertial measurement unit is configured to measure inertial parameters of the intelligent device.

(51) The state estimation module is configured to perform data fusion of the AoAs, the ToFs and the inertial parameters by a sliding window estimator to estimate a state variable of the intelligent device.

(52) The control module is configured to adjust a motion state of the intelligent device with reference to the state variable, thereby achieving autonomous navigation of the intelligent device.

(53) In which, the wireless signal is an OFDM signal, the sliding window estimator is configured to save the latest state variables of the intelligent device and position information of the wireless signal transmitting device(s), and the antenna array is a linear antenna array composed by a plurality of antennas on a network card in the intelligent device, each of the antenna being an array element.

(54) In the present embodiment, modes for carrying out the functional modules may refer to the description in the foregoing method embodiments, and are not repeated herein.

(55) A motion track is preset, and the final navigation effect achieved by the intelligent device navigation method and navigation system according to the present disclosure is shown in FIGS. 4A and 4B. FIG. 4A shows a difference between the actual three-dimensional position of the intelligent device in the X, Y, Z direction and the preset motion trajectory at any time, which reflects the navigation error. Curves with intersections correspond to an actual motion trajectory of the intelligent device, and curves without intersections correspond to the preset motion trajectory. As can be seen from the results shown in FIG. 4A, change in values of the estimated trajectory on the x-y-z coordinate axes is very close to that of the actual motion trajectory, indicating that the intelligent device navigation method and navigation system according to the present disclosure can accurately estimate the trajectory of the intelligent device. FIG. 4B shows a positioning effect of a signal transmitting end (such as a router), in which curves with intersections correspond to estimated router positions, and the straight lines without intersections corresponds to an actual position of the router. Initially, the router's position is assumed to be (0, 0, 0). Then, the system gradually corrects the router's position by calculation, so that the router's position gets close to the real position after about 10 seconds. It can be seen from the results in FIG. 4B that the intelligent device navigation method and navigation system according to the present disclosure can accurately estimate the position of the router while achieving autonomous navigation of the intelligent device. Therefore, even in GPS unreliable environments, the present disclosure can achieve accurate autonomous navigation of the intelligent device, thereby solving problems such as complicated calculation, susceptibility to lighting conditions, expensive equipment, heavy weight and limited application caused by positioning by virtue of visual sensing and laser sensing in the existing navigation systems.

(56) It should be readily understood to those skilled in the art that the above description is only preferred embodiments of the present disclosure, and does not limit the scope of the present disclosure. Any change, equivalent substitution and modification made without departing from the spirit and scope of the present disclosure should be included within the scope of the protection of the present disclosure.