Localization and attitude estimation method using magnetic field and system thereof

Abstract

A localization and attitude estimation method using magnetic fields includes the following steps. First, in three-dimensional coordinates, at least three magnetic landmarks arbitrarily disposed around a moving carrier are selected, wherein any two of the at least three magnetic landmarks have different magnetic directions. One set of at least five tri-axes magnetic sensors is used to sense the magnetic fields of the at least three magnetic landmarks. Three magnetic components on three axes of a current position of each of the tri-axes magnetic sensors are respectively generated by a demagnetization method. Five non-linear magnetic equations are solved to obtain position information and magnetic moment information of the at least three magnetic landmarks in the three-dimensional coordinates. Position vectors and attitude vectors of the set of at least five tri-axes magnetic sensors in a three-dimensional space are estimated based on tri-axes magnetic moment vectors of the magnetic landmarks.

Claims

1. A localization and attitude estimation method using magnetic fields to localize a moving carrier, on which one set of at least five tri-axes magnetic sensors is disposed, the localization and attitude estimation method comprising: selecting at least three magnetic landmarks arbitrarily disposed around the moving carrier in three-dimensional coordinates, wherein any two of the at least three magnetic landmarks have different magnetic directions, wherein position information and magnetic moment information of the at least three magnetic landmarks in the three-dimensional coordinates are unknown information; using the set of at least five tri-axes magnetic sensors to sense the magnetic fields of the at least three magnetic landmarks, and respectively generating three magnetic components on three axes of a current position of each of the tri-axes magnetic sensors using a demagnetization method, wherein three-dimensional coordinates of the set of at least five tri-axes magnetic sensors are not located on a same plane; obtaining five non-linear magnetic equations according to the three magnetic components on the three axes of the current position of each of the tri-axes magnetic sensors, and then solving the five non-linear magnetic equations according to magnetic moment vectors of the at least three magnetic landmarks in a null space to obtain the position information and the magnetic moment information of the at least three magnetic landmarks in the three-dimensional coordinates, and estimating position vectors and attitude vectors of the set of at least five tri-axes magnetic sensors in the three-dimensional coordinates according to tri-axes magnetic moment vectors of the at least three magnetic landmarks, and estimating position information and magnetic moment information of at least one unknown magnetic landmark in the three-dimensional coordinates according to the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors at a new position in the three-dimensional coordinates when the moving carrier moves to the new position, when a distance of at least one of the three magnetic landmarks exceeds a predetermined range from the moving carrier and the at least one unknown magnetic landmark is disposed around the moving carrier, the method re-estimates the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors in the three-dimensional coordinates by selecting the at least one unknown magnetic landmark, and obtaining position information and magnetic moment information of the at least one unknown magnetic landmark in the three-dimensional coordinates.

2. The localization and attitude estimation method according to claim 1, wherein the demagnetization method comprises: using a band pass filter to divide the magnetic fields of the at least three magnetic landmarks into the three magnetic components on the three axes of the three-dimensional coordinates; and using an extended Kalman filter having three different constant frequencies functioning as restriction items to resolve waveforms and amplitudes of the at least three magnetic landmarks and obtain at least three sets of waveforms and amplitudes as the three magnetic components of the at least three magnetic landmarks on the three axes of the three-dimensional coordinates.

3. A localization and attitude estimation system using magnetic fields to localize a moving carrier, the localization and attitude estimation system comprising: at least three magnetic landmarks arbitrarily disposed around the moving carrier in three-dimensional coordinates, wherein any two of the at least three magnetic landmarks have different magnetic directions; one set of at least five tri-axes magnetic sensors disposed on the moving carrier, wherein three-dimensional coordinates of the set of at least five tri-axes magnetic sensors are not located on a same plane; and a logic operation processing unit connected to the set of at least five tri-axes magnetic sensors, wherein the set of at least five tri-axes magnetic sensors senses the magnetic fields of the at least three magnetic landmarks, and generates at least fifteen sets of magnetic field information to the logic operation processing unit, the logic operation processing unit estimates position vectors and attitude vectors of the set of at least five tri-axes magnetic sensors in a three-dimensional coordinates according to tri-axes magnetic moment vectors of the at least three magnetic landmarks sensed by each of the tri-axes magnetic sensors, when the moving carrier moves to a new position and at least one unknown magnetic landmark is disposed around the moving carrier, the logic operation processing unit estimates and records position information and magnetic moment information of the at least one unknown magnetic landmark in the three-dimensional coordinates in the logic operation processing unit according to the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors at the new position, when a distance of at least one of the three magnetic landmarks exceeds a predetermined range from the moving carrier and the at least one unknown magnetic landmark is disposed around the moving carrier, the logic operation processing unit re-estimates the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors in the three-dimensional coordinates by selecting the at least one unknown magnetic landmark, and obtaining position information and magnetic moment information of the at least one unknown magnetic landmark in the three-dimensional coordinates.

4. The localization and attitude estimation system according to claim 3, wherein the logic operation processing unit comprises a single-chip microprocessor.

5. The localization and attitude estimation system according to claim 3, wherein the logic operation processing unit uses a band pass filter to divide the magnetic fields of the at least three magnetic landmarks into three magnetic components on three axes of the three-dimensional space, and uses an extended Kalman filter having three different constant frequencies functioning as restriction items to resolve waveforms and amplitudes of the at least three magnetic landmarks, and obtain at least three sets of waveforms and amplitudes as the three magnetic components of the at least three magnetic landmarks on the three axes of the three-dimensional coordinates.

6. The localization and attitude estimation system according to claim 3, wherein each of the magnetic landmarks comprises an active variable frequency magnetic generating element.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) FIG. 1 is a flow chart showing a localization and attitude estimation method using magnetic fields according to an embodiment of this disclosure.

(2) FIG. 2 is a schematic view showing a localization and attitude estimation system using magnetic fields according to an embodiment of this disclosure.

(3) FIG. 3 is a schematic view showing that the tri-axes magnetic sensors select one unknown magnetic landmark and obtain position information and magnetic moment information of the unknown magnetic landmark in three-dimensional coordinates.

(4) FIG. 4 is a schematic view showing that the tri-axes magnetic sensors reselect other three unknown magnetic landmarks and obtain position information and magnetic moment information of the three magnetic landmarks in three-dimensional coordinates.

(5) In the following detailed description, for purposes of explanation, numerous specific details are set forth in order to provide a thorough understanding of the disclosed embodiments. It will be apparent, however, that one or more embodiments may be practiced without these specific details. In other instances, well-known structures and devices are schematically shown in order to simplify the drawing.

DETAILED DESCRIPTION

(6) The embodiments are described in detail as below, and the examples are only intended to be illustrative and not intended to limit the scope of this disclosure. In the following illustration, the same/similar symbols represent the same/similar elements. Directional terms, such as up, down, left, right, front, back and the like, mentioned in the following examples, only refer to the directions of the drawings. Therefore, the directional terms are used to describe but not to limit this disclosure.

(7) Referring to FIGS. 1 and 2, a localization and attitude estimation method using magnetic fields according to an embodiment of this disclosure includes the following steps S11 to S15 and can be used to localize a moving carrier 120. The moving carrier 120 may be, for example, an automated guided vehicle, an unmanned vehicle, an unmanned aircraft or the like. The moving carrier 120 can arbitrarily move in a three-dimensional space, and one set of at least five td-axes magnetic sensors 121 to 125 is disposed on the moving carrier 120, wherein three-dimensional coordinate points P1 to P5 of the set of at least five tri-axes magnetic sensors 121 to 125 are not located on the same plane. That is, at least one point of the three-dimensional coordinate points P1 to P5 is not co-planar with other points. Referring to FIG. 2, a localization and attitude estimation system 100 includes at least three magnetic landmarks 111 to 113, one set of at least five tri-axes magnetic sensors 121 to 125 and a logic operation processing unit 130.

(8) The number of the magnetic landmarks 111 to 113 is not restricted, and may be an arbitrary number equal to or greater than 3. The magnetic landmarks 111 to 113 may be disposed in an area around the moving carrier 120, such as the ground, walls or any point in the three-dimensional space. When the moving carrier 120 moves to a current position, at least three magnetic landmarks 111 to 113 arbitrarily disposed around the current position of the moving carrier 120 are selected as the magnetic landmarks for localization. When the moving carrier 120 moves to a new position, at least three magnetic landmarks 111 to 113 disposed around the new position of the moving carrier 120 may be selected as the magnetic landmarks for localization, and so on.

(9) The logic operation processing unit 130 may be, for example, a computer or a single-chip microprocessor disposed in the computer, or may include a computer program stored in a computer readable recording medium. In another embodiment, the logic operation processing unit 130 can be disposed on the moving carrier 120. Before the logic operation processing unit 130 receives magnetic field information, noise in the magnetic field information may be reduced through a low-pass filter, and the signal to noise ratio is increased. Then, the magnetic field information may be converted into digital magnetic field information through an analog/digital converter.

