CN114264292B - Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass - Google Patents
Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass Download PDFInfo
- Publication number
- CN114264292B CN114264292B CN202111528083.8A CN202111528083A CN114264292B CN 114264292 B CN114264292 B CN 114264292B CN 202111528083 A CN202111528083 A CN 202111528083A CN 114264292 B CN114264292 B CN 114264292B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- accelerometer
- vector
- gnss
- sun sensor
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 239000013598 vector Substances 0.000 claims abstract description 40
- 238000005259 measurement Methods 0.000 claims abstract description 20
- 239000011159 matrix material Substances 0.000 claims abstract description 17
- 230000005484 gravity Effects 0.000 claims abstract description 4
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 238000010606 normalization Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000009434 installation Methods 0.000 claims description 2
- 230000007547 defect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
The invention provides a posture determining method and a digital compass based on an accelerometer, a sun sensor and a GNSS, wherein the posture determining method comprises the following steps: calculating a measurement vector a of an accelerometer relative to a carrier coordinate system b Measurement vector S of sun sensor relative to carrier coordinate system b Theoretical vector S of solar vector in North west sky coordinate system g The method comprises the steps of carrying out a first treatment on the surface of the According to a b 、S g 、S b And the theoretical vector a of the gravity vector in the North-west-day coordinate system g Calculating an attitude matrix of a carrier coordinate system relative to a North-west-sky coordinate system by using a double-vector attitude determination algorithmAccording to the gesture matrixAnd solving a yaw angle, a pitch angle and a roll angle by using an attitude angle solving formula. The north-seeking precision is improved to 0.1 degrees, a geomagnetic method is completely abandoned, the problem that the environment disturbs geomagnetic disturbance so as to influence the orientation and attitude-determining precision is avoided, and meanwhile, the advantages of miniaturization, light weight and convenience of the digital compass are inherited.
Description
Technical Field
The invention relates to the technical field of north-seeking measurement and digital compass, in particular to a gesture determining method based on an accelerometer, a sun sensor and a GNSS and a digital compass.
Background
The digital compass is a north seeker, overcomes the defects of complex structure, large volume and high price of the traditional mechanical compass, is widely applied as gesture and heading measuring equipment, and has wide application in the fields of vehicle navigation, robots, unmanned aerial vehicles, radars, aviation, navigation and the like.
The traditional electronic compass/digital compass is based on a fluxgate sensor or a magnetic resistance sensor and a double-shaft inclination angle sensor, and an included angle between the sensor and magnetic north is calculated through a built-in microprocessor. In general, the geomagnetic principle is measured by using a magneto-resistance sensor, the heading measurement accuracy is about 0.5-1 degrees, the anti-interference capability is poor, in the practical application process, an interference magnetic field is superposed on the geomagnetic field, the size and the direction of the magnetic field can be changed, the magneto-resistance sensor can be interfered, and a large measurement error is generated.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides the attitude determination method based on the accelerometer, the sun sensor and the GNSS and the digital compass, wherein the north-seeking precision is improved to 0.1 degrees, the geomagnetic method is completely abandoned, the problem that the environment disturbs geomagnetic disturbance so as to influence the orientation attitude determination precision is avoided, and meanwhile, the advantages of miniaturization, light weight and convenience of the digital compass are inherited.
In order to achieve the object of the invention, the following scheme is adopted:
the attitude determination method based on the accelerometer, the sun sensor and the GNSS comprises the following steps:
calculating a measurement vector a of an accelerometer relative to a carrier coordinate system b ；
Calculating a measurement vector S of the sun sensor relative to the carrier coordinate system b ；
Calculating solar vectorTheoretical vector S of quantity in North western day coordinate system g The method comprises the following steps: calculating parameter R of solar vector under inertial system i Conversion array for calculating inertial system to ground systemTransformation matrix of computing ground fixation system to North west sky coordinate system>According to the formula->Obtaining S g ；
According to a b 、S g 、S b And the theoretical vector a of the gravity vector in the North-west-day coordinate system g Calculating an attitude matrix of a carrier coordinate system relative to a North-west-sky coordinate system by using a double-vector attitude determination algorithmWherein a is g Is [0, 1 ]]’；
According to the gesture matrixAnd solving a yaw angle, a pitch angle and a roll angle by using an attitude angle solving formula.
The digital compass based on the accelerometer, the sun sensor and the GNSS is used for realizing the attitude determination method, and comprises the following components: the solar sensor, the triaxial accelerometer and the processor are arranged on the same carrier, the processor is connected with the solar sensor and the triaxial accelerometer, and the processor is also used for receiving GNSS signals input from outside.
The invention has the beneficial effects that:
1. different from the existing electronic compass based on the principle of north seeking by a magnetic sensor, the invention based on north seeking by a sun sensor ensures that the north seeking precision is improved from 0.5 degree of magnetic north seeking to 0.1 degree of solar north seeking, is suitable for the attitude measurement in open air and has no special requirement on the use working condition environment of a carrier;
2. the output azimuth data has high speed and no waiting time; the stability of output data is good, the output data does not drift for a long time, the influence of the handheld stability of the instrument is small, and the output attitude angle basically has no fluctuation;
3. is a north-seeking scheme with miniaturization, light weight and low power consumption. The method has valuable application in the aspects of north seeking of individual equipment, determination of the heading of a micro light aircraft and an unmanned aerial vehicle, field orientation and the like.
Drawings
Fig. 1 is a schematic flow chart of a gesture determining method according to an embodiment of the present application.
FIG. 2 is a theoretical vector S of a calculated solar vector in the North-west-day coordinate system according to an embodiment of the present application g Is a flow diagram of (a).
Fig. 3 is a block diagram of a digital compass according to an embodiment of the present application.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be described in further detail with reference to the accompanying drawings. It should be noted that in the detailed description that follows, the definition of the sequence of method steps should be limited to the specific examples listed, under certain implementation conditions, at least some of which, in the case of sequential replacement, do not affect implementation.
Coordinate system definition:
ground fixation (Fe): namely an earth fixedly-connected coordinate system/a geodetic coordinate system/an 84 coordinate system, wherein the Xe axis points to a zero-degree meridian in the equatorial plane, the Ze axis points to the direction of the north pole of the earth, and the Ye axis, the Xe axis and the Ze axis form a right-hand system.
The inertial system (Fi), the J2000 inertial coordinate system, has the Xi axis parallel to the epoch J2000.0 equatorial plane.
The north-west coordinate system (Fg), which is a reference coordinate system corresponding to the position of the carrier, is defined as the Xg axis pointing north, the Yg axis pointing west, and the Zg axis pointing the day.
Carrier coordinate system (Fb): the Xb axis points forward, the Yb axis points to the left, and the Zb axis points upward.
The embodiment of the application provides a digital compass based on an accelerometer, a sun sensor and a GNSS and a gesture determination method. Specifically, as shown in fig. 3, the digital compass includes: the sensor, the triaxial accelerometer and the processor/microprocessor circuit are mounted on the same carrier, and the GNSS information is externally input to the carrier and transmitted to the processor. The processor is connected with the sun sensor and the triaxial accelerometer. Specifically, the triaxial accelerometer is additionally arranged inside the sun sensor. The processor is also integrated within the sun sensor. The processor adopts a singlechip and/or a programmable logic device.
The sun sensor is an APS digital sun sensor and is used for measuring the sun vector double-axis angle. The accelerometer is used to measure the acceleration data of the carrier. The processor is used for inputting GNSS signals through the outside, and the current time and longitude W, latitude L and altitude h information of the current time. The processor is also used for carrying out relevant calculation implemented by the attitude determination method. Specifically, according to the attitude determination method, the yaw angle, the pitch angle and the roll angle are obtained by calculating through a processor according to data acquired/measured by each device.
As shown in fig. 1, the gesture determining method in the embodiment of the present application includes the following steps:
s100, calculating a measurement vector a of the accelerometer relative to a carrier coordinate system b 。
According to the measurement data of the triaxial accelerometer as a1, a2 and a3 and the installation vectors of the accelerometer head under the accelerometer coordinate system as VA 1, VA 2 and VA 3, the three-axis accelerometer is calculated by the following conversion formula:
and then carrying out normalization treatment:
wherein,for accelerometersA transformation matrix of the coordinate system to the carrier coordinate system.
S200, calculating a measurement vector S of the sun sensor relative to a carrier coordinate system b 。
The two angles of the sun measured by the sun sensor are respectively represented by alpha and beta in a digital sun sensor coordinate system, and a conversion matrix from the sun sensor coordinate system to a carrier coordinate system is as followsMeasurement vector S of sun sensor relative to carrier coordinate system b The method is calculated by the following formula:
S s1 ＝-tana*S s3
S s2 ＝-tanβ*S s3
s300, calculating a theoretical vector S of the solar vector in a North western day coordinate system g . As shown in fig. 2, the method specifically comprises the following steps:
s310, calculating the parameter R of the solar vector under the inertia system i Calculated by the following formula:
a＝149598023280.828
e＝0.01670862-0.00004204*T c -0.00000124*T c 2
i＝0.42909280228307-0.0002269655829*T c -0.00000000279*T c 2
ω＝4.9381882820077+0.00562976999*T c -0.00000275*T c 2
M＝mod(6.24005996669206+628.3019551321268*T c ，2*pi)
u＝ω+f
obtaining R i And then carrying out normalization treatment on the obtained product:
wherein t represents the time of the current moment relative to the preset time, the unit is seconds, the time of the current moment is obtained through an externally input GNSS signal, and the value of omega is 0.
S320, calculating a conversion array from an inertial system to a ground systemCalculated by the following formula:
a 1 ＝0.01118*T c
a 2 ＝0.00971717*T c
b1＝2.18243862-33.75704593*T c
b2＝-2.77625791+1256.66393243*T c
d 1 ＝-8.3386*10 -5 *sin(b1)-6.393*10 -6 *sin(b2)
d 2 ＝4.4615*10 -5 *cos(b1)+2.781*10 -6 *cos(b2)
d 3 ＝0.91747721*d 1
λ g ＝(280.4606184+360.9856122863*t/86400)*π/180
λ＝mod(λ g ,2*π)
s330, transforming array from computing ground fixed system to North western day coordinate system
Firstly, acquiring precision W, latitude L and height h information of the current moment through externally input GNSS signals, and transforming an earth system into a North western day coordinate systemThe calculation adopts the following formula:
x＝(R+h)*cosW*cosL
y＝(R+h)*cosW*sinL
z＝(R+h)*sinL
r＝(x,y,z)
Z e ＝[0 0 1] T
Y c ＝Z c ×Z e
X c ＝Y c ×Z c
s340, according to the formulaObtaining S g The method comprises the steps of carrying out a first treatment on the surface of the Then for the calculated S g Normalization is carried out:
s400, according to a b 、S g 、S b And the theoretical vector a of the gravity vector in the North-west-day coordinate system g Calculating an attitude matrix of a carrier coordinate system relative to a North-west-sky coordinate system by using a double-vector attitude determination algorithmWherein a is g Is [0, 1 ]]'A'; gesture matrixCalculated by the following formula:
V 2i ＝a g ×S g
V 3i ＝a g ×V 2i
V 2b ＝a b ×S b
V 3b ＝a b ×V 2b
s500, according to the gesture matrixSolving a yaw angle, a pitch angle and a roll angle:
yaw angle
Pitch angle
Roll angle
According to the scheme, the existing nano digital sun sensor (small in size, light in weight and low in power consumption) is internally provided with the triaxial accelerometer by utilizing an aerospace mature product, and under the condition that GNSS signals are externally input, the attitude of the carrier under a reference system is determined by a certain algorithm in the micro processing circuit/processor. Compared with the magnetic measurement scheme, the north-seeking precision is remarkably improved, the magnetic measurement scheme can be replaced under certain conditions, and the magnetic measurement scheme has the advantages of light weight, low power consumption and strong applicability to a field mobile handheld environment.
The foregoing is merely a preferred embodiment of the present invention and is not meant to be the only or limiting of the present invention. It will be understood by those skilled in the art that various changes may be made and equivalents may be substituted for elements thereof without departing from the scope of the invention.
Claims (8)
1. The attitude determination method based on the accelerometer, the sun sensor and the GNSS is characterized by comprising the following steps:
calculating a measurement vector a of an accelerometer relative to a carrier coordinate system b ；
Calculating a measurement vector S of the sun sensor relative to the carrier coordinate system b ；
Calculating a theoretical vector S of a solar vector in a North western day coordinate system g The method comprises the following steps: calculating parameter R of solar vector under inertial system i Conversion array for calculating inertial system to ground systemTransformation matrix of computing ground fixation system to North west sky coordinate system>According to the formula->Obtaining S g ；
According to a b 、S g 、S b And the theoretical vector a of the gravity vector in the North-west-day coordinate system g Calculating an attitude matrix of a carrier coordinate system relative to a North-west-sky coordinate system by using a double-vector attitude determination algorithmWherein a is g Is [0, 1 ]]'A'; gesture matrix->Calculated by the following formula:
according to the gesture matrixSolving a yaw angle, a pitch angle and a roll angle by using an attitude angle solving formula:
yaw angle:
pitch angle:
roll angle:
2. the method of attitude determination based on accelerometers, sun sensors and GNSS according to claim 1, characterized in that the measurement vector a of the accelerometer with respect to the carrier coordinate system is calculated b The method comprises the following steps: according to the measurement data of the triaxial accelerometer as a1, a2 and a3 and the installation vectors of the accelerometer head under the accelerometer coordinate system as VA 1, VA 2 and VA 3, the three-axis accelerometer is calculated by the following conversion formula:
and then carrying out normalization treatment:
wherein,is a transformation matrix of the accelerometer coordinate system to the carrier coordinate system.
3. The method for determining the attitude based on an accelerometer, a sun sensor and a GNSS according to claim 1, wherein the sun sensor is an APS digital sun sensor for measuring the sun vector double-axis angle, the two angles of the sun measured by the sun sensor are respectively represented by alpha and beta in a digital sun sensor coordinate system, and a conversion matrix from the sun sensor coordinate system to a carrier coordinate system isMeasurement vector S of sun sensor relative to carrier coordinate system b The method is calculated by the following formula:
S s1 ＝-tana*S s3
S s2 ＝-tanβ*S s3
4. the method for determining the attitude based on an accelerometer, a sun sensor and a GNSS according to claim 1, wherein the parameter R of the sun vector in the inertial frame i Calculated by the following formula:
a＝149598023280.828
e＝0.01670862-0.00004204*T c -0.00000124*T c 2
i＝0.42909280228307-0.0002269655829*T c -0.00000000279*T c 2
ω＝4.9381882820077+0.00562976999*T c -0.00000275*T c 2
M＝mod(6.24005996669206+628.3019551321268*T c ，2*pi)
u＝ω+f
obtaining R i And then carrying out normalization treatment on the obtained product:
wherein t represents the time of the current moment relative to the preset time, the unit is seconds, the time of the current moment is obtained through GNSS signals, and the value of omega in the formula is 0.
5. The method of attitude determination based on accelerometers, sun sensors and GNSS according to claim 1, characterized in that the inertial to ground system transformation matrixCalculated by the following formula:
a 1 ＝0.01118*T c
a 2 ＝0.00971717*T c
b1＝2.18243862-33.75704593*T c
b2＝-2.77625791+1256.66393243*T c
d 1 ＝-8.3386*10 -5 *sin(b1)-6.393*10 -6 *sin(b2)
d 2 ＝4.4615*10 -5 *cos(b1)+2.781*10 -6 *cos(b2)
d 3 ＝0.91747721*d 1
λ g ＝(280.4606184+360.9856122863*t/86400)*π/180
λ＝mod(λ g ,2*π)
wherein t represents the time of the current moment relative to the preset time, the unit is seconds, and the time of the current moment is obtained through GNSS signals.
6. The method of accelerometer, sun sensor and GNSS based pose determination according to claim 1, wherein the transform matrix is computationally fixed to the north west day coordinate systemWhen the method is used, firstly, longitude W, latitude L and altitude h information of the current moment are acquired through GNSS signals, and the following formula is adopted for calculation:
x＝(R+h)*cosW*cosL
y＝(R+h)*cosW*sinL
z＝(R+h)*sinL
r＝(x,y,z)
Z e ＝[0 0 1] T
Y c ＝Z c ×Z e
X c ＝Y c ×Z c
7. the method for determining the pose based on accelerometer, sun sensor and GNSS according to claim 1, characterized in that S is calculated g When it is also toCalculated S g Normalization is carried out:
8. digital compass based on accelerometer, sun sensor and GNSS for implementing the method of pose determination according to any of claims 1 to 7, characterized in that it comprises: the solar sensor, the triaxial accelerometer and the processor are arranged on the same carrier, the processor is connected with the solar sensor and the triaxial accelerometer, and the processor is also used for receiving GNSS signals input from outside.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111528083.8A CN114264292B (en) | 2021-12-14 | 2021-12-14 | Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111528083.8A CN114264292B (en) | 2021-12-14 | 2021-12-14 | Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114264292A CN114264292A (en) | 2022-04-01 |
| CN114264292B true CN114264292B (en) | 2024-04-02 |
Family
ID=80827091
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111528083.8A Active CN114264292B (en) | 2021-12-14 | 2021-12-14 | Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114264292B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111307139A (en) * | 2019-12-09 | 2020-06-19 | 北京航空航天大学 | Course and attitude determination method based on polarization/astronomical information fusion |
| CN112066979A (en) * | 2020-08-27 | 2020-12-11 | 北京航空航天大学 | Polarization pose information coupling iteration autonomous navigation positioning method |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9829582B2 (en) * | 2011-09-19 | 2017-11-28 | Raytheon Company | Method and apparatus for differential global positioning system (DGPS)-based real time attitude determination (RTAD) |
| US20150219767A1 (en) * | 2014-02-03 | 2015-08-06 | Board Of Regents, The University Of Texas System | System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates |
-
2021
- 2021-12-14 CN CN202111528083.8A patent/CN114264292B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111307139A (en) * | 2019-12-09 | 2020-06-19 | 北京航空航天大学 | Course and attitude determination method based on polarization/astronomical information fusion |
| CN112066979A (en) * | 2020-08-27 | 2020-12-11 | 北京航空航天大学 | Polarization pose information coupling iteration autonomous navigation positioning method |
Non-Patent Citations (2)
| Title |
|---|
| Optimization-based Attitude Determination in Geodetic Applications;Alexander Pesterev;Ivan Matrosov;Yury Morozov;《Open Computer Science》;第10卷(第1期);270-275 * |
| 基于MEMS AHRS/GNSS 的全组合导航;毛红瑛, 陈至坤;《网络安全技术与应用》;128-129 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114264292A (en) | 2022-04-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3848941B2 (en) | Geomagnetic sensor attitude error compensation apparatus and method | |
| CN107314718B (en) | High speed rotation bullet Attitude estimation method based on magnetic survey rolling angular rate information | |
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| Wu et al. | Fast complementary filter for attitude estimation using low-cost MARG sensors | |
| CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
| CN107270893B (en) | Lever arm and time asynchronous error estimation and compensation method for real estate measurement | |
| US8577595B2 (en) | Location and path-map generation data acquisition and analysis systems | |
| CN110007354B (en) | Device and method for measuring flight parameters of semi-aviation transient electromagnetic receiving coil of unmanned aerial vehicle | |
| CN109556631B (en) | INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares | |
| CN106767804A (en) | The multidimensional data measurement apparatus and method of a kind of moving object | |
| CN112197765B (en) | Method for realizing fine navigation of underwater robot | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN111189474A (en) | Autonomous calibration method of MARG sensor based on MEMS | |
| CN110779514B (en) | Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation | |
| CN108444468B (en) | Directional compass integrating downward vision and inertial navigation information | |
| CN112525203A (en) | Spacecraft autonomous astronomical navigation method based on angle constraint auxiliary measurement | |
| De Alteriis et al. | Accurate attitude inizialization procedure based on MEMS IMU and magnetometer integration | |
| Li et al. | Unmanned aerial vehicle position estimation augmentation using optical flow sensor | |
| CN114264292B (en) | Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass | |
| CN110954080A (en) | Magnetic compass calibration method for eliminating carrier magnetic interference | |
| CN110954081A (en) | Quick calibration device and method for magnetic compass | |
| CN111649738B (en) | Method for calculating initial attitude of accelerometer under microgravity field | |
| CN206479651U (en) | A kind of air-ground TEM reception systems with posture writing function | |
| CN113237485A (en) | SLAM method and system based on multi-sensor fusion | |
| Tripathi et al. | Design considerations of orientation estimation system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |