CN116125789A - Gesture algorithm parameter automatic matching system and method based on quaternion - Google Patents
Gesture algorithm parameter automatic matching system and method based on quaternion Download PDFInfo
- Publication number
- CN116125789A CN116125789A CN202211694677.0A CN202211694677A CN116125789A CN 116125789 A CN116125789 A CN 116125789A CN 202211694677 A CN202211694677 A CN 202211694677A CN 116125789 A CN116125789 A CN 116125789A
- Authority
- CN
- China
- Prior art keywords
- algorithm
- angle
- angle sensor
- gesture
- quaternion
- 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.)
- Pending
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 126
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000012545 processing Methods 0.000 claims abstract description 28
- 238000005457 optimization Methods 0.000 claims abstract description 15
- 230000008569 process Effects 0.000 claims abstract description 15
- 239000013598 vector Substances 0.000 claims description 21
- 238000004891 communication Methods 0.000 claims description 20
- 238000012360 testing method Methods 0.000 claims description 20
- 230000001133 acceleration Effects 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 10
- 230000004044 response Effects 0.000 claims description 10
- 238000005259 measurement Methods 0.000 claims description 7
- 230000010354 integration Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 3
- 230000003068 static effect Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 4
- 238000001514 detection method Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 230000000295 complement effect Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- 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.
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- User Interface Of Digital Computer (AREA)
Abstract
The invention provides a quaternion-based automatic matching system and a quaternion-based automatic matching method for gesture algorithm parameters, which can accurately reflect the real gesture of equipment in the working process of the equipment, provide reliable data for equipment operators and ensure that the equipment is in a safe working state when in work. The system comprises: calibrating a turntable: the system is used for providing standard angle data for the upper computer, so that the comparison of the output values of the angle sensors is facilitated; an angle sensor: the device is fixed on a calibration turntable, is a calibrated sample, is a carrier of an attitude algorithm, is loaded with the initialized attitude algorithm and is used for transmitting an angle value of an angle sensor to an upper computer; the upper computer: the device is used for controlling the calibration turntable to rotate to a certain angle; acquiring angle values of a calibration turntable and an angle sensor; and according to the standard angle data and the angle value of the angle sensor, carrying out data processing and parameter optimization of a posture algorithm to obtain a posture algorithm with updated parameters, and transmitting the posture algorithm to the angle sensor. According to the invention, parameters in an attitude algorithm adopted by the angle sensor are automatically matched through the calibration turntable and the upper computer.
Description
Technical Field
The invention relates to an automatic matching system and method for gesture algorithm parameters based on quaternion, and belongs to the technical field of automatic control.
Background
Along with the requirements of the market on crane running safety and comfort, stability and operability during operation and intelligence, the requirements of an inertial angle sensor (also called a car body posture detection inertial angle sensor) on precision and accuracy are higher and higher as a basic component for finishing fine control and intelligence of a car body. The diversified sensing detection unit and the multi-angle sensor information fusion technology are bases for accurate motion control and intellectualization of the crane. How to accurately detect and judge the body posture and the arm support posture of the crane in the running or operation process of the crane is a key problem for realizing the intelligent control of the crane.
Attitude colloquially refers to the state of pitch (pitch), roll (roll), heading (yaw) of an aircraft we stand on the ground. The working equipment needs to know the current posture in real time so as to be capable of controlling the following actions according to the needs, such as keeping stable and rolling. The posture mathematical model is used to describe the angular positional relationship (posture angle) between the fixed coordinate system of one rigid body and the reference coordinate system. The gesture algorithm is a five-flower eight-door algorithm, and the same algorithm can be realized in various ways by different rotation expression methods. The main algorithms essentially comprise a directional cosine matrix method, an equivalent rotation vector method and a quaternion method.
Regardless of the algorithm used, the parameters in the algorithm need to be optimally matched. In the prior art, the parameter matching of the gesture algorithm adopts a single repeated test, data comparison is carried out again every time the parameter adjustment is carried out, detection comparison is carried out under ideal conditions, and corresponding parameters are manually adjusted. The actual conditions are not taken into account.
The existing angle sensor attitude algorithm parameter matching is single, and system integration is not formed. The parameters cannot be automatically matched, and the parameters can be selected and allocated only manually, so that time and labor are wasted; in the parameter searching process, the actual working condition of the equipment is not considered, the possibility of larger parameter matching error exists, the accuracy of the output value of the angle sensor is greatly reduced, and therefore the safety and reliability of the working state of the whole equipment are not guaranteed.
Disclosure of Invention
The invention aims to overcome the defects in the prior art, and provides a quaternion-based automatic matching system and method for gesture algorithm parameters, which can accurately reflect the real gesture of equipment in the working process of the equipment, provide reliable data for equipment operators and ensure that the equipment is in a safe working state when working.
In order to achieve the above purpose, the invention is realized by adopting the following technical scheme:
in a first aspect, the present invention provides an automatic matching system for gesture algorithm parameters based on quaternion, including:
calibrating a turntable: the system is used for providing standard angle data for the upper computer, so that the comparison of the output values of the angle sensors is facilitated;
an angle sensor: the device is fixed on a calibration turntable, is a calibrated sample, is a carrier of an attitude algorithm, is loaded with the initialized attitude algorithm and is used for transmitting an angle value of an angle sensor to an upper computer; when the angle value output by the angle sensor is closer to the angle value output by the calibration turntable, the more successful the gesture algorithm parameter is matched;
the upper computer: the device is used for controlling the calibration turntable to rotate to a certain angle; acquiring angle values of a calibration turntable and an angle sensor; and according to the standard angle data and the angle value of the angle sensor, carrying out data processing and parameter optimization of a posture algorithm to obtain a posture algorithm with updated parameters, and transmitting the posture algorithm to the angle sensor.
Further, the upper computer includes:
the turntable control module is used for controlling the rotation of the calibration turntable;
the turntable data acquisition module is used for acquiring and calibrating the angle value of the turntable;
the angle sensor data acquisition module is used for acquiring an output angle value of the angle sensor;
the gesture algorithm module is used for calculating the output value of the angle sensor;
the angle sensor index requirement module is used for acquiring requirements of response speed, azimuth angle precision, position precision and acceleration precision setting of the angle sensor;
the data processing module is used for determining whether the matched parameters meet the requirements or not through processing the calibration turntable data and the angle sensor data;
the algorithm parameter optimization module is used for automatically matching the Mahony gesture algorithm parameters of the quaternion algorithm to find the optimal parameters in the gesture algorithm, and when a certain parameter is substituted into the gesture algorithm to enable the output angle of the angle sensor to meet the index requirement, the parameter is the matched parameter.
Further, the data processing module performs data processing on the data of the calibration turntable and the data of the angle sensor to obtain the response speed, azimuth angle precision, position precision and acceleration precision of the angle sensor;
based on the acquired requirements of the response speed, azimuth angle precision, position precision and acceleration precision of the angle sensor, if the response speed, azimuth angle precision, position precision and acceleration precision of the angle sensor meet the set requirements, determining that parameters in the attitude algorithm meet algorithm requirements at the moment; if the data processing result can not meet the technical index requirements of the angle sensor, continuing to search for other parameters to bring into a gesture algorithm, and then re-resolving the gesture of the angle sensor through the corresponding gesture algorithm until the data processing result can meet the relevant technical index of the angle sensor, so as to obtain optimized parameters; the optimized parameters are parameters of a final angle sensor attitude algorithm.
Further, the automatic matching method of the mahonyl gesture algorithm parameters of the algorithm parameter optimization module aiming at the quaternion algorithm comprises the following steps:
according to the principle of a mahonyl gesture algorithm, introducing a quaternion, wherein the quaternion comprises a real part, three imaginary parts and q 0 +q 1 i+q 2 j+q 3 k is represented by a vector represented by the following formula 1;
the Mahony gesture is calculated to obtain a gesture obtained by integrating angular velocity and a gesture obtained by an accelerometer;
the gesture obtained based on the angular velocity integration is discretized by using a forward difference method through a quaternion differential equation 2 to obtain a discretized equation 3;
wherein, the angle mark b represents the equipment coordinate system, the angle mark n represents the navigation coordinate system, b ω Representing the angular velocity of the device coordinate system, i.e. the measured value of the gyroscope;
based on the posture observed by the accelerometer, measuring by the accelerometer to obtain triaxial acceleration; by means of
Representing the rotation matrix from b to n, the vector b in b V By taking the left side +>
Can be converted into a vector n in an n-series V Namely the following formula 4;
when the IMU is stationary in the navigation coordinate system, the accelerometer output is:
wherein g is gravitational acceleration;
the accelerometer output in the body coordinate system is:
normalizing the measured value of the accelerometer to obtain a vector 8;
representing equation 7 as a matrix in the form of a quaternion, equation 9 can be obtained;
the gravity direction vector is expressed as formula 10;
the error resulting from the vector cross product is equation 11;
E rr =a' ×v 11
Performing error compensation by using a PI control algorithm, wherein the compensation quantity is the measurement deviation of the angular velocity, and the formula 12;
ω bias ＝K p ·E rr +K i ·∫E rr 12. Fig.
Wherein K is p Is a proportionality coefficient, K i Is an integral coefficient; k (K) p Representing the degree of trust, K, of the angle sensor p The larger the value, the more trusted the accelerometer measurements, and vice versa,the more trusted the gyroscope data; k (K) i For eliminating static errors, i.e. zero offset of the gyroscope;
parameter K in Mahony gesture algorithm p 、K i During initialization, a smaller K is set first p Gradually increasing larger K in parameter optimization process p Until the best K is found p The method comprises the steps of carrying out a first treatment on the surface of the For K i A larger K is firstly set i Gradually reducing K in parameter optimization process i Until the optimal K is found i 。
Further, the automatic matching method further comprises the following steps:
converting the quaternion into an Euler angle through a formula, wherein the expression formula is shown as formula 13;
wherein phi is the roll angle, θ is the pitch angle, and ψ is the yaw angle.
Further, the upper computer further includes:
and the data display module is used for displaying the current calibration turntable angle and the sensor output angle.
Further, the angle sensor is connected with the upper computer through CAN communication, and the calibration turntable is connected with the upper computer through serial communication;
furthermore, when the field working condition of the equipment is simulated, the angle sensor and the calibration turntable are fixed on the vibration test bed.
In a second aspect, the present invention provides a method for automatically matching parameters of a gesture algorithm based on quaternion, based on the system of the first aspect, comprising the following steps:
setting a vibration test bed according to the actual working condition of the angle sensor and the acquired related data, so that the vibration test bed is maximally close to the actual working condition;
fixing the angle sensor on a calibration rotary table, and fixing the calibration rotary table on a test vibration table;
the upper computer is respectively communicated with the calibration turntable and the angle sensor through a serial port communication cable and a CAN communication bus;
and the speed resolution, the angular position accuracy, the angular position repeatability and the position resolution of the calibration turntable in the automatic matching system of the gesture algorithm parameters meet the requirements of each sensing calibration.
Further, the method further comprises:
initializing parameters of an attitude algorithm of the angle sensor by an upper computer, and calculating the attitude angle of the angle sensor by the attitude algorithm;
setting an operation mode, an operation direction and an operation mode of the calibration turntable, enabling the calibration turntable to operate according to set parameters, and enabling the angle sensor to operate along with the calibration turntable.
Further, the method further comprises:
starting a vibration test bed and simulating the actual working condition of the angle sensor;
the upper computer collects real-time data of the turntable through the serial port communication cable; and meanwhile, the angle sensor outputs real-time data under the attitude algorithm of the initialized parameters, and the data are transmitted to a data processing module of the upper computer through the CAN communication bus.
Compared with the prior art, the invention has the beneficial effects that:
1. the invention provides a quaternion-based attitude algorithm parameter automatic matching system and a quaternion-based attitude algorithm parameter automatic matching method, and a system configuration and interaction process of the quaternion-based attitude algorithm parameter automatic matching system and the quaternion-based attitude algorithm parameter automatic matching method, and provides a basis for developing a new upper computer.
2. The invention provides a quaternion-based automatic matching system and a quaternion-based automatic matching method for gesture algorithm parameters, which can simulate actual working conditions and rapidly match the gesture algorithm parameters, and save the calibration time of an angle sensor.
3. The invention provides a system and a method for automatically matching parameters Kp and Ki of a mahonn gesture algorithm based on quaternions, which can quickly and accurately determine the parameters Kp and Ki of the algorithm through the system for automatically matching parameters of the mahonn gesture algorithm, so that an angle sensor of the system meets the requirements of related technical indexes, and the accuracy of the angle sensor is ensured. The high-frequency characteristic of the gyroscope and the low-frequency characteristic of the accelerometer are fused, the most suitable fusion point is found, complementary filtering is carried out, and the angle measured by the angle sensor is more accurate.
4. The invention provides a quaternion-based attitude algorithm parameter automatic matching system and a quaternion-based attitude algorithm parameter automatic matching method, which reduce the calibration time of an angle sensor, and the measured angle is more accurate, so that the whole equipment can be operated more safely and reliably.
Drawings
FIG. 1 is a block diagram of an automatic matching system for gesture algorithm parameters;
FIG. 2 is a flow chart for automatic matching of gesture algorithm parameters;
fig. 3 is a flowchart for automatically matching parameters of a mahonyl gesture algorithm.
Detailed Description
The invention is further described below with reference to the accompanying drawings. The following examples are only for more clearly illustrating the technical aspects of the present invention, and are not intended to limit the scope of the present invention.
Embodiment one:
the embodiment provides a quaternion-based attitude algorithm parameter automatic matching system and a quaternion-based attitude algorithm parameter automatic matching method.
The automatic matching system structure of the gesture algorithm parameters is shown in fig. 1, and mainly comprises a calibration turntable 1, an angle sensor 2 and an upper computer 3. The angle sensor is fixed on the calibration rotary table, the angle sensor is connected with the upper computer through CAN communication 5, and the calibration rotary table is connected with the upper computer through serial communication 4. The simulation site of the equipment working condition can be realized through a vibration test bed, and the angle sensor and the calibration turntable can be fixed on the vibration test bed when the equipment working condition is simulated.
The upper computer in the automatic matching system of the gesture algorithm parameters mainly comprises the following modules: the system comprises a turntable control module, turntable data acquisition, an angle sensor data acquisition and attitude algorithm, data display, data processing, angle sensor index requirements and algorithm parameter optimization.
A specific flow of the automatic matching system of gesture algorithm parameters is shown in fig. 2. The specific process is as follows:
according to the actual working condition of the angle sensor and the collected related data, the vibration test bed is set, so that the vibration test bed is maximally close to the actual working condition. The angle sensor is fixed on a calibration rotary table, and the calibration rotary table is fixed on the test vibration table. The upper computer is communicated with the calibration turntable and the angle sensor respectively through the serial port communication cable and the CAN communication bus.
Technical parameters such as rate resolution, angular position accuracy, angular position repeatability and position resolution of the calibration turntable in the automatic matching system of gesture algorithm parameters meet the requirements of sensing calibration.
Initializing parameters of an attitude algorithm of the angle sensor by the upper computer, and calculating the attitude angle of the angle sensor by the attitude algorithm. And simultaneously setting an operation mode, an operation direction and an operation mode of the calibration turntable. The calibration turntable is operated according to the set parameters, and the angle sensor is operated along with the calibration turntable. And starting the vibration test bed, and simulating the actual working condition of the angle sensor. The upper computer collects real-time data of the turntable through the serial port communication cable; and meanwhile, the angle sensor outputs real-time data under the attitude algorithm of the initialized parameters, and the data are transmitted to a data processing module of the upper computer through the CAN communication bus. The data processing module comprehensively processes the data of the calibration turntable and the data of the angle sensor, and in the processing process, if the technical parameters of the angle sensor such as the response speed, the azimuth angle precision, the position precision, the acceleration precision and the like of the angle sensor meet the set requirements, the system automatically determines that the parameters in the attitude algorithm meet the algorithm requirements at the moment; if the data processing result can not meet the technical index requirements of the angle sensor, the attitude algorithm parameters are required to be optimized, and then the attitude algorithm parameters are subjected to the optimization processing and then are subjected to the calculation again through the corresponding attitude algorithm until the data processing result can meet the technical index related to the angle sensor. The optimized parameters are parameters of a final angle sensor attitude algorithm.
An automatic matching flow chart of the mahonyl gesture algorithm parameters for the quaternion algorithm is shown in fig. 3.
According to the principle of a mahonyl gesture algorithm, a quaternion is introduced, wherein the quaternion comprises a real part and three partsImaginary part, q 0 +q 1 i+q 2 j+q 3 k is represented by a vector expressed as the following formula 1.
The mahonyl pose solution requires a pose integrated by angular velocity and a pose obtained by an accelerometer. The attitude obtained based on the angular velocity integration is discretized by using a forward difference method through a quaternion differential equation 2, and a discretized equation 3 is obtained.
Wherein, the angle mark b represents the equipment coordinate system, the angle mark n represents the navigation coordinate system, b ω Representing the angular velocity of the device coordinate system, i.e. the measured value of the gyroscope.
Based on the posture observed by the accelerometer, three-axis acceleration is measured by the accelerometer. By means of
Representing the rotation matrix from b to n, the vector b in b V By taking the left side +>
Can be converted into a vector n in an n-series V I.e., formula 4 below.
When the IMU is stationary in the navigation coordinate system, the accelerometer output is:
where g is the gravitational acceleration.
The accelerometer output in the body coordinate system is:
the high-frequency characteristic of the gyroscope and the low-frequency characteristic of the accelerometer are fused, and the effects of better high-frequency and low-frequency characteristics can be obtained by utilizing complementary filtering. This filtering is an error feedback based filter, representing the error in terms of vector cross products, with the larger the difference between the two vectors, the larger their cross product modulo length. Normalizing the accelerometer measurements yields vector 8.
Equation 7 is expressed as a matrix in the form of a quaternion, and equation 9 can be obtained.
The gravity direction vector is expressed as equation 10.
The error resulting from the vector cross product is equation 11.
E rr =a' ×v 11
Error compensation is performed using a PI control algorithm, and the compensation amount is the measurement deviation of the angular velocity, equation 12.
ω bias ＝K p ·E rr +K i ·∫E rr 12. Fig.
Wherein K is p Is a proportionality coefficient, K i Is an integral coefficient. K (K) p Representing the degree of trust, K, of the angle sensor p The larger the value, the more trusted the accelerometer measurements, and conversely, the more trusted the gyroscope data; k (K) i For eliminating static errors, i.e. zero-bias of gyroscopes. From the aspects of stability, response speed, overshoot, steady state progress and the like of the system, the parameter K in the Mahony gesture algorithm p 、K i During initialization, a smaller K is set first p Gradually increasing larger K in parameter optimization process p Until the best K is found P The method comprises the steps of carrying out a first treatment on the surface of the For K i A larger K is firstly set i Gradually reducing K in parameter optimization process i Until the optimal K is found i 。
The system can well optimize the parameter K by the parameter optimizing program p And K is equal to i And (5) performing automatic matching. The corrected angular velocity value is obtained, the fused gesture can be obtained by recursively pushing the gesture through the formula 3, the gesture is not very visual represented by the quaternion, the quaternion can be converted into the Euler angle through a formula, and the expression formula is shown as formula 13.
Wherein phi is the roll angle, θ is the pitch angle, and ψ is the yaw angle.
The algorithm parameters Kp and Ki can be rapidly and accurately determined through the Mahony gesture algorithm parameter automatic matching system, so that the angle sensor can meet the requirements of related technical indexes, and the accuracy of the angle sensor is ensured.
Embodiment two:
the embodiment provides a quaternion-based automatic matching method for gesture algorithm parameters, which is based on the system of the embodiment one and comprises the following steps:
setting a vibration test bed according to the actual working condition of the angle sensor and the acquired related data, so that the vibration test bed is maximally close to the actual working condition;
fixing the angle sensor on a calibration rotary table, and fixing the calibration rotary table on a test vibration table;
the upper computer is respectively communicated with the calibration turntable and the angle sensor through a serial port communication cable and a CAN communication bus;
and the speed resolution, the angular position accuracy, the angular position repeatability and the position resolution of the calibration turntable in the automatic matching system of the gesture algorithm parameters meet the requirements of each sensing calibration.
Initializing parameters of an attitude algorithm of the angle sensor by an upper computer, and calculating the attitude angle of the angle sensor by the attitude algorithm;
setting an operation mode, an operation direction and an operation mode of the calibration turntable, enabling the calibration turntable to operate according to set parameters, and enabling the angle sensor to operate along with the calibration turntable.
It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
The foregoing is merely a preferred embodiment of the present invention, and it should be noted that modifications and variations could be made by those skilled in the art without departing from the technical principles of the present invention, and such modifications and variations should also be regarded as being within the scope of the invention.
Claims (10)
1. An automatic matching system for gesture algorithm parameters based on quaternion is characterized by comprising:
calibrating a turntable: the system is used for providing standard angle data for the upper computer, so that the comparison of the output values of the angle sensors is facilitated;
an angle sensor: the device is fixed on a calibration turntable, is a calibrated sample, is a carrier of an attitude algorithm, is loaded with an initialized attitude algorithm and is used for transmitting an angle value of an angle sensor to an upper computer;
the upper computer: the device is used for controlling the calibration turntable to rotate to a certain angle; acquiring angle values of a calibration turntable and an angle sensor; and according to the standard angle data and the angle value of the angle sensor, carrying out data processing and parameter optimization of a posture algorithm to obtain a posture algorithm with updated parameters, and transmitting the posture algorithm to the angle sensor.
2. The quaternion-based gesture algorithm parameter automatic matching system according to claim 1, wherein the upper computer comprises:
the turntable control module is used for controlling the rotation of the calibration turntable;
the turntable data acquisition module is used for acquiring and calibrating the angle value of the turntable;
the angle sensor data acquisition module is used for acquiring an output angle value of the angle sensor;
the gesture algorithm module is used for calculating the output value of the angle sensor;
the angle sensor index requirement module is used for acquiring requirements of response speed, azimuth angle precision, position precision and acceleration precision setting of the angle sensor;
the data processing module is used for determining whether the matched parameters meet the requirements or not through processing the calibration turntable data and the angle sensor data;
the algorithm parameter optimization module is used for automatically matching the Mahony gesture algorithm parameters of the quaternion algorithm to find the optimal parameters in the gesture algorithm, and when a certain parameter is substituted into the gesture algorithm to enable the output angle of the angle sensor to meet the index requirement, the parameter is the matched parameter.
3. The quaternion-based attitude algorithm parameter automatic matching system according to claim 2, wherein the data processing module performs data processing on data of the calibration turntable and data of the angle sensor to obtain response speed, azimuth angle precision, position precision and acceleration precision of the angle sensor;
based on the acquired requirements of the response speed, azimuth angle precision, position precision and acceleration precision of the angle sensor, if the response speed, azimuth angle precision, position precision and acceleration precision of the angle sensor meet the set requirements, determining that parameters in the attitude algorithm meet algorithm requirements at the moment; if the data processing result can not meet the technical index requirements of the angle sensor, continuing to search for other parameters to bring into a gesture algorithm, and then re-resolving the gesture of the angle sensor through the corresponding gesture algorithm until the data processing result can meet the relevant technical index of the angle sensor, so as to obtain optimized parameters; the optimized parameters are parameters of a final angle sensor attitude algorithm.
4. The quaternion-based automatic matching system for gesture algorithm parameters according to claim 2, wherein the automatic matching method for the mahonyl gesture algorithm parameters of the algorithm parameter optimization module for the quaternion algorithm comprises the following steps:
according to the principle of a mahonyl gesture algorithm, introducing a quaternion, wherein the quaternion comprises a real part, three imaginary parts and q 0 +q 1 i+q 2 j+q 3 k is represented by a vector represented by the following formula 1;
the Mahony gesture is calculated to obtain a gesture obtained by integrating angular velocity and a gesture obtained by an accelerometer;
the gesture obtained based on the angular velocity integration is discretized by using a forward difference method through a quaternion differential equation 2 to obtain a discretized equation 3;
wherein, the angle mark b represents the equipment coordinate system, the angle mark n represents the navigation coordinate system, b ω Representing the angular velocity of the device coordinate system, i.e. the measured value of the gyroscope;
based on the posture observed by the accelerometer, measuring by the accelerometer to obtain triaxial acceleration; by means of
Representing the rotation matrix from b to n, the vectors in b b v by left multiplication->
Can be converted into vectors in n series n v, formula 4;
when the IMU is stationary in the navigation coordinate system, the accelerometer output is:
wherein g is gravitational acceleration;
the accelerometer output in the body coordinate system is:
equations 5 and 6 are obtained by rotating the matrix:
normalizing the measured value of the accelerometer to obtain a vector 8;
representing equation 7 as a matrix in the form of a quaternion, equation 9 can be obtained;
the gravity direction vector is expressed as formula 10;
the error resulting from the vector cross product is equation 11;
E rr =a' ×v 11
Performing error compensation by using a PI control algorithm, wherein the compensation quantity is the measurement deviation of the angular velocity, and the formula 12;
ω bias ＝K p ·E rr +K i ·∫E rr 12. Fig.
Wherein K is p Is a proportionality coefficient, K i Is an integral coefficient; k (K) p Representing the degree of trust, K, of the angle sensor p The larger the value, the more trusted the accelerometer measurements, and conversely, the more trusted the gyroscope data; k (K) i For eliminating static errors, i.e. zero offset of the gyroscope;
parameter K in Mahony gesture algorithm p 、K i During initialization, a smaller K is set first p Gradually increasing larger K in parameter optimization process p Until the best K is found p The method comprises the steps of carrying out a first treatment on the surface of the For K i A larger K is firstly set i Gradually reducing K in parameter optimization process i Until the optimal K is found i 。
5. The quaternion-based gesture algorithm parameter automatic matching system according to claim 1, wherein the automatic matching method further comprises:
converting the quaternion into an Euler angle through a formula, wherein the expression formula is shown as formula 13;
wherein phi is the roll angle, θ is the pitch angle, and ψ is the yaw angle.
6. The quaternion-based gesture algorithm parameter automatic matching system according to claim 1, wherein the upper computer further comprises:
and the data display module is used for displaying the current calibration turntable angle and the sensor output angle.
7. The quaternion-based attitude algorithm parameter automatic matching system according to claim 1, wherein the angle sensor is connected with an upper computer through CAN communication, and the calibration turntable is connected with the upper computer through serial communication;
and when the field working condition of the equipment is simulated, the angle sensor and the calibration turntable are fixed on the vibration test bed.
8. A quaternion-based automatic matching method for gesture algorithm parameters, which is based on the system according to any one of claims 1-7 and comprises the following steps:
setting a vibration test bed according to the actual working condition of the angle sensor and the acquired related data, so that the vibration test bed is maximally close to the actual working condition;
fixing the angle sensor on a calibration rotary table, and fixing the calibration rotary table on a test vibration table;
the upper computer is respectively communicated with the calibration turntable and the angle sensor through a serial port communication cable and a CAN communication bus;
and the speed resolution, the angular position accuracy, the angular position repeatability and the position resolution of the calibration turntable in the automatic matching system of the gesture algorithm parameters meet the requirements of each sensing calibration.
9. The quaternion-based gesture algorithm parameter automatic matching method of claim 8, further comprising:
initializing parameters of an attitude algorithm of the angle sensor by an upper computer, and calculating the attitude angle of the angle sensor by the attitude algorithm;
setting an operation mode, an operation direction and an operation mode of the calibration turntable, enabling the calibration turntable to operate according to set parameters, and enabling the angle sensor to operate along with the calibration turntable.
10. The quaternion-based gesture algorithm parameter automatic matching method of claim 8, further comprising:
starting a vibration test bed and simulating the actual working condition of the angle sensor;
the upper computer collects real-time data of the turntable through a serial port communication cable; and meanwhile, the angle sensor outputs real-time data under the attitude algorithm of the initialized parameters, and the data are transmitted to a data processing module of the upper computer through the CAN communication bus.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211694677.0A CN116125789A (en) | 2022-12-28 | 2022-12-28 | Gesture algorithm parameter automatic matching system and method based on quaternion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211694677.0A CN116125789A (en) | 2022-12-28 | 2022-12-28 | Gesture algorithm parameter automatic matching system and method based on quaternion |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116125789A true CN116125789A (en) | 2023-05-16 |
Family
ID=86302086
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211694677.0A Pending CN116125789A (en) | 2022-12-28 | 2022-12-28 | Gesture algorithm parameter automatic matching system and method based on quaternion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116125789A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116588261A (en) * | 2023-07-03 | 2023-08-15 | 上海新纪元机器人有限公司 | Active compensation control method and system for seat |
-
2022
- 2022-12-28 CN CN202211694677.0A patent/CN116125789A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116588261A (en) * | 2023-07-03 | 2023-08-15 | 上海新纪元机器人有限公司 | Active compensation control method and system for seat |
| CN116588261B (en) * | 2023-07-03 | 2024-02-09 | 上海新纪元机器人有限公司 | Active compensation control method and system for seat |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102017404B1 (en) | How to update the full posture angle of agricultural machinery based on 9 axis MEMS sensor | |
| CN105203129B (en) | A kind of inertial nevigation apparatus Initial Alignment Method | |
| CN105588567B (en) | A kind of attitude heading reference system and method for autoelectrinic compass calibration assist type | |
| CN106767804B (en) | The multidimensional data measurement apparatus and method of a kind of moving object | |
| CN107560613B (en) | Robot indoor track tracking system and method based on nine-axis inertial sensor | |
| CN107655493B (en) | SINS six-position system-level calibration method for fiber-optic gyroscope | |
| CN106969783B (en) | Single-axis rotation rapid calibration technology based on fiber-optic gyroscope inertial navigation | |
| CN104811588B (en) | A kind of boat-carrying based on gyroscope is surely as control method | |
| CN108458714B (en) | Euler angle solving method without gravity acceleration in attitude detection system | |
| CN103808331A (en) | MEMS (micro-electromechanical system) three-axis gyroscope error calibration method | |
| CN109724602A (en) | A kind of attitude algorithm system and its calculation method based on hardware FPU | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| CN109540135B (en) | Method and device for detecting pose and extracting yaw angle of paddy field tractor | |
| CN108318038A (en) | A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method | |
| CN108731676B (en) | Attitude fusion enhanced measurement method and system based on inertial navigation technology | |
| CN104764463B (en) | A kind of self-sensing method of inertial platform leveling collimating fault | |
| CN102937450B (en) | A kind of relative attitude defining method based on gyro to measure information | |
| CN103940442A (en) | Location method and device adopting accelerating convergence algorithm | |
| CN114216456B (en) | Attitude measurement method based on fusion of IMU and robot body parameters | |
| CN107402007A (en) | A kind of method for improving miniature AHRS modules precision and miniature AHRS modules | |
| CN110793515A (en) | Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition | |
| CN115540860A (en) | Multi-sensor fusion pose estimation algorithm | |
| CN109612476A (en) | Map reconstructing method, device, inertial navigation system and computer storage medium based on inertial navigation technology | |
| CN108318027A (en) | The determination method and apparatus of the attitude data of carrier | |
| TW201833557A (en) | Determination of a spatial orientation |
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 |