AU2021104827A4 - Digital system and control method for attitude compensation and wave measurement - Google Patents
Digital system and control method for attitude compensation and wave measurement Download PDFInfo
- Publication number
- AU2021104827A4 AU2021104827A4 AU2021104827A AU2021104827A AU2021104827A4 AU 2021104827 A4 AU2021104827 A4 AU 2021104827A4 AU 2021104827 A AU2021104827 A AU 2021104827A AU 2021104827 A AU2021104827 A AU 2021104827A AU 2021104827 A4 AU2021104827 A4 AU 2021104827A4
- Authority
- AU
- Australia
- Prior art keywords
- attitude
- unit
- wave
- data
- calculation
- 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
- 238000005259 measurement Methods 0.000 title claims abstract description 47
- 238000000034 method Methods 0.000 title claims abstract description 26
- 230000001133 acceleration Effects 0.000 claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims abstract description 19
- 238000006243 chemical reaction Methods 0.000 claims abstract description 12
- 238000004891 communication Methods 0.000 claims abstract description 8
- 230000008859 change Effects 0.000 claims abstract description 5
- 230000005484 gravity Effects 0.000 claims abstract description 5
- 238000004364 calculation method Methods 0.000 claims description 41
- 238000005070 sampling Methods 0.000 claims description 28
- 230000010354 integration Effects 0.000 claims description 15
- 230000002452 interceptive effect Effects 0.000 claims description 10
- 238000006073 displacement reaction Methods 0.000 claims description 9
- 238000010606 normalization Methods 0.000 claims description 9
- 238000011156 evaluation Methods 0.000 claims description 6
- 230000008569 process Effects 0.000 claims description 4
- 238000003491 array Methods 0.000 claims description 3
- 230000003993 interaction Effects 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 abstract description 7
- 230000006870 function Effects 0.000 abstract description 6
- 230000000712 assembly Effects 0.000 abstract description 4
- 238000000429 assembly Methods 0.000 abstract description 4
- 238000009434 installation Methods 0.000 abstract description 4
- 230000006872 improvement Effects 0.000 abstract description 3
- 230000033001 locomotion Effects 0.000 description 4
- 239000000203 mixture Substances 0.000 description 3
- 238000001914 filtration Methods 0.000 description 2
- 108010079923 lambda Spi-1 Proteins 0.000 description 2
- 230000000717 retained effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000003990 capacitor Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
- G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C13/00—Surveying specially adapted to open water, e.g. sea, lake, river or canal
- G01C13/002—Measuring the movement of open water
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/02—Magnetic compasses
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
- G01C17/38—Testing, calibrating, or compensating of compasses
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C9/00—Measuring inclination, e.g. by clinometers, by levels
- G01C9/02—Details
- G01C9/06—Electric or photoelectric indication or reading means
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C9/00—Measuring inclination, e.g. by clinometers, by levels
- G01C9/02—Details
- G01C9/08—Means for compensating acceleration forces due to movement of instrument
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B11/00—Automatic controllers
- G05B11/01—Automatic controllers electric
- G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
- G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B6/00—Internal feedback arrangements for obtaining particular characteristics, e.g. proportional, integral or differential
- G05B6/02—Internal feedback arrangements for obtaining particular characteristics, e.g. proportional, integral or differential electric
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Life Sciences & Earth Sciences (AREA)
- Hydrology & Water Resources (AREA)
- Gyroscopes (AREA)
Abstract
Disclosed are a digital system and a control method for attitude compensation and wave
measurement. The digital system comprises a control unit, and further comprises a 6-axis MEMS
inertial navigation unit, a 3-axis magnetometer unit, a clock unit, a storage unit, a level
5 conversion unit for serial communication, and a power supply unit, which are connected to the
control unit. The invention enables to carry out three frequently required functions, including
attitude measurement, attitude compensation, and wave measurement, by the same assembly,
which provides improvement in multifunction performance of marine measurement equipment.
By providing the corresponding units in the system, the present invention solves the prior art
0 problems including the complicated steps of attitude measurement and attitude compensation,
and thereby ensures the timely and accurate corrections of attitude of on-site observation on
sway platforms. Also, the digital-type system makes it possible to avoid the problem of large
sizes and inconvenient installation of mechanical gimbal assemblies, effectively reducing the
cost, size, and weight of marine equipment.
3/5
Start
putNum= ? No Gyroscope
Parameter 10m period compensation based
initialization ends ? on PI
Yes ''Solve
Read ADIS16485 data quaternion
Read RM3100 data differential
Hardware
equation
initialization Define Kp 'omaia'o
of quaternion
Acceleration data
initialization
Calculate
Ir attitude angle
Main program Estimate direction and
while(l) change of gravity
attitude
compensation
Calculate the matrix
error
Calculate
Integrate acceleration
the error
FIG 3
Description
3/5
Start
putNum= ? No Gyroscope Parameter 10m period compensation based ends ? on PI initialization
Yes ''Solve Read ADIS16485 data quaternion Read RM3100 data differential equation Hardware initialization Define Kp 'omaia'o of quaternion Acceleration data initialization Calculate Ir attitude angle Main program Estimate direction and while(l) change of gravity
attitude compensation Calculate the matrix error
Calculate Integrate acceleration the error
FIG 3
The present invention relates to the field of marine observations, and particularly relates to a digital system and a control method for attitude compensation and wave measurement.
Unmanned marine observation platforms, such as research vessels, unmanned vessels, buoys, and gliders that usually operate on the sea for a long period of time, are inevitably affected by the marine dynamic environment, changing frequently in their trajectory and attitude with motions including rotation, fluctuation, and swing. When the attitude of an unmanned platform changes drastically, it will affect the marine observations by sensors, resulting in reduced accuracy and reliability of data. Therefore, it is of great significance for obtaining high-quality observation data, if we manage to timely detect motion and attitude of the platform and carry out real-time attitude compensation, so as to obtain the true vector values in the natural geographic coordinate system. For marine vector measurement carried out on a sway platform, it is conventional to utilize measurement equipment in combination with a compass to obtain data which will be analyzed later by professionals. On-site observation workers failing to provide the correct installation parameters to the professionals will result in failures in attitude compensation.
Wave observation based on inertial navigation system also requires attitude measurement and attitude compensation caused by waves. Traditional solutions for reducing shaking for wave sensors mainly involve disposing an inertial measurement unit (IMU) on a mechanical gimbal assembly which maintains in the horizontal or vertical position in a shaking environment. However, mechanical gimbal assemblies are usually large, heavy, and complicated to assemble.
In view of the above, one object of the present invention is to provide a digital system and a control method for attitude compensation and wave measurement. The invention enables to carry out attitude measurement, attitude compensation, and wave measurement by one single system, thereby solving the prior art problems including the complicated steps of attitude measurement, attitude compensation, and gimbal assembling.
In order to realize the above object, the present invention comprises the following technical solutions.
The first aspect is a digital system for attitude compensation and wave measurement, which comprises a control unit, and further comprises a 6-axis MEMS (micro-electro-mechanical system) inertial navigation unit, a 3-axis magnetometer unit, a real-time clock unit, a storage unit, a TTL to RS232 converter unit for serial communication, and a power supply unit, which are connected to the control unit, wherein,
the control unit is configured to process data received from the other units, and send control commands to the other units;
the 6-axis MEMS inertial navigation unit is configured to obtain angular motion information and acceleration information;
the 3-axis magnetometer unit is configured to obtain geomagnetic information;
the real-time clock unit is configured to generate clock signals;
the storage unit is configured to store raw sampling data and wave measurement statistical results;
the TTL to RS232 converter unit is configured to produce standard RS232 voltage signal for serial communication; and
the power supply unit is configured to supply electric power to the control unit, the 6-axis MEMS inertial navigation unit, the 3-axis magnetometer unit, the clock unit, the storage unit, and the level conversion unit.
In some embodiments, the control unit comprises a STM32F446 microprocessor, the 6-axis MEMS inertial navigation unit comprises an ADIS16485 chip, the 3-axis magnetometer unit comprises an RM3100 chip, the real-time clock unit comprises a DS1302 chip, the storage unit comprises an SD card (Secure Digital memory card), and the level conversion unit comprises a MAX3232 chip.
In addition, the present invention also provides a control method for using the above device, comprising the following steps:
the control unit performing data interaction with a host computer through predetermined interactive commands, the interactive commands including attitude compensation mode/wave operation mode switching command, clock setting command, attitude measurement frequency setting command, and wave sampling frequency setting command; the host selecting attitude compensation mode or wave operation mode; in the attitude ) compensation mode, obtaining attitude calculating results; in the wave operation mode, obtaining wave characteristics.
5 In some embodiments, interactive command formats include header-1, header-2, command flags, user settings, and checksum, wherein, the header-i and the header-2 are each set to a fixed value, and the command flags are set to 5 values respectively corresponding to the attitude compensation mode, the wave operation mode, attitude measurement frequency setting mode, wave sampling frequency setting mode, and time setting mode.
0 In some embodiments, the attitude compensation mode comprises the following steps:
performing parameter initialization and hardware initialization, running infinite loop main program to sequential perform acceleration data collecting, magnetometer data collecting, attitude angle calculation, attitude compensation matrix calculation, and calculation result output, at a predetermined frequency.
5 In some embodiments, before the steps of attitude angle calculation and attitude compensation matrix calculation, the main program further comprises determining whether the attitude output heartbeat interval (OutputNum) is equal to a first predetermined value, and determining whether the timing value is greater than or equal to a second predetermined value; if both determination results are yes, then reading data from the 6-axis MEMS inertial navigation 0 unit and the 3-axis magnetometer unit, or otherwise, continuing the determination steps and dynamically defining the proportional coefficient Kp of PID (proportional-integral-derivative) control according to the returned gyroscope data and acceleration data.
In some embodiments, after the step of defining the proportional coefficient Kp, the main program further comprises normalization of the acceleration data, obtaining evaluation parameters of gravity direction and change, calculating an error between the evaluation parameters and the acceleration data, integrating the error, subjecting the integration result to PI (proportional-integral) control and sending to the gyroscope as compensation input, solving quaternion differential equation, normalization of quaternion, calculating the attitude angle from the normalization of quaternion, calculating the attitude compensation matrix from the attitude angle, and calculating the acceleration in the north-west-sky coordinate system from the attitude compensation matrix.
In some embodiments, the proportional coefficient Kp is defined to be 0.001 if the gyroscope in the 6-axis MEMS inertial navigation unit exhibits a rotating speed of more than 50/10 ms, or otherwise the proportional coefficient Kp is defined to be 1.
In some embodiments, the wave operation mode comprises the following steps:
step 1: performing parameter initialization and hardware initialization; step 2: performing inertial data sampling, and performing acceleration mapping from body frame to ENU coordinate; step 3: performing calculation on the wave characteristics to give calculation results, wherein the wave characteristics include wave height, wave period and wave direction, and the calculation comprises FFT operation of vertical acceleration to frequency domain; step 4: calculating the wave height and wave period using the zero crossing method, and calculating the wave direction using the triangulation method; step 5: outputting and storing the calculation results.
In some embodiments, the integral transforms comprises dynamic memory allocation, discretization of circular frequency, generating FFT (fast Fourier transform) arrays, performing FFT calculation, changing time domain representation to frequency domain representation, performing quadratic integration to obtain frequency domain displacement signal, removing the high-frequency/low-frequency cutoff frequency signals in the frequency domain, performing IFFT (inverse fast Fourier transform) calculation, and changing the frequency domain displacement signal to time domain representation.
The present invention has the following beneficial effects:
1. The invention enables to carry out three frequently required functions, including attitude measurement, attitude compensation, and wave measurement, by the same assembly, which provides improvement in multifunction performance of marine measurement equipment. By providing the corresponding units in the system, the present invention solves the prior art problems including the complicated steps of attitude measurement and attitude compensation, and thereby ensures the timely and accurate corrections of attitude of on-site observation on shaking platforms. Also, the digital-type system makes it possible to avoid the problem of large sizes and inconvenient installation of mechanical gimbal assemblies, effectively reducing the cost, size, and weight of marine equipment.
2. For low-frequency wave acceleration signals, use of frequency domain integration can effectively solve the problem of displacement trend drift caused by time domain integration. Also, the filter parameters of high-frequency/low-frequency cutoff frequency in the frequency domain provide improved self-adaptability as compared with the filtering performance of the traditional time domain integration.
FIG. 1 shows a hardware configuration of the system.
FIG. 2 shows an interactive command set of the control method.
FIG. 3 shows a flow chart of the attitude compensation mode.
FIG. 4 shows the output formats of the attitude compensation mode.
FIG. 5 shows a flow chart of the wave operation mode.
In order to better and more clearly disclose the objects, technical solutions, and advantages of the present invention, the content of the present invention will be further described in detail below with reference to the drawings and specific embodiments. It can be understood that the specific examples described herein are only used to explain the present invention, but not to limit the present invention. In addition, it should be noted that, the drawings do not illustrate all the features related to the present invention.
Example 1
As shown in FIG. 1, this example proposed a digital system for attitude compensation and wave measurement, which comprises a control unit, and further comprises a 6-axis MEMS (micro-electro-mechanical system) inertial navigation unit, a 3-axis magnetometer unit, a clock unit, a storage unit, a level conversion unit for serial communication, and a power supply unit, which are connected to the control unit, wherein,
the control unit is configured to process data received from the other units, and send control commands to the other units;
the 6-axis MEMS inertial navigation unit is configured to obtain angular motion information and acceleration information;
the 3-axis magnetometer unit is configured to obtain geomagnetic information;
the clock unit is configured to generate clock signals;
the storage unit is configured to store raw sampling data and wave measurement statistical results; the TTL to RS232 converter unit is configured to produce standard RS232 voltage signal for serial communication; and the power supply unit is configured to supply electric power to the control unit, the 6-axis MEMS inertial navigation unit, the 3-axis magnetometer unit, the clock unit, the storage unit, and the level conversion unit.
The system enables to carry out three frequently required functions, including attitude measurement, attitude compensation, and wave measurement, by the same assembly, which provides improvement in multifunction performance of marine measurement equipment. By providing the corresponding units in the system, the present invention solves the prior art problems including the complicated steps of attitude measurement and attitude compensation, and thereby ensures the timely and accurate corrections of attitude of on-site observation on shaking platforms. Also, the digital-type system makes it possible to avoid the problem of large sizes and inconvenient installation of mechanical gimbal assemblies, effectively reducing the cost, size, and weight of marine equipment.
The synergy of the above units allows one single system to carry out measurements that are used to require the use of two systems.
Example 2
As to the device of Example 1, FIG. 1 only shows the pin connections of the main devices but the arrangements of capacitors, resistors, inductors, and power supply in the devices are omitted. The control unit comprises a STM32F446 microprocessor, the 6-axis MEMS inertial navigation unit comprises an ADIS16485 chip, the 3-axis magnetometer unit comprises an RM3100 chip, the clock unit comprises a DS1302 chip, the storage unit comprises an SD card, and the level conversion unit comprises a MAX3232 chip. Furthermore, the core process used is a STM32F446RC processor (Ul), wherein the tactical-grade 6-axis inertial navigation unit ADIS16485 (U2) is connected to the SPI 1 channel of Ul through the SPI bus; specifically, Pin 3, Pin 4, Pin 5, Pin 6 and Pin 9 of the ADIS16485 are respectively connected to Pin 21, Pin 22, Pin 23, Pin 20 and Pin 24 of the STM32F446RC. The RM3100 chip (U3) of the 3-axis magnetometer unit is connected to the SPI 2 channel of Ul through the SPI bus; specifically, Pin 1, Pin 3, Pin 23, Pin 27 and Pin 28 of the RM3100 are respectively connected to Pin 36, Pin 33, Pin 37, Pin 34 and Pin 35 of U. Slots of the SD card (U4) are connected to the corresponding pins of Ul through the SDIO bus; specifically, Pin 1, Pin 2, Pin 3, Pin 5, Pin 7, Pin 8 and Pin 9 of the SD card are respectively connected to Pin 51, Pin 52, Pin 54, Pin 53, Pin 39, Pin 40 and Pin 41 of Ul. The clock chip DS1302 (U5) is connected to the STM32F446RC through the IIC bus; specifically, Pin 5, Pin 6 and Pin 7 of DS1302 are respectively connected to Pin 27, Pin 26 and Pin 25 of Ul. Pin11 and Pin 12 of the TTL to RS232 converter chip MAX3232 are respectively connected to Pin 42 and Pin 43 of the STM32F446RC.The above chips for the present invention are selected based on the requirements for both attitude measurement and wave measurement, and specifically based on several parameters in the order of measurement accuracy performance, reliability (tactical grade), price, and power consumption.
Example 3
Disclosed herein is a control method for using the device, comprising the following steps:
the control unit performing data interaction with a host computer through predetermined interactive commands, the interactive commands including attitude compensation mode/wave operation mode switching command, clock setting command, attitude measurement frequency setting command, and wave sampling frequency setting command;
the host selecting attitude compensation mode or wave operation mode; in the attitude compensation mode, obtaining attitude calculating results; in the wave operation mode, obtaining wave characteristics.
As shown in FIG. 2, interactive command formats include header-i (1 byte), header-2 (1 byte), command flags (1 byte), user settings (1 byte for frequency setting; 6 bytes for time setting), and checksum (1 byte). The header-i and the header-2 are each set to a fixed value (0x55 and Oxaa respectively). The command flags are set to 5 values (0x10, OxI, 0x72, 0x73, and Oxf2) respectively corresponding to the attitude compensation mode, the wave operation mode, attitude measurement frequency setting mode, wave sampling frequency setting mode, and time setting mode. The 4th byte is the attitude measurement frequency (hexadecimal) that needs to be set by users, wherein the allowable frequency is 1-100 Hz. For wave sampling frequency setting command, the 4 th byte is the wave sampling frequency (hexadecimal) set by users, wherein the allowable frequency is 2 Hz or 4 Hz. For clock setting command, the 4th 1 9th
bytes are time values (hexadecimal) set by users, wherein, the 4th byte represents the year, and the allowable value is 0-100 (which represents the year 2000-2100); the 5th byte represents the month, and the allowable value is 1-12 (which represents January to December); the 6thbyte represents the day, and the allowable value is 1-31 (which represents the 1' to 31' day); the 7th byte represents the hour, and the allowable value is 1-24 (which represents the 1' to 2 4 th hour); the 8 th byte represents the minute, and the allowable value is 1-60 (which represents the 1" to 60th minute); the 9 th byte represents the second, and the allowable value is 1-60 (which represents the 1t to 6 0 th second).
As shown in FIG. 3, the attitude compensation mode is carried out based on the on-board source program for attitude compensation mode of STM32F446, and comprises the following steps:
performing parameter initialization and hardware initialization, running infinite loop main program to sequential perform acceleration data collecting, magnetometer data collecting, attitude angle calculation, attitude compensation matrix calculation, and calculation result output, at a predetermined frequency.
The steps of performing parameter initialization and hardware initialization mainly comprise: reading the mode flag bit (ModeFlag), measurement frequency value (FreFlag) and system time in the on-chip Flash, and assigning the corresponding operating variables; next, performing initialization of the proportional coefficient Kp of PID (proportional-integral-derivative) and the attitude output heartbeat interval (OutputNum); eventually, performing the hardware initialization, including initialization of TIMER3, UART4, GPIO, SPI 1 channel, and SPI 2 channel of STM32F446, and also initialization of the 6-axis MEMS inertial navigation unit and the 3-axis magnetometer unit.
Before the steps of attitude angle calculation and attitude compensation matrix calculation, the main program further comprises determining whether the attitude output heartbeat interval (OutputNum) is equal to a first predetermined value, and determining whether the timing value is greater than or equal to a second predetermined value; if both determination results are yes, then reading data from the 6-axis MEMS inertial navigation unit and the 3-axis magnetometer unit, or otherwise, continuing the determination steps and dynamically defining the proportional coefficient Kp of PID (proportional-integral-derivative) control according to the returned gyroscope data and acceleration data. In the present invention, depending on the rotating speed of the gyroscope, Kp may be defined to be two different values. It is discovered that, a desire setting comprises defining the proportional coefficient Kp to be 0.001 if the gyroscope of the ADIS16485 exhibits a rotating speed of more than 50/10 ms, or otherwise defining to be 1.
After the step of defining the proportional coefficient Kp, the main program further comprises normalization of the acceleration data, obtaining evaluation parameters of gravity direction and change, calculating an error between the evaluation parameters and the acceleration data, integrating the error, subjecting the integration result to PI (proportional-integral) control and sending to the gyroscope as compensation input, solving quaternion differential equation, normalization of quaternion, calculating the attitude angle from the normalization of quaternion, calculating the attitude compensation matrix from the attitude angle, and calculating the acceleration in the north-west-up coordinate system from the attitude compensation matrix.
As shown in FIG. 4, the results of attitude angle calculation and attitude compensation matrix calculation will be output sequentially through the serial port in specified formats, wherein the output parameters comprises: roll, pitch, yaw, quaternion (qo, q, q2, q3), attitude conversion matrix Cbn for converting from body coordinate system to north-west-sky coordinate system, 3-axis accelerations (axb, ayb, azb) in the body coordinate system, and 3-axis accelerations (ax, ayn, azn) in the north-west-sky coordinate system. Every single output consists of 92 bytes, wherein the headers constitute 2 bytes (0x55,Oxaa), the checksum constitutes 2 bytes, and each of the parameters is represented by a 4-byte float, being transmitted in big-endian byte order. The attitude conversion matrix Cn is a 3x3 matrix, its main function of digital attitude compensation can be used to carry out attitude correction on the vector measurement in the body coordinate system, by converting the measured vector values of the body coordinate system to values of the north-west-sky coordinate system, so as to obtain true vector values in the natural geographic coordinate system. The system can be applied in the attitude correction on 3-dimensional vector measurement carried out on sway platforms such as unmanned vehicles, unmanned vessels, buoys, and gliders. For example, as to correction on the 3-dimensional wind speed measurement carried out on a sway buoy, assuming that a moored buoy is not moving but only sway, and the 3-dimensional wind speed value measured on the buoy is Vb=[Ub, V, Wb]T, then the corrected value will be Vn=V*Cbn after the correction to the north-west-sky coordinate system is carried out.
As shown in FIG.5, the wave operation mode is carried out based on the on-board source program for wave operation mode of STM32F446, and comprises the following steps:
step 1: performing parameter initialization and hardware initialization; step 2: performing inertial data sampling, and performing acceleration mapping from body frame to ENU coordinate; step 3: performing calculation on the wave characteristics to give calculation results, wherein the wave characteristics include wave height, wave period and wave direction, and the calculation comprises FFT operation of vertical acceleration to frequency domain step 4: calculating the wave height and wave period using the zero crossing method, and calculating the wave direction using the triangulation method; step 5: outputting the calculation results through series communication and storing the calculation results. Furthermore, the step of inertial data sampling comprises obtaining angular acceleration information, acceleration information, and geomagnetic information. The acceleration-specific force conversion comprises attitude angle calculation, attitude compensation matrix calculation, and acceleration conversion from the body coordinate system to the north-west-sky coordinate system.
Step 1 comprises initializing the corresponding intermediate running variable array (high pass Fh and low pass cutoff frequency Fl), and reading the wave frequency value (FreFlagW) and system time in the on-chip Flash and assigning them to the corresponding running variables. The initializations of other parameters and hardware are carried out according to the methods mentioned in the attitude compensation mode. After the initializations are complete, the program will enter a 90-second period, where it keeps inputting the "*" character at 1 second intervals, waiting for external commands such as clock setting and wave sampling frequency setting (2 Hz or 4 Hz). The user shall complete the clock setting and wave sampling frequency setting within 90 seconds after the initialization.
After the 90-second period, the program will enter the data sampling stage, with a wave sampling period of 17 minutes and 30 seconds. The samples comprise 2100 groups when the sampling frequency is 2 Hz and 4200 group when it is 4 Hz. After the sampling step is complete, data of the beginning and ending will be removed. Specifically, data of the beginning 26 groups (13 seconds) and last 26 groups will be removed, while data of the 2048 groups therebetween will be retained, when the sampling frequency is 2 Hz. When the sampling frequency is 4 Hz, then data of the beginning 52 groups (13 seconds) and last 52 groups will be removed, and data of the 4096 groups therebetween will be retained.
In step 2, assuming that the sampling frequency is 4 Hz, there will be 4096 groups of data, including the 3-axis accelerations (ax, ayn, and azo) in the north-west-sky coordinate system, which have been subjected to attitude compensation. The vertical acceleration az" will be converted to the displacement szn by twice integrations, wherein the core integration algorithm uses frequency domain numerical integration. For low-frequency wave acceleration signals, use of frequency domain integration can effectively solve the problem of displacement trend drift caused by time domain integration. Also, the filter parameters of high-frequency/low-frequency cutoff frequency in the frequency domain provide improved self-adaptability as compared with the filtering performance of the traditional time domain integration.
In step 3, the integral transforms comprises dynamic memory allocation, discretization of circular frequency, generating FFT (fast Fourier transform) arrays, performing FFT calculation by calling DSP library functions of STM32F446RC, changing time domain representation to frequency domain representation, performing quadratic integration to obtain frequency domain displacement signal, removing the high-frequency/low-frequency cutoff frequency signals in the frequency domain, performing IFFT (inverse fast Fourier transform) calculation by calling DSP library functions of STM32F4, and changing the frequency domain displacement signal to time domain representation.
In step 4, the wave direction calculation comprises: defining 16 direction zones by dividing the range of 0-360° at 22.50 intervals, counting the number of compositions of the two horizontal accelerations, ax, and ayn, whose angles fall in each direction zone starting from the zero point, and defining the direction zone with the largest number as the main direction of wave. Specifically, it is realized by: calculating the angles of the compositions of the 4096 groups, wherein, since it is the north-west-up coordinate system after the compensation, the direction of ax, is north, the direction of ayn is west (the direction of -ayn is east), and thereby the angle of the composition of the two accelerations can be expressed asO=arctan(-ayn / ax); then, counting the number of the angles fall in each direction zone starting from the zero point, and defining the direction zone with the largest number as the main direction of wave.
The wave characteristic calculation result can be output as:
$WAVE,YYYY-MM-DDHH:MM:SS,Hm,Tm,HioI 0 ,T i/o,H 3,Ti 3 ,Ha,Ta,Dm<CR><LF>
$WAVE is the identification string. YYYY-MM-DDHH:MM:SS represents the wave measurement time (the moment when the measurement is complete). Hm,Tm,Hvi/o,Tvi/o,Hu 3 s ,T1 1 Ha, D,Dm respectively represent maximum wave height, maximum wave period, 1/10 wave height, 1/10 wave period, 1/3 wave height, 1/3 wave period, average wave height, average wave period, main direction. Symbol ',' is provided as the separator between each parameter. <CR><LF> is the terminator. Each wave characteristic parameter constitutes 5 characters, two characters before and after the decimal point respectively, giving a 2 decimal place precision. For example, output of one group can be:
$WAVE,2020-02-12 11:55:30,01.01,05.02,00.85,04.51,00.72,04.31,00.54,03.92, 67.5
The above output indicates the wave characteristics measured at year 2020, the month 2, day 12, and a period of 17 minutes and 30 seconds from 11:38:00 to 11:55:30 am:
a maximum wave height of 1.01 meter, a maximum wave period of 5.02 seconds, a 1/10 wave height of 0.85 meter, a 1/10 wave period of 4.51 seconds, a 1/3 wave height of 0.72 meter, a 1/3 wave period of 4.31 seconds, an average wave height of 0.54 meter, an average wave period of 3.92 seconds, and a main direction of 67.5 degrees.
In the wave operation mode of step 5, after the wave characteristics calculation, in addition to sending the results to the host computer, the results will be also stored in the SD card at the same time, which includes raw sampling data and wave statistical results of the 4096 groups. The 4096 groups of raw sampling data will be stored in a .txt document with the current date as the file name. For example, a file named as "02121138.txt" is to store the 4096 groups of raw sampling data obtained on 12 February 2020 starting at 11:38 am. The raw sampling data can be stored as:
YYYY-MM-DDHH:MM:SS axb ayb azb pitch roll yaw ax, ayn az <CR><LF>
For example, among the 4096 groups of raw data stored in the 02121138.txt document in the root directory of the onboard SD card, 8 groups are stored as:
2020-02-12 11:38:03 0.01 0.56 1.28 19.8 02.2 46.5 0.07 0.08 1.40
2020-02-12 11:38:03 -0.07 0.11 1.38 07.1 07.8 46.1 0.10 -0.06 1.39
2020-02-12 11:38:03 -0.05 -0.29 0.45 -18.9 11.1 45.2 0.02 -0.13 0.52
2020-02-12 11:38:03 0.00 -0.14 0.50 -33.5 00.0 45.1 0.04 0.15 0.49
2020-02-12 11:38:04 -0.04 -0.30 0.58 -27.8 -01.8 43.2 -0.06 0.01 0.65
2020-02-12 11:38:04 0.13 -0.21 0.96 -07.3 -02.7 43.6 0.07 -0.10 0.98
2020-02-12 11:38:04 0.02 0.14 1.15 11.0 02.0 45.6 0.05 -0.08 1.16
2020-02-12 11:38:04 -0.36 0.44 1.07 25.9 14.3 45.8 -0.06 -0.06 1.21
The wave statistical results are all stored in a wave.txt document in the root directory of the onboard SD card, in a format identical to that of wave output. For example, 3 groups of statistical results can be stored as:
$WAVE,2020-02-12 11:55:30,01.01,05.02,00.85,04.51,00.72,04.31,00.54,03.92, 67.5
$WAVE,2020-02-12 12:25:30,01.13,05.18,00.92,04.63,00.76,04.36,00.59,03.96, 45.0
$WAVE,2020-02-12 12:55:30,01.06,05.10,00.88,04.55,00.73,04.33,00.56,04.12, 67.
The above-mentioned examples are only to illustrate the technical concept and characteristics of the present invention, and their purpose is to enable those of ordinary skill in the art to understand the content of the present invention and implement accordingly, rather than to limit the scope of the present invention. All equivalent changes or modifications made according to the essence of the present invention should fall within the scope of the present invention.
Claims (10)
1. A digital system for attitude compensation and wave measurement, comprising a control unit, and further comprising a 6-axis MEMS inertial navigation unit, a 3-axis magnetometer unit, a real-time clock unit, a storage unit, a level conversion unit for serial communication, and a power supply unit, which are connected to the control unit, wherein,
the control unit is configured to process data received from the other units, and send control commands to the other units;
the 6-axis MEMS inertial navigation unit is configured to obtain angular acceleration information and acceleration information;
the 3-axis magnetometer unit is configured to obtain geomagnetic information;
the real-time clock unit is configured to generate clock signals;
the storage unit is configured to store raw sampling data and wave measurement statistical results;
the TTL to RS232 converter unit is configured to produce RS232 voltage level for serial communication; and
the power supply unit is configured to supply electric power to the control unit, the 6-axis MEMS inertial navigation unit, the 3-axis magnetometer unit, the clock unit, the storage unit, and the TTL to RS232 converter unit.
2. The system of claim 1, wherein the control unit comprises a STM32F446 microprocessor, the 6-axis MEMS inertial navigation unit comprises an ADIS16485 chip, the 3-axis magnetometer unit comprises an RM3100 chip, the clock unit comprises a DS1302 chip, the storage unit comprises an SD card (Secure Digital memory card), and the level conversion unit comprises a MAX3232 chip.
3. A control method for using the system of claim 1 or 2, comprising the following steps:
the control unit performing data interaction with a host computer through predetermined interactive commands, the interactive commands including attitude compensation mode/wave operation mode switching command, clock setting command, attitude measurement frequency setting command, and wave sampling frequency setting command;
the host selecting attitude compensation mode or wave operation mode; in the attitude compensation mode, obtaining attitude calculating results; in the wave operation mode, obtaining wave characteristics.
4. The control method of claim 3, wherein interactive command formats include header-1, ) header-2, command flags, user settings, and checksum, wherein, the header-i and the header-2 are each set to a fixed value, and the command flags are set to 5 values respectively corresponding to the attitude compensation mode, the wave operation mode, attitude measurement frequency setting mode, wave sampling frequency setting mode, and time setting mode.
5. The control method of claim 3, wherein the attitude compensation mode comprises the following steps:
performing parameter initialization and hardware initialization, running infinite loop main program to sequential perform acceleration data collecting, magnetometer data collecting, attitude angle calculation, attitude compensation matrix calculation, and calculation result output, at a predetermined frequency.
6. The control method of claim 5, wherein, before the steps of attitude angle calculation and attitude compensation matrix calculation, the main program further comprises determining whether the attitude output heartbeat interval is equal to a first predetermined value, and determining whether the timing value is greater than or equal to a second predetermined value; if both determination results are yes, then reading data from the 6-axis MEMS inertial navigation unit and the 3-axis magnetometer unit, or otherwise, continuing the determination steps and dynamically defining the proportional coefficient Kp of PID (proportional-integral-derivative) control according to the returned gyroscope data and acceleration data.
7. The control method of claim 6, wherein, after the step of defining the proportional coefficient Kp, the main program further comprises normalization of the acceleration data, obtaining evaluation parameters of gravity direction and change, calculating an error between the evaluation parameters and the acceleration data, integrating the error, subjecting the integration result to PI control and sending to the gyroscope as compensation input, solving quaternion differential equation, normalization of quaternion, calculating the attitude angle from the normalization of quaternion, calculating the attitude compensation matrix from the attitude angle, and calculating the acceleration in the north-west-sky coordinate system from the attitude compensation matrix.
8. The control method of claim 7, wherein, the proportional coefficient Kp is defined to be 0.001 if the gyroscope in the 6-axis MEMS inertial navigation unit exhibits a rotating speed of more than 50/10 ms, or otherwise the proportional coefficient Kp is defined to be 1.
9. The control method of claim 3, wherein, the wave operation mode comprises the following steps:
step 1: performing parameter initialization and hardware initialization; step 2: performing inertial data sampling, and performing acceleration mapping from body frame to ENU coordinate ; step 3: performing calculation on the wave characteristics to give calculation results, wherein the wave characteristics include wave height, wave period and wave direction, and the calculation comprises FFT operation of vertical acceleration to frequency domain; step 4: calculating the wave height and wave period using the zero crossing method; step 5: outputting and storing the calculation results.
10. The control method of claim 9, wherein, the integral transforms comprises dynamic memory allocation, discretization of circular frequency, generating FFT arrays, performing FFT calculation, changing time domain representation to frequency domain representation, performing quadratic integration to obtain frequency domain displacement signal, removing the high-frequency/low-frequency cutoff frequency signals in the frequency domain, performing IFFT calculation, and changing the frequency domain displacement signal to time domain representation.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| AU2021104827A AU2021104827A4 (en) | 2021-08-02 | 2021-08-02 | Digital system and control method for attitude compensation and wave measurement |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| AU2021104827A AU2021104827A4 (en) | 2021-08-02 | 2021-08-02 | Digital system and control method for attitude compensation and wave measurement |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| AU2021104827A4 true AU2021104827A4 (en) | 2021-09-30 |
Family
ID=77857849
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| AU2021104827A Active AU2021104827A4 (en) | 2021-08-02 | 2021-08-02 | Digital system and control method for attitude compensation and wave measurement |
Country Status (1)
| Country | Link |
|---|---|
| AU (1) | AU2021104827A4 (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113879472A (en) * | 2021-11-03 | 2022-01-04 | 江苏科技大学 | Semi-submersible drilling platform system with pose compensation capability and control method |
-
2021
- 2021-08-02 AU AU2021104827A patent/AU2021104827A4/en active Active
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113879472A (en) * | 2021-11-03 | 2022-01-04 | 江苏科技大学 | Semi-submersible drilling platform system with pose compensation capability and control method |
| CN113879472B (en) * | 2021-11-03 | 2024-01-30 | 江苏科技大学 | Semi-submersible drilling platform system with pose compensation capability and control method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN1740746B (en) | Micro-dynamic carrier attitude measuring apparatus and measuring method thereof | |
| CN100593689C (en) | Gasture estimation and interfusion method based on strapdown inertial nevigation system | |
| CN101672649B (en) | Mooring alignment method of optical fiber strapdown system for ship based on digital low-pass filtering | |
| CN108827299A (en) | A kind of attitude of flight vehicle calculation method based on improvement quaternary number second order complementary filter | |
| CN111412900B (en) | Digital attitude compensation and wave measurement system and control method | |
| CN108759845A (en) | A kind of optimization method based on inexpensive multi-sensor combined navigation | |
| CN106482734A (en) | A kind of filtering method for IMU Fusion | |
| CN101840529B (en) | Optic fiber gyroscope random drift modeling method based on locally variable integrated neural network | |
| CN105021183A (en) | Low-cost GPS and INS integrated navigation system for multi-rotor aircrafts | |
| CN106017452B (en) | Double tops disturbance rejection north finding method | |
| CN109443342A (en) | NEW ADAPTIVE Kalman's UAV Attitude calculation method | |
| AU2021104827A4 (en) | Digital system and control method for attitude compensation and wave measurement | |
| CN101393028B (en) | Rapid estimating and compensating system with IMU mounting angle obliquely set | |
| CN103712598B (en) | Attitude determination method of small unmanned aerial vehicle | |
| CN110174121A (en) | A kind of aviation attitude system attitude algorithm method based on earth's magnetic field adaptive correction | |
| CN111812737B (en) | Integrated system for underwater navigation and gravity measurement | |
| CN115560778A (en) | Real-time error compensation method for inertial measurement system based on resonant inertial device | |
| CN111504278A (en) | Sea wave detection method based on self-adaptive frequency domain integration | |
| CN110207647A (en) | A kind of armlet attitude angle calculation method based on complementary Kalman filter | |
| CN103123361A (en) | MEMS (micro-electromechanical system) angular speed and acceleration sensor automatic demarcating method and system thereof | |
| Carratù et al. | Self-alignment procedure for IMU in automotive context | |
| CN105241453A (en) | Unmanned aerial vehicle navigation system and unmanned aerial vehicle | |
| CN115931001A (en) | Inertial measurement unit calibration method and device, computer equipment and storage medium | |
| CN1670487A (en) | Data recording equipment for mini aircrafts | |
| CN114858187A (en) | MEMS inertial navigation system calibration method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FGI | Letters patent sealed or granted (innovation patent) |