(10) Referring to FIGS. 1 and 2, in the step S11, in three-dimensional coordinates (X, Y, Z), at least three magnetic landmarks 111 to 113 arbitrarily disposed around the current position of the moving carrier 120 are selected. any two magnetic landmarks of the at least three magnetic landmarks 111 to 113 have different magnetic directions, and position vectors (relative to the original O) and attitude vectors of the at least three magnetic landmarks 111 to 113 in the three-dimensional coordinates (X, Y, Z) are unknown vectors. That is, it is unnecessary to establish the map of the magnetic landmarks 111 to 113 in advance. In this embodiment, single-magnetic-moment electronic magnet landmarks are taken as examples, and each of the magnetic landmarks 111 to 113 is, for example, an active variable frequency magnetic generating element for generating magnetic signals with different constant frequencies. Sine waves with different constant frequencies are respectively inputted to the magnetic landmarks 111 to 113 to facilitate the subsequent demagnetization.

(11) In one embodiment, the at least three magnetic landmarks 111 to 113 do not located at the same point in the three-dimensional coordinates (X, Y, Z) and are not restricted to be orthogonal to one another. That is, the three magnetic landmarks 111 to 113 need not to be restricted to be orthogonal to one another and located at the same point as long as the sum of any two of magnetic field vectors is unequal to the multiple of the other one of the magnetic field vectors, so that the flexibility of the system is enhanced.

(12) Referring to FIGS. 1 and 2, in the step S12, the set of at least five tri-axes magnetic sensors 121 to 125 is used to sense magnetic fields of the at least three magnetic landmarks 111 to 113 to obtain a sum of the magnetic field vectors of the at least three magnetic landmarks 111 to 113, and the subsequent demagnetization process can be performed.

(13) In this embodiment, each of the tri-axes magnetic sensors 121 to 125 is connected to the logic operation processing unit 130, to generate at least three sets of magnetic field information to the logic operation processing unit 130, and thus at least fifteen sets of magnetic field information may be provided to the logic operation processing unit 130. In order to facilitate calculation of magnetic field components of the at least three magnetic landmarks 111 to 113, magnetic fields of the three magnetic landmarks 111 to 113 may be separated using a demagnetization method, wherein each of the magnetic sensors obtains the sum of the magnetic field vectors of the three magnetic landmarks 111 to 113, and the sum of the magnetic field vectors B.sub.S1, B.sub.S2, . . . , B.sub.SN is expressed as follows, N is an integer equal to 5 or greater than 5:

(14) $\hspace{1em}\begin{array}{cc}\begin{array}{c}{B}_{S1}={\left[\begin{array}{ccc}{B}_{S1x}& B_{S1y}& B_{S1z}\end{array}\right]}^T,\\ B_{S2}={\left[\begin{array}{ccc}{B}_{S2x}& B_{S2y}& B_{S2z}\end{array}\right]}^T,\\ \mathrm{.Math.}\\ B_{SN}={\left[\begin{array}{ccc}{B}_{S\mathrm{Nx}}& B_{S\mathrm{Ny}}& B_{S\mathrm{Nz}}\end{array}\right]}^T\end{array}& \left(1\right)\end{array}$

(15) Next, the logic operation processing unit 130 may use the demagnetization method of a band pass filter to obtain three magnetic components of magnetic field vectors having different constant frequencies on three axes of the current position of each of the tri-axes magnetic sensors 121 to 125, which are respectively expressed as B.sub.S1′, B.sub.S2′, . . . , B.sub.SN′, where

(16) $\hspace{1em}\begin{array}{cc}\begin{array}{c}{B}_{S1}^{\prime }=\underset{i=1}{\overset{\mathrm{NL}}{.Math.}}{B}_{S1\mathrm{Li}}^{\prime },\\ \hspace{1em}{B}_{S2}^{\prime }=\underset{i=1}{\overset{\mathrm{NL}}{.Math.}}{B}_{S2\mathrm{Li}}^{\prime },\\ \mathrm{.Math.}\\ B_{SN}^{\prime }=\underset{i=1}{\overset{\mathrm{NL}}{.Math.}}{B}_{S\mathrm{NLi}}^{\prime }\end{array}& \left(2\right)\end{array}$

(17) The demagnetization method includes using an extended Kalman filter having three different constant frequencies functioning as restriction items to resolve waveforms and amplitudes of the at least three magnetic landmarks 111 to 1113 and obtain at least three sets of waveforms and amplitudes as the three magnetic components of the at least three magnetic landmarks 111 to 113 on the three axes of the three-dimensional coordinates (X, Y, Z). Next, in the step S13, for the same magnetic landmark, the position vector and the attitude vector of each of the magnetic landmarks 111 to 113 may be obtained through the following steps.

(18) The magnetic field of each of the magnetic landmarks is known as

(19) $\begin{array}{cc}B=B_T\left(\frac{3\left(H.Math.X\right)X}{R^5}-\frac{H}{R^3}\right)& \left(3\right)\end{array}$
where X=P−O.sub.B, R=|X|, |H|=1, P is a position of a single tri-axes magnetic sensor, O.sub.B is a position of a single magnetic landmark, H is a magnetic moment unit vector of the single magnetic landmark, X is a position vector of the single tri-axes magnetic sensor relative to each of the magnetic landmarks, and R is an absolute value of the position vector X (i.e., a distance). In FIG. 2, the tri-axes magnetic sensors 121 to 125 are respectively placed on the X axis, the Y axis, the Z axis and the original O of a sensor coordinate system, and the distance between each of the tri-axes magnetic sensors 121 to 125 and the original O is known (this is the pervious speculated result). So, in the Equation (3), P and B are known, but O.sub.B, H and B.sub.T are unknown.

(20) In addition, the magnetic field of each of the magnetic landmarks 111 to 113 is a closed curve, and the magnetic force lines are not interlaced. So, a magnetic field vector and a magnetic moment and a displacement vector of each point on each magnetic force line are disposed on the same plane, and an inner product space thereof is null, that is, B×X.Math.H=0. According to the above-mentioned X=P−O.sub.B, B×P.Math.H+H×O.sub.B.Math.B=0 may be obtained. Rearrange the above results can obtain

(21) $\left[\begin{array}{cc}{B}^T& {\left(B\times P\right)}^T\end{array}\right]\left[\begin{array}{c}H\times O_B\\ H\end{array}\right]=0.$

(22) For all tri-axes magnetic sensors 121 to 125, at least five non-linear magnetic equations may be obtained as follows according to the three magnetic components on the three axes of the current position of each of the tri-axes magnetic sensors 121 to 125,

(23) $\hspace{1em}\left[\begin{array}{cc}{B}_{s1\mathrm{Li}}^T& {\left(B_{s1\mathrm{Li}}\times P_1\right)}^T\\ B_{s2\mathrm{Li}}^T& {\left(B_{s2\mathrm{Li}}\times P_2\right)}^T\\ \mathrm{.Math.}& \mathrm{.Math.}\\ B_{s\mathrm{NLi}}^T& {\left(B_{s\mathrm{NLi}}\times P_N\right)}^T\end{array}\right]\left[\begin{array}{c}{H}_{\mathrm{Li}}\times O_{\mathrm{BLi}}\\ H_{\mathrm{Li}}\end{array}\right]=0$

(24) According to the above-mentioned relationship, it is obtained that

(25) $\left[\begin{array}{c}{H}_{\mathrm{Li}}\times O_{\mathrm{BLi}}\\ H_{\mathrm{Li}}\end{array}\right]\in {1\times 6}^{},$
the above-mentioned five non-linear magnetic equations are solved according to magnetic moment vectors of the at least three magnetic landmarks 111 to 113 in a null space, and the unit vector of the null space may be determined. The calculation process is as follows. Firstly, let the null space be

(26) $V=\left[\begin{array}{c}{H}_{\mathrm{Li}}\times O_{\mathrm{BLi}}\\ H_{\mathrm{Li}}\end{array}\right],$
then H.sub.Li=V(4:6). Let r=H.sub.Li×O.sub.BLi=V(1:3), and O.sub.BLi=r×H.sub.Li+tH.sub.Li, and substitute O.sub.BLi back to Equation (3), so that the parameter t can be solved, and O.sub.BLi is obtained. B.sub.T is determined according to O.sub.BLi and H.sub.Li. If B.sub.T is negative, then H.sub.Li is multiplied by a minus sign.

(27) According to the above-mentioned results, the logic operation processing unit 130 may obtain the position information and the magnetic moment information (i.e., O.sub.BLi and H.sub.Li) of the at least three magnetic landmarks 111 to 113 in the three-dimensional coordinates (X, Y, Z). As shown in FIG. 2, the position information and the magnetic moment information of the magnetic landmark 111 are O.sub.BL1 and H.sub.L1, the position information and the magnetic moment information of the magnetic landmark 112 are O.sub.BL2 and H.sub.L2, the position information and the magnetic moment information of the magnetic landmark 113 are O.sub.BL3 and H.sub.L3.

(28) Next, in the step S14, after the position information and the magnetic moment information of the at least three magnetic landmarks 111 to 113 are obtained, the position vector of each of the tri-axes magnetic sensors 121 to 125 relative to the each of the magnetic landmarks 111 to 113 can be obtained according to Equation (1), and the attitude vector of each of the tri-axes magnetic sensors 121 to 125 in the three-dimensional coordinates (X, Y, Z) can be estimated according to the tri-axes magnetic moment vector of each of the three magnetic landmarks 111 to 113 sensed by each of the tri-axes magnetic sensors 121 to 125.

(29) As shown in FIG. 2, where {right arrow over (X.sub.P1BL1)} represents the position vector of the tri-axes magnetic sensor 121 relative to the magnetic landmark 111, {right arrow over (X.sub.P1BL2)} represents the position vector of the tri-axes magnetic sensor 121 relative to the magnetic landmark 112, and {right arrow over (X.sub.P1BL3)} represents the position vector of the tri-axes magnetic sensor 121 relative to the magnetic landmark 113. The position vectors of other magnetic sensors 122 to 125 are similar will not be depicted again.

(30) Next, in the step S15, an magnetic moment pointing matrix of the current magnetic landmark is [H.sub.L1 H.sub.L2 . . . H.sub.LN]=H.sub.L. If an initial state matrix is known as H.sub.0, then the current rotation matrix relative to the initial state is R=(H.sub.0H.sub.L.sup.T(H.sub.LH.sub.L.sup.T).sup.−1). Any current position of the magnetic landmark is o.sub.B. If the initial position of the magnetic landmark is known as o.sub.B.sub.0, then the original position of the current sensor array system in the initial coordinates is P.sub.Global=R.sup.T(−o.sub.B)+o.sub.B.sub.0.

(31) After P.sub.Global and R are obtained, the magnetic moment information H.sub.L and the position information O.sub.B of a unknown magnetic landmark relative to H.sub.L0 and O.sub.B0 of the initial coordinate system can be calculated according to the following equations, and the magnetic moment information H.sub.L and position information O.sub.B can be recorded, where
O.sub.B0=R.sup.TO.sub.B+P.sub.Global
H.sub.L0=R.sup.TH.sub.L

(32) That is, in the step S15, when the moving carrier 120 moves to a new position A (see FIG. 3) and an unknown magnetic landmark 114 is disposed around the moving carrier 120, the logic operation processing unit 130 may estimate the position information and the magnetic moment information of the unknown magnetic landmark 114 in the three-dimensional coordinates according to the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors 121 to 125 at the new position A in the three-dimensional coordinates (X, Y, Z).

(33) Referring to FIG. 3, because magnetic fields generated by the magnetic landmarks descend with the sensing distance, when the distance of at least one of the magnetic landmarks exceeds a predetermined range, the tri-axes magnetic sensors 121 to 125 cannot sense changes in the magnetic field of the at least one of the magnetic landmarks. Therefore, the localization and attitude estimation system 100 of this embodiment can estimate the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors 121 to 125 in the three-dimensional coordinates by selecting an unknown new magnetic landmark 114, and obtaining position information and magnetic moment information of the new magnetic landmark 114 in the three-dimensional coordinates by way of calculation.

(34) Alternatively, in FIG. 4, when the moving carrier 120 moves to a new position B, other three unknown magnetic landmarks 114, 115 and 116 may be selected, and position information and magnetic moment information of the three magnetic landmarks 114 to 116 in the three-dimensional coordinates are obtained through calculations of the above process steps S11 to S14 to estimate the position vectors and the attitude vectors of the set of at least five tri-axes magnetic sensors 121 to 125 in the three-dimensional coordinates.

(35) The localization and attitude estimation method using magnetic fields and the system thereof disclosed by the above-mentioned embodiment of this disclosure may be used to detect the position and the attitude of the moving carrier (such as the unmanned vehicle or the arbitrary object) in the space, and the position of the magnetic landmark may be arbitrarily placed without establishing the map in advance to reduce the data quantity and the computation amount for the map. Therefore, this disclosure does not require site layout of the environment in advance, so that the localization system is more flexible and the localization range is wider, and the mobility and convenience of the site layout are possessed.

(36) It will be apparent to those skilled in the art that various modifications and variations can be made to the disclosed embodiments. It is intended that the specification and examples be considered as exemplary only, with a true scope of the disclosure being indicated by the following claims and their equivalents.