CN102353375B - Dynamic parameter adjustment method of motion attitude data and device thereof - Google Patents
Dynamic parameter adjustment method of motion attitude data and device thereof Download PDFInfo
- Publication number
- CN102353375B CN102353375B CN 201110183686 CN201110183686A CN102353375B CN 102353375 B CN102353375 B CN 102353375B CN 201110183686 CN201110183686 CN 201110183686 CN 201110183686 A CN201110183686 A CN 201110183686A CN 102353375 B CN102353375 B CN 102353375B
- Authority
- CN
- China
- Prior art keywords
- data
- noise covariance
- attitude
- field intensity
- moving object
- 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
Images
Abstract
The invention provides a dynamic parameter adjustment method of motion attitude data, comprising the following steps of: respectively calculating and analyzing the data change rate and percentage of outrange time of observed quantity data and attitude data when carrying out recursive autoregressive filtering; allowing the data change rate to be multiplied by the corresponding percentage of outrange time so as to respectively obtain a first dynamic adjustment coefficient and a second dynamic adjustment coefficient; allowing the first dynamic adjustment coefficient to be multiplied by the measurement noise covariance when the observed quantity data is obtained so as to obtain the updated measurement noise covariance; allowing the second adjustment coefficient to be multiplied by the excitation noise covariance when measuring the attitude data so as to obtain the updated excitation noise covariance; and feeding back the updated measurement noise covariance and excitation noise covarianceto the recursive autoregressive filtering process. The invention also provides a dynamic parameter adjustment device of the motion attitude data. The method provided by the invention is used to endowthe recursive autoregressive filtering process with strong adaptability.
Description
The application's patent is that application number is 201110116679.7 patented claim (internal number: the dividing an application MP1101398 case), 201110116679.7 the applying date of case is: on May 6th, 2011, denomination of invention is: the athletic posture data are obtained, human motion attitude method for tracing and relevant device.
Technical field
The present invention relates to micromechanics (MEMS) field, relate in particular to dynamic parameter method of adjustment and equipment that a kind of athletic posture data are obtained.
Background technology
The attitude tracer technique of moving object extensively applies to each field, especially in fields such as space flight navigation, antenna radar, the modelings of human motion attitude.In recent years, along with developing rapidly of MEMS, the athletic posture tracer technique is integrated into the theory of MEMS gradually, athletic posture is followed the trail of required measurement component microminiaturization, integrated, form the miniature athletic posture tracing equipment that integrates micro mechanism, microsensor, miniature actuator and signal processing and control circuit etc.These equipment have characteristics such as cost is low, volume is little, in light weight, thereby favored by people.
The attitude of moving object is followed the trail of the attitude data that need at first obtain moving object.In the prior art, miniature athletic posture data acquisition facility commonly used is collected attitude data, this equipment mainly is integrated with the gyroscope device, after being installed on this equipment on the target object, in running order gyro sensor will be collected the attitude data (roll angle, the angle of pitch, course angle) of moving object.The athletic posture data can utilize these data that the dummy model of this moving object is driven after obtaining, thereby the motion process of real-time reproduction of moving object is realized following the trail of.When carrying out the tracking of human motion attitude, a plurality of miniature athletic posture data acquisition facilities that are integrated with the gyroscope device are bundled in the main joint part of human body, collect the attitude data of each motive position of human body by these devices, utilize these data-driven manikin corresponding site motions then, and demonstrate the human motion process intuitively with the 3D picture, thereby realize that the human motion attitude follows the trail of.
Yet the moving object attitude data that the gyroscope device is measured has error, could reflect the athletic posture of moving object after must revising truly.The roughly process that this error produces is: the data that the gyroscope device is directly measured are angular velocity, this magnitude of angular velocity is the moment amount, in most cases can not directly use, and need carry out time integral to this angular velocity, obtain angle variable quantity, add that then initial angle is only the attitude data of moving object as last angle value, this integral process integral time (dt) is more little, the angle value that obtains is more accurate, because the gyroscope survey benchmark is himself but not external absolute object of reference, integral time, (dt) can not infinitely dwindle in addition, the cumulative errors of integration will be passed in time and be increased sharply, and then cause athletic posture data and the real data generation deviation measured.A kind of method that solves gyroscope integration cumulative errors is to increase acceleration transducer in the athletic posture tracing equipment, be used for measuring the acceleration value of gravity direction, under no external force acceleration situation, the roll angle of output movement object and the angle of pitch comparatively exactly, if the external force accelerating effect is arranged, carry out data fusion by the Kalman filter with recurrence autoregression filter function, draw the athletic posture data of moving object at last.But, because gravity direction and the course angle quadrature of acceleration analysis, can't cause course angle data and the actual value of athletic posture inconsistent with the gyroscope cumulative errors of acceleration transducer elimination of level direction, so when carrying out the moving object tracking tracing movement object exactly.
Summary of the invention
In view of this, the present invention increases the geomagnetic field sensors part in existing system, by terrestrial magnetic field data that this senser element is collected as carrying out data fusion in the observed reading input recurrence autoregression unit, utilize the recurrence convergence function of recurrence autoregression unit to eliminate angular velocity instrument cumulative errors in the horizontal direction, obtain optimum attitude data, and then realize the tracking of moving object preferably.
The acquisition methods of athletic posture data provided by the invention comprises:
Geomagnetic field intensity when record moving object is in the first measurement course angle, this geomagnetic field intensity are as first magnetic field intensity, and the numerical value of the described first measurement course angle is identical with the numerical value at the true course angle of described moving object;
When moving object moves to the second measurement course angle, direction revolution α angle with the geomagnetic field intensity of described moving object position correspondence, the numerical value of described α is that the second measurement course angle and first is measured the poor of course angle, the geomagnetic field intensity of the position after the record revolution α angle, this geomagnetic field intensity is as second magnetic field intensity;
Described first magnetic field intensity and second magnetic field intensity are obtained the magnetic field intensity observed quantity after according to the preset rules computing;
The measurement attitude data of described magnetic field intensity observed quantity and described moving object is carried out recurrence autoregression filtering, obtain the attitude data of described moving object.
Preferably, the described first measurement course angle is the zero degree course angle.
Preferably, when attitude data is measured in described magnetic field intensity observed quantity and moving object carried out recurrence autoregression filtering, described method also comprises:
Extract data variation rate and the outrange percentage of time of described magnetic field intensity observed quantity and measurement attitude data respectively;
Described data variation rate and outrange percentage of time multiplied each other obtain first respectively and dynamically adjust coefficient and second and dynamically adjust coefficient;
Dynamically adjust coefficient with described first and multiply by observation noise covariance after the observation noise covariance that produces when obtaining described magnetic field intensity observed quantity obtains upgrading, and the excitation noise covariance of the excitation noise covariance that will described second produces when dynamically the adjustment coefficient multiply by described measurement attitude data after obtaining upgrading;
Observation noise covariance after the described renewal and excitation noise covariance are fed back to described recurrence autoregression filtering.
Preferably, the recurrence autoregression of the observed quantity of described moving object magnetic field intensity and moving object measurement attitude data is filtered into Kalman filtering.
A kind of human motion attitude method for tracing provided by the invention comprises:
Geomagnetic field intensity when record human body motive position is in the first measurement course angle, this geomagnetic field intensity are as first magnetic field intensity, and the numerical value of the described first measurement course angle is identical with the numerical value at the true course angle of described moving object;
When the human motion position moves to the second measurement course angle, direction revolution α angle with the geomagnetic field intensity of position, described human motion position correspondence, the numerical value of described α is that the second measurement course angle and first is measured the poor of course angle, the geomagnetic field intensity of the position after the record revolution α angle, this geomagnetic field intensity is as second magnetic field intensity;
Described first magnetic field intensity and second magnetic field intensity are obtained the magnetic field intensity observed quantity after according to the preset rules computing;
The measurement attitude data at described magnetic field intensity observed quantity and described human motion position is carried out the attitude data that recurrence autoregression filtering obtains described human motion position;
The attitude data of described each motive position of human body is transferred to the attitude data processing enter, uses the corresponding site of described data-driven manikin to move by this processing enter, and human motion attitude virtual reappearance is come out.
Preferably, described attitude data with the partes corporis humani position is transferred to the attitude data processing enter and comprises wireless transmission.
The deriving means of a kind of athletic posture data provided by the invention comprises: measuring unit, geomagnetic field intensity sensing unit and recurrence autoregression filter unit, described measuring unit, geomagnetic field intensity sensing unit are electrically connected with recurrence autoregression filter unit respectively, wherein:
Described measuring unit is used for measuring the attitude data of moving object;
Described geomagnetic field intensity sensing unit, geomagnetic field intensity when being in the first measurement course angle for record moving object, this geomagnetic field intensity is as first magnetic field intensity, and the numerical value of the described first measurement course angle is identical with the numerical value at the true course angle of described moving object; When moving object moves to the second measurement course angle, direction revolution α angle with the geomagnetic field intensity of described moving object position correspondence, the numerical value of described α is that the described second measurement course angle and first is measured the poor of course angle, the geomagnetic field intensity of the position after the record revolution α angle, this geomagnetic field intensity is as second magnetic field intensity; Described first magnetic field intensity and second magnetic field intensity are obtained the magnetic field intensity observed quantity after according to the preset rules computing;
Described recurrence autoregression filter unit is used for the measurement attitude data of described magnetic field intensity observed quantity and described moving object is carried out the attitude data that recurrence autoregression filtering obtains described moving object.
Preferably, the described first measurement course angle is the zero degree course angle.
Preferably, described device further comprises dynamic adjustment unit, when being used for described magnetic field intensity observed quantity and moving object measured attitude data and carry out recurrence autoregression filtering, data variation rate and the outrange percentage of time of the described magnetic field intensity observed quantity of statistical study and measurement attitude data respectively; Described data variation rate and outrange percentage of time multiplied each other obtain first respectively and dynamically adjust coefficient and second and dynamically adjust coefficient; Dynamically adjust coefficient with described first and multiply by observation noise covariance after the observation noise covariance that produces when obtaining described magnetic field intensity observed quantity obtains upgrading; With the described second excitation noise covariance of dynamically adjusting after the excitation noise covariance that produces when coefficient multiply by described measurement attitude obtains upgrading; Observation noise covariance after the described renewal and excitation noise covariance are fed back to described recurrence autoregression filtering.
Preferably, described recurrence autoregression filter unit is Kalman filter.
A kind of human motion attitude tracing system provided by the invention comprises: at least one human motion position attitude data deriving means, sending module, receiver module, human body attitude reconstructed module and human body attitude present module, wherein:
Described human motion position attitude data deriving means is for giving sending module with this data transmission behind the attitude data of collecting each motive position of human body; This device comprises measuring unit, geomagnetic field intensity sensing unit and recurrence autoregression filter unit, and described measuring unit, geomagnetic field intensity sensing unit are electrically connected with recurrence autoregression filter unit respectively, wherein:
Described measuring unit is for the attitude data of measuring the human motion position;
Described geomagnetic field intensity sensing unit, geomagnetic field intensity when being in first course angle for record human body motive position, this magnetic field intensity is as first magnetic field intensity, and the numerical value of the described first measurement course angle is identical with the numerical value at the true course angle of described moving object; When the human motion position moves to the second measurement course angle, direction revolution α angle with the geomagnetic field intensity of described moving object position correspondence, the numerical value of described α is that the second measurement course angle and first is measured the poor of course angle, the geomagnetic field intensity of the position after the record revolution α angle, this geomagnetic field intensity is as second magnetic field intensity; First magnetic field intensity and second magnetic field intensity are obtained the magnetic field intensity observed quantity after according to the preset rules computing;
Described recurrence autoregression filter unit is used for the measurement attitude data at described magnetic field intensity observed quantity and described human motion position is carried out the attitude data that recurrence autoregression filtering obtains described human motion position;
Described sending module, the attitude data that is used for each motive position of human body that described human motion attitude data deriving means is obtained sends to described human body attitude reconstructed module;
Described receiver module is for the attitude data of each motive position of human body that receives described human motion position attitude data deriving means;
Described human body attitude reconstructed module, the attitude data of each motive position of human body that receiver module is received is used for driving the motion of manikin corresponding site;
Described human body attitude presents module, is used for the motion of virtual reappearance human body.
Preferably, this system further comprises first wireless communication unit and second wireless communication unit, and described sending module sends to described human body attitude reconstructed module by first wireless communication unit with the attitude data of described each motive position of human body; Described receiver module receives the attitude data that described human motion attitude data deriving means obtains by second wireless communication unit.
Further preferably, described first wireless communication unit is integrated in the described attitude data sending module; Described second wireless communication unit is integrated in the described attitude data receiver module.
The present invention introduces magnetic field intensity on the prior art basis, magnetic field strength date is carried out recurrence autoregression filtering as the priori estimates of observed reading and moving object attitude, utilize the cumulative errors of the recurrence convergence function elimination gyrohorizon direction of recurrence autoregression filtering, obtain course angle data comparatively accurately thus, thereby realize the tracking of moving object attitude.
Description of drawings
Fig. 1 carries out the athletic posture data for method of the present invention and collects the coordinate system synoptic diagram that adopts;
Fig. 2 is the process flow diagram of method embodiment 1 of the present invention;
Fig. 3 is the Kalman filtering process synoptic diagram of method embodiment 2 of the present invention;
Fig. 4 is the dynamic adjustment synoptic diagram of method embodiment 4 recurrence autoregression filterings of the present invention;
Fig. 5 is the composition frame chart of device embodiment 5 of the present invention;
Fig. 6 is the composition frame chart of the dynamic adjustment unit of device embodiment 6 of the present invention;
Fig. 7 is the composition frame chart of system embodiment 7 of the present invention.
Embodiment
Main thought of the present invention is: collect the geomagnetic field intensity data on the prior art basis and carry out recurrence autoregression filtering as observed reading and moving object measurement attitude data, utilize the cumulative errors of the recurrence convergence function elimination gyrohorizon direction of recurrence autoregression filtering, obtain course angle data comparatively accurately thus, thereby realize the tracking of moving object attitude.
The recurrence autoregression filtering that attitude data is measured in magnetic field strength date and moving object has multiple implementation, and the present invention preferably uses the kalman filter method with recurrence autoregression filter function to carry out this processing procedure.For ease of explained in detail technical scheme of the present invention, the principle of work to Kalman filtering briefly introduces earlier.Kalman filtering is a kind of recurrence autoregression data processing algorithm, and its method estimation procedure state by FEEDBACK CONTROL is to corrections that circulate of the state outcome of each output, until the process-state data that obtains optimum.Kalman filtering can be divided into two cyclic processes: time renewal process and measurement renewal process, and the former is responsible in time calculating that forward the estimated value of current state variable and error covariance is to construct the prior estimate of next time state; The latter estimates prior estimate and measurand combination to construct improved posteriority; The time renewal process can be considered the process of estimating, and measures renewal process and can be considered trimming process, and whole algorithm for estimating essence is a kind of estimating-correcting algorithm of numerical solution that have.The Kalman filtering process can be with five following equation expressions.
Formula 1: by previous moment system state estimation present moment system state
X(k|k-1)＝AX(k-1|k-1)+BU(k)
In the formula, X (k|k-1) is the k system state constantly of utilizing system (k-1) system state estimation constantly, be called prior estimate, the optimum state value of etching system when X (k-1|k-1) is (k-1), U (k) is k system's control input quantity constantly, A, B are systematic parameters, represent system state transition matrix and external drive input matrix respectively.
Formula 2: the systematic error covariance of being estimated present moment by previous moment systematic error covariance
P(k|k-1)＝AP(k-1|k-1)A
T+Q
In the formula, P (k|k-1) is the k systematic error covariance constantly of utilizing system (k-1) error covariance constantly to estimate, and Q is the excitation noise covariance.
System k priori estimates X (k|k-1) constantly according to formula 1 obtains can extrapolate k system state optimal value X (k|k) constantly in conjunction with k measured value Z (k) constantly again, and prediction equation is:
Formula 3: calculate the system state optimal value by priori estimates and measured value
X(k|k)＝X(k|k-1)+K(k)[Z(k)-HX(k|k-1)]
Wherein H is matrix, is the systematic survey parameter, the gain of expression state variable, and the H matrix associates observational variable and state variable; K (k) is kalman gain, is obtained by formula 4:
K(k)＝P(k|k-1)H
T[HP(k|k-1)H
T+R]
-1
R in the formula is the observation noise covariance.
Formula 5: the estimate covariance that is produced by the structure prior estimate and kalman gain are calculated will be for the k+1 error covariance in the moment:
P(k|k)＝[I-K(k)H]P(k|k-1)
In the formula, I is matrix, measures I=1 for the single model list.
For making those skilled in the art can further understand feature of the present invention and technology contents, below in conjunction with drawings and Examples, technical scheme of the present invention is described in detail.
Embodiment one
The parametric description (being the orientation of moving object in the reference space) that need know the moving object attitude is followed the trail of in realization moving object.The moving object attitude usually by with the motion reference coordinate system OX of moving object Joint
CY
CZ
CAnd the angle of deciding between the reference frame OXYZ is represented.The initial point of two coordinate systems all is taken at the moving object barycenter, and decide reference frame X-axis level and point to east, the horizontal energized north of Y-axis, Z axle vertical ground points to zenith; X with the motion reference coordinate system of moving object Joint
CThe vertical movement movement direction of object points to right, Y
CAxle is along moving object direction of motion directed forward, Z
CAlong moving object longitudinal axis points upwards.Decide the relation of reference frame and motion reference coordinate system as shown in Figure 1.Overlap with deciding the reference frame initial point when supposing that the moving object coordinate system is initial, according to above-mentioned definition, any attitude of moving object all can obtain by following three rotations: (1) is around Y-axis rotary luffing angle θ; (2) rotate roll angle Ψ around X-axis; (3) around Z axle rotation course angle ψ.Thus, realize that the tracking of moving object attitude only needs to obtain above-mentioned three data and gets final product.Three-axis gyroscope can be used for measuring these data, but there is error problem in gyroscope as previously mentioned, and the moving object attitude data of measuring (θ, Ψ, ψ) can depart from actual value at short notice.Add three axis accelerometer on this basis and can eliminate pitching angle theta that gyroscope records and the cumulative errors of two aspects of roll angle Ψ to a certain extent.The method that above-mentioned use three-axis gyroscope is collected data setting movement object attitude is called " Three Degree Of Freedom localization method ", and the method for using three-axis gyroscope and three axis accelerometer to collect data setting movement object attitude is called " six degree of freedom localization method ".Adopt " six degree of freedom localization method " although can eliminate the fractional error that gyroscope self rotation brings, do not eliminate the cumulative errors of horizontal direction, namely course angle ψ can depart from actual value gradually in moving object attitude data measuring process.Present embodiment is introduced the magnetic field instrument on this basis, be used for measuring geomagnetic field intensity, and with the measurement attitude data of this magnetic field strength date as correction motion object in the observed reading input recurrence autoregressive filter, and then reduce and eliminate gyrostatic cumulative errors, obtain attitude data comparatively accurately, realize the tracking of moving object.
Referring to accompanying drawing 2, the moving object attitude data acquisition methods that present embodiment provides comprises:
Step 101: the geomagnetic field intensity when record moving object is in the first measurement course angle, this geomagnetic field intensity are as first magnetic field intensity, and the numerical value of the described first measurement course angle is identical with the numerical value at the true course angle of described moving object;
The first measurement course angle here is the datum course angle, and the course angle ψ in the measurement attitude data that this position angle speed instrument integration draws with real course angle numerical value deviation does not take place; The zero degree course angle is selected at this datum course angle usually for use, in fact also can be that other can precalibrated course angle, as long as guarantee that the course angle that angular velocity sensing instrument measures is consistent with actual heading angle numerical value.
Step 102: when described moving object moves to the second measurement course angle, direction revolution α angle with the terrestrial magnetic field of described moving object position correspondence, the numerical value of described α is that the second measurement course angle and first is measured the poor of course angle, the geomagnetic field intensity of the position after the record revolution α angle, this geomagnetic field intensity is as second magnetic field intensity;
Second to measure course angle be the course angle that angular velocity sensing instrument records, and this course angle is because the integral process the during work of angular velocity sensing instrument makes the numerical value generation deviation at true course angle of the numerical value of this measurement course angle and this position; The magnetic field sensing instrument can record the magnetic field data of moving object position objectively, the direction that comprises this position magnitude of field intensity and magnetic field intensity, obtain after the direction of this location, position magnetic field intensity this direction revolution certain angle, this angle is above-mentioned two and measures the poor of course angle numerical value, because second course angle of measuring course angle and reality has deviation, must have with the first measurement course angle after rotation is gone back and depart from, this departure degree can react the cumulative errors that angular velocity sensing instrument integration causes.
Step 103: described first magnetic field intensity and second magnetic field intensity is poor, obtain the magnetic field intensity observed quantity;
Above-mentioned two magnetic field intensitys ask poor result can weigh angular velocity sensing instrument horizontal direction cumulative errors, in fact, except making the difference mode, also can adopt other operation rules to carry out the processing of two magnetic field strength date, such as asking the difference of two squares, mean square deviation etc. all can realize weighing the purpose of error.
Step 104: the measurement attitude data of described magnetic field intensity observed quantity and described moving object is carried out the attitude data that recurrence autoregression filtering obtains described moving object;
The measurement attitude data of described moving object is the attitude data of the moving object that obtains of angular velocity sensing instrument integration, and these data had been eliminated the error of roll angle and the angle of pitch by the acceleration sensing instrument before carrying out the described step of present embodiment.
The present invention introduces magnetic field intensity on the prior art basis, magnetic field intensity when measuring course angle with first is benchmark, to be in the magnetic field intensity that second direction of geomagnetic field intensity of measuring the moving object position correspondence of course angle rotates back to this position of record behind the certain angle, ask difference to obtain the magnetic field intensity observed quantity above-mentioned two magnetic field intensitys then, and then use this magnetic field intensity observed quantity input recurrence autoregression filter unit to carry out data fusion, the fusion process correction moving object attitude data that obtains of angular velocity sensing instrument integration, eliminated the cumulative errors of horizontal direction.
Embodiment two
Mention the data fusion process of recurrence autoregression filtering in above-described embodiment step 104, in fact, the specific implementation of carrying out recurrence autoregression filtering between moving object attitude data and the magnetic field strength date has multiple, and the present invention preferably adopts Kalman filtering algorithm to realize this process.Referring to accompanying drawing 3, the data fusion process of kalman filter method is:
Moving object M represents with hypercomplex number at k-1 attitude parameter constantly, and hypercomplex number is to utilize a kind of supercomplex to wait the validity response vector to rotate.Any one vector all can be expressed as the real part hypercomplex number compound with plural number, such as:
(wherein w is constant), the parameter in this formula satisfies following relation:
q＝[w x y z]
2
|q|
2＝w
2+x
2+y
2+z
2＝1
The hypercomplex number structure foundation data of moving object attitude come from the measurement data of three axis angular rate sensing instrument.Can construct a hypercomplex number in the following formula of three parameters (θ, Ψ, ψ) any one substitution with the moving object attitude that measures:
w＝cos(α/2)
x＝sin(α/2)cos(β
x)
y＝sin(α/2)cos(β
y)
z＝sin(α/2)cos(β
z)
Wherein: α is the angle that moving object is rotated around coordinate axis, cos (β
x), cos (β
y) cos (β
z) be that above-mentioned attitude parameter is at each axial component.Moving object M at k-1 state constantly with hypercomplex number quarter (q
1 K-1|k-1, q
2 K-1|k-1, q
3 Sk-1|k-1, q
4 K-1|k-1).Cause the measured value biasing because there is error in a variety of causes when three-axis gyroscope carries out the athletic posture DATA REASONING, the biasing of three direction of measurement of three-axis gyroscope is estimated as (q
Bias1 K-1|k-1, q
Bias2 K-1|k-1, q
Bias3 K-1|k-1).Estimate to constitute jointly state variable by moving object 3 d pose data with to the biasing of these data, be that state vector X (k-1|k-1) is the 7 degree of freedom vector in the present embodiment, the initial value of this state variable when carrying out Kalman filtering first can be chosen arbitrarily, because the Kalman filtering process has the recurrence convergence function, the original state of choosing does not arbitrarily produce significant influence to the output result of Kalman filtering.
Thus, according to the formula 1 of Kalman filtering, can estimate moving object M in k state X (k|k-1)=X (k|k-1)=AX (k-1|k-1)+BU (k) constantly, i.e. prior estimate.The impossible absolutely accurate of the process of structure prior estimate itself, this uncertain size is on the one hand because the numerical value that the rotation of gyroscope self causes departs from and drift causes, and one side is caused by the excitation noise of introducing in the prior estimate.These deviation use error covariances represent, therefore, next need the estimating system error covariance being used for calculating kalman gain, and then are used for the update mode variable.According to the formula 2 of Kalman filtering, extrapolate k systematic error covariance P (k|k-1)=P (k|k-1)=AP (k-1|k-1) A constantly
T+ Q.This error covariance value can be chosen arbitrarily when carrying out Kalman filter first, because Kalman filtering recurrence convergence function, the original state of choosing does not arbitrarily produce significant influence to the output result of Kalman filter.After obtaining k error covariance constantly, the observation noise error covariance during in conjunction with Magnetic Sensor numerical value can calculate kalman gain K (k)=P (k|k-1) H according to the formula 4 of Kalman filtering
T[HP (k|k-1) H
T+ R]
-1After obtaining k kalman gain K (k) constantly, can update mode variable X (k|k)=X (k|k)=X (k|k-1)+K (k) [Z (k)-HX (k|k-1)] in observed reading Z (k) the substitution Kalman filtering formula 3 of the magnetic field vector that Magnetic Sensor is obtained.Arrive this, finished the once circulation of Kalman filtering, obtained advancing the moving object attitude data of revising.But in order to carry out next one circulation, also need the error covariance of update system, namely calculate that according to the formula 5 of Kalman filtering being used for k+1 error constantly defences poor P (k|k)=[I-K (k) H] P (k|k-1) jointly.
Top process constantly circulates, the athletic posture data of the magnetic field strength date diagonal angle velocity pick-up instrument integration output constantly that obtains by the magnetic field sensing instrument are carried out feedback modifiers, the horizontal direction cumulative errors that is produced by angular velocity sensing instrument integration is eliminated, and the course angle number of moving object attitude and actual value approach.
Embodiment three
In above-described embodiment, what the moving object attitude parameter in the state variable was described use is the hypercomplex number representation.In fact can directly use Eulerian angle to represent for the moving object attitude parameter, even other method for expressing, different method for expressing only is formal difference, all can change mutually by these method for expressing of mathematical programming.Provide the conversion formula between hypercomplex number and the Eulerian angle below.
Quaternary is counted to the conversion formula of Eulerian angle:
The result of arctan and arcsin is in the following formula
Need replace arctan with atan2 for other angles.Namely realize conversion with following formula:
Eulerian angle are to the conversion formula of hypercomplex number:
So the concrete mathematical form of not constrained motion of the present invention object attitude description, as long as do not hinder the present invention to realize solving the purpose of prior art problem.
Embodiment four
The present invention introduces the problem that geomagnetic field intensity sensing instrument can solve the gyroscope cumulative errors as the method for above-mentioned description, but geomagnetic field intensity sensing instrument has certain instability, is subjected to the influence of external magnetic field variation greatly.Such as when bigger variation takes place suddenly in the external magnetic field, very big variation appears in the raw data that the magnetic field sensing instrument obtains, wave filter is owing to not knowing that this is that the external magnetic field changes the result who causes, can cause error occurring through the moving object attitude data after the recurrence autoregression according to update mode equation under the normal situation.Also such as, if error appears in the moving object attitude data that Magnetic Sensor also may cause obtaining when outrange takes place.For making recurrence autoregression filtering have stronger adaptive characteristic, embodiments of the invention further take dynamically to adjust measure, eliminate the above-mentioned error that may occur.Kalman filtering is still adopted in the recurrence autoregression filtering of present embodiment.
Participate in shown in the accompanying drawing 4.The dynamic adjustment process of present embodiment is realized by the observation noise covariance matrix R of renewal Kalman filter and the mode of excitation noise covariance matrix Q and then adjusting fiducial interval degree scope.Dynamically the detailed process of adjusting is as follows:
Excitation noise covariance matrix Q and observation noise covariance matrix R need to multiply by separately dynamically-adjusting parameter respectively at each circulation time, the data variation rate of dynamically-adjusting parameter and sensing instrument and above the product relation of being inversely proportional to of the percentage of range time.Represent this process with mathematical expression below: the data variation rate of establishing the attitude data of angular velocity sensing instrument measurement is that α 1 and outrange percentage of time are β 1, the data variation rate of the magnetic field strength date of magnetic field sensing instrument observation is α 2, the outrange percentage of time is β 2, then:
D1＝1/α1β1 D2＝1/α2β2
Q′
k＝D1Q
k R′
k＝D2R
k
In the formula, α 1, α 2 are by obtaining described data differentiate; β 1, β 2 for Preset Time at interval in sensing instrument measurement data reach or surpass the time ratio of range, by the statistics Preset Time at interval in measurement data time of meeting or exceeding range account for the percentage of this predetermined interval T.T. and obtain.
With excitation noise covariance matrix Q and the observation noise covariance matrix R substitution Kalman filter that above-mentioned process is dynamically adjusted, the recurrence convergence function of process Kalman filter can overcome above-mentioned technical matters.
In fact, the technical matters that runs into during sensing instrument measurement data in the present embodiment exists in the acceleration sensing instrument in the athletic posture measuring system equally: (1) is jumped to changing fast or when opposite by slow variation when the target object motion state, the filter status adaptations lags behind, and the direct measuring result error of motion state system increases like this; (2) when the target object athletic posture is in big variation range, very likely approach even exceed the maximum range scope of sensor, the raw data measured of sensing instrument will depart from the real motion situation like this, causes measuring and interrupts or the measurement result inefficacy.These technical matterss can take the method for present embodiment to solve equally: be parameter to the fiducial interval [D of inductive sensing instrument mutually with data variation rate and the time ratio that meets or exceeds full scale of sensing instrument, + D] carry out FEEDBACK CONTROL: the rate of change of sensing instrument data is more big, it is more little to put the letter space, and degree of confidence is more low; Reach or the time of outrange more many, it is more little to put the letter space, degree of confidence is more low.
Embodiment five
Accompanying drawing 5 is composition frame charts of the embodiment of apparatus of the present invention 500.In the present embodiment, the athletic posture data acquisition facility comprises measuring unit 501, this unit comprises three axis angular rate sensing units 5011,3-axis acceleration sensing unit 5012, three geomagnetic field intensity sensing units 502 and recurrence autoregression filter unit 503 are formed, measuring unit 501, three geomagnetic field intensity sensing units 502 are electrically connected with recurrence autoregression filter unit 503 respectively, measuring unit 501 is used for measuring the attitude data of moving object, and send these data to recurrence autoregression unit 503, three geomagnetic field intensity sensing units 502 are used for obtaining magnetic field strength date, and send these data to recurrence autoregression unit 503.
The course of work of the device 500 that present embodiment provides is as follows: the work coordinate system of moving object attitude measurement is set up in preliminary election, and such coordinate system is as described in the embodiment one, herein not in superfluous words.Collecting and changing the moving object attitude data then: three-axis gyroscope 5011 is collected three axial data of moving object XYZ in above-mentioned coordinate system, 3-axis acceleration sensing unit 5012 is collected acceleration information on the moving object gravity direction, and these six data are changed the back as the parametric description (course angle, the angle of pitch, roll angle) of moving object attitude with the hypercomplex number method; Magnetic field vector when three geomagnetic field intensity sensing units 502 are set the first measurement course angle is collected magnetic field strength date as with reference to vector as benchmark.To be in the second terrestrial magnetic field direction of measuring the moving object correspondence of course angle position and rotate the angle of two course angle difference sizes, and recording the magnetic field strength date of this postrotational position, the difference of these two magnetic field intensitys has been reacted the possible deviation of angular velocity sensing unit 5011.Next these data are input in the recurrence autoregression filter unit 503, carry out Data Fusion, because recurrence autoregression filter unit 503 has the recurrence convergence function, import the data of this unit and progressively optimized, at last the optimum moving object attitude data of output.
Present embodiment is introduced three geomagnetic field intensity sensing units on the prior art basis, the magnetic field intensity of moving object position correspondence is as benchmark when measuring course angle with first, the angular dimension of the difference of two course angles of direction rotation of the terrestrial magnetic field of moving object position correspondence when measuring course angle with second, the magnetic field intensity of the position after the record revolution, with this magnetic field intensity with respect to the size of reference magnetic field intensity as the magnetic field intensity observed reading, and this magnetic field intensity observed reading is imported recurrence autoregression unit carry out data fusion, thereby eliminated the cumulative errors of angular velocity sensing unit, the data of recurrence autoregression unit output have also been reacted the attitude of moving object comparatively truly, and then can realize the accurate tracking of moving object.
Angular velocity sensing unit in the present embodiment comprises gyro sensor, and the acceleration sensing unit comprises accelerometer, and the geomagnetic field intensity sensing unit comprises magnetometer.Recurrence autoregression unit in above-described embodiment can be all electronic components with same filter function, and preference card Thalmann filter of the present invention is realized this process.The Kalman filtering process is shown in above-mentioned embodiment 4, here not at repeated description.
Embodiment six
As previously mentioned, angular velocity sensing unit and geomagnetic field intensity sensing unit have certain instability, are subjected to the influence of external magnetic field variation greatly.For making recurrence autoregression filter unit have stronger adaptive characteristic, embodiments of the invention further take dynamically to adjust measure.In above-described embodiment, can also comprise dynamic adjustment unit 604, be used for dynamically adjusting observation noise covariance matrix and the excitation noise covariance matrix of Kalman filtering process, with the covariance matrix input recurrence autoregression filter unit 603 after adjusting.Shown in accompanying drawing 6, the dynamic adjustment unit 604 of present embodiment comprises data statistic analysis unit 6041, performance coeffcient computing unit 6042 and Covariance Transformation unit 6043, wherein:
Data statistic analysis unit 6041 is used for the described magnetic field intensity observed quantity of statistical study respectively and measures attitude data obtaining the data variation rate and surpassing the range percentage of time;
Performance coeffcient computing unit 6042 is used for described data variation rate and outrange percentage of time multiplied each other and obtains first respectively and dynamically adjust dynamically adjustment coefficient of coefficient and second;
Covariance Transformation unit 6043 be used for will described first dynamically the adjustment coefficient multiply by observation noise covariance after the observation noise covariance that produces when obtaining described magnetic field intensity observed quantity obtains upgrading; With the described second excitation noise covariance of dynamically adjusting after the excitation noise covariance that produces when coefficient multiply by described measurement attitude data obtains upgrading; Observation noise covariance after the described renewal and excitation noise covariance are fed back to described recurrence autoregressive process.
With excitation noise covariance matrix Q and the observation noise covariance matrix R substitution Kalman filter that above-mentioned process is dynamically adjusted, overcome the instability of angular velocity sensing unit and magnetic field sensing unit through the recurrence convergence function of data fusion unit.
In fact, the technical matters that runs into when angular velocity sensing unit and magnetic field sensing unit measurement data in the present embodiment exists in the acceleration sensing unit in the athletic posture measuring system equally, and mode and the said process of solution are similar, here no longer repetition.
Embodiment seven
Accompanying drawing 7 is composition frame charts of human motion attitude tracing system embodiment provided by the invention.The system 700 of present embodiment comprises that at least one human motion position attitude data deriving means 701, sending module 702, receiver module 703 and human body attitude reconstructed module 704 and human body attitude present module 705; Wherein, human motion position attitude data deriving means 701 is used for realizing obtaining of human motion position attitude data as described in the embodiment three; Sending module 702 is electrically connected with human motion position attitude data deriving means 701, and the attitude data that is used for human motion attitude data deriving means 701 is obtained sends to human body attitude reconstructed module 704 by receiver module 703; Receiver module 703 is electrically connected with human body attitude reconstructed module 704, is used for receiving the attitude data that human body athletic posture data acquisition facility 701 sends by sending module 702; Human body attitude reconstructed module 704 is used for driving the motion of manikin corresponding site with the attitude data of each motive position of human body of receiving; Human body attitude presents module 705, is used for the motion of virtual reappearance human body.The job step of native system is as follows:
Step S701: attitude data deriving means 701 methods according to claim 1 in human motion position are obtained the attitude data of each motive position of human body; This device 701 comprises one at least, anchors in advance on each motive position of human body, and the position of concrete set is selected according to actual needs, can be the main articulation point of human body, and these articulation points can reflect the athletic posture of human body;
Step S702: the athletic posture data of each motive position of human body that step S701 is obtained are transferred to human body attitude reconstructed module 704 by sending module 702;
Step S703: after human body attitude reconstructed module 704 receives described partes corporis humani position athletic posture data by receiving element 703, drive each the corresponding site motion in the manikin;
Step S704: human motion presents module 705 the human body attitude virtual reappearance is come out.
Attitude data transmits except being wired mode between human motion attitude data deriving means 701 and human body attitude reconstructed module in the present embodiment, and preferred wireless mode of the present invention is transmitted.The system of present embodiment further comprises first wireless communication unit 706 and second wireless communication unit 707 during wireless transmission, and the wireless transmission of data is realized in two unit thus.First wireless communication unit 706 can be used as independent module and is electrically connected with attitude data transmitting element 702, also can be integrated in the attitude data sending module 702.Similarly, second wireless communication unit 707 in the present embodiment can be used as independent module and is electrically connected with attitude data receiver module 703, can be integrated in the attitude data receiver module 703, embodiment provided by the invention does not limit the connected mode of these two modules yet.Implementation as for concrete wireless connections can adopt bluetooth, WIFI or next-generation communication network LTE etc., employed host-host protocol can be based on the star-like agreement in upper strata of IEEE802.15.4, also can be other host-host protocols of realizing the wireless connections mode, all not hinder the realization of present embodiment goal of the invention.
Present embodiment has provided the holonomic system that the human motion attitude is followed the trail of, and this system has adopted embodiment three described devices, has solved the problem of gyroscope cumulative errors.Simultaneously, present embodiment has been realized the comprehensive unrestricted measurement of athletic posture by the wireless connections between sending module 702 and the receiver module 703, has enlarged the usable range of athletic posture tracing system.
The above only is preferred embodiment of the present invention, and is in order to limit the present invention, within the spirit and principles in the present invention not all, any modification of doing, is equal to replacement, improvement etc., all should be included within the protection domain of invention.
Claims (5)
1. the dynamic parameter method of adjustment of athletic posture data is characterized in that, this method comprises:
When the measurement attitude data of the moving object that will measure carries out recurrence autoregression filtering with the observed quantity data that observe, data variation rate and the outrange percentage of time of the difference described observed quantity data of statistical study and attitude data;
Described data variation rate and corresponding outrange percentage of time are multiplied each other, obtain first respectively and dynamically adjust coefficient and the second dynamic coefficient of adjusting;
Dynamically adjust coefficient with described first and multiply by the observation noise covariance that produces when obtaining the observed quantity data, the observation noise covariance after obtaining upgrading; Dynamically adjust the excitation noise covariance that produces when coefficient multiply by the measurement attitude data, the excitation noise covariance after obtaining upgrading with described second;
Observation noise covariance after the described renewal and excitation noise covariance are fed back to described recurrence autoregression filtering.
2. method according to claim 1 is characterized in that, described observed quantity data comprise the geomagnetic field intensity data, and/or, acceleration information.
3. method according to claim 1 and 2 is characterized in that, described recurrence autoregression is filtered into Kalman filtering.
4. the dynamic parameter of athletic posture data is adjusted equipment, it is characterized in that this equipment comprises data statistic analysis unit, performance coeffcient computing unit and Covariance Transformation unit, wherein:
Described data statistic analysis unit, be used for when the measurement attitude data of the moving object that will measure carries out recurrence autoregression filtering with the observed quantity data that observe, respectively data variation rate and the outrange percentage of time of the described observed quantity data of statistical study and attitude data;
Described performance coeffcient computing unit is used for described data variation rate and corresponding outrange percentage of time are multiplied each other, and obtains the first dynamic coefficient and second of adjusting respectively and dynamically adjusts coefficient;
Described Covariance Transformation unit, be used for will described first dynamically the adjustment coefficient multiply by the observation noise covariance that produces when obtaining the observed quantity data, the observation noise covariance after obtaining upgrading; Dynamically adjust the excitation noise covariance that produces when coefficient multiply by the measurement attitude data, the excitation noise covariance after obtaining upgrading with described second; Observation noise covariance after the described renewal and excitation noise covariance are fed back to described recurrence autoregression filtering.
5. equipment according to claim 4 is characterized in that, described observed quantity data comprise the geomagnetic field intensity data, and/or, acceleration information.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN 201110183686 CN102353375B (en) | 2011-05-06 | 2011-05-06 | Dynamic parameter adjustment method of motion attitude data and device thereof |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN 201110183686 CN102353375B (en) | 2011-05-06 | 2011-05-06 | Dynamic parameter adjustment method of motion attitude data and device thereof |
Related Parent Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201110116679.7A Division CN102252676B (en) | 2011-05-06 | 2011-05-06 | Method and related equipment for acquiring movement attitude data and tracking human movement attitude |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN102353375A CN102353375A (en) | 2012-02-15 |
| CN102353375B true CN102353375B (en) | 2013-09-18 |
Family
ID=45576992
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN 201110183686 Active CN102353375B (en) | 2011-05-06 | 2011-05-06 | Dynamic parameter adjustment method of motion attitude data and device thereof |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN102353375B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102679976B (en) * | 2012-05-20 | 2014-07-09 | 西安费斯达自动化工程有限公司 | Aircraft quaternion revising model based on accelerated speed |
| CN106021194B (en) * | 2016-05-19 | 2017-10-03 | 哈尔滨工业大学 | A kind of multi-sensor multi-target tracking bias estimation method |
| CN109254548A (en) * | 2018-08-30 | 2019-01-22 | 浙江工业大学 | A kind of wooden shot based on STM32F407ZET6 swings speed detection system |
| CN110044350A (en) * | 2019-04-15 | 2019-07-23 | 北京航空航天大学 | The MEMS gyro random drift modeling method of application enhancements dynamic recurrent neural network |
| CN110186453A (en) * | 2019-06-29 | 2019-08-30 | 西安六韬信息科技有限公司 | A kind of acquisition device and method of athletic posture data |
| CN113959447B (en) * | 2021-10-19 | 2023-06-27 | 北京京航计算通讯研究所 | Relative navigation high noise measurement identification method, device, equipment and storage medium |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN201229355Y (en) * | 2008-07-07 | 2009-04-29 | 李乔峰 | Wireless body sport attitude detection system |
| US7543392B2 (en) * | 2003-11-08 | 2009-06-09 | Samsung Electronics Co., Ltd. | Motion estimation method and system for mobile body |
| EP0986736B1 (en) * | 1998-04-01 | 2010-02-24 | Alenia Marconi Systems S.p.A. | Inertial and magnetic sensors systems designed for measuring the heading angle with respect to the north terrestrial pole |
| CN101915580A (en) * | 2010-07-14 | 2010-12-15 | 中国科学院自动化研究所 | Self-adaptation three-dimensional attitude positioning method based on microinertia and geomagnetic technology |
-
2011
- 2011-05-06 CN CN 201110183686 patent/CN102353375B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP0986736B1 (en) * | 1998-04-01 | 2010-02-24 | Alenia Marconi Systems S.p.A. | Inertial and magnetic sensors systems designed for measuring the heading angle with respect to the north terrestrial pole |
| US7543392B2 (en) * | 2003-11-08 | 2009-06-09 | Samsung Electronics Co., Ltd. | Motion estimation method and system for mobile body |
| CN201229355Y (en) * | 2008-07-07 | 2009-04-29 | 李乔峰 | Wireless body sport attitude detection system |
| CN101915580A (en) * | 2010-07-14 | 2010-12-15 | 中国科学院自动化研究所 | Self-adaptation three-dimensional attitude positioning method based on microinertia and geomagnetic technology |
Non-Patent Citations (2)
| Title |
|---|
| 徐烨烽等.微小型航姿测量系统设计及误差补偿.《仪器仪表学报》.2010,第31卷(第10期),全文. |
| 微小型航姿测量系统设计及误差补偿;徐烨烽等;《仪器仪表学报》;20101031;第31卷(第10期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN102353375A (en) | 2012-02-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN102252676B (en) | Method and related equipment for acquiring movement attitude data and tracking human movement attitude | |
| CN202177381U (en) | Motion gesture data acquisition device and human body motion gesture tracking system | |
| CN102353375B (en) | Dynamic parameter adjustment method of motion attitude data and device thereof | |
| US10322819B2 (en) | Autonomous system for taking moving images from a drone, with target tracking and improved target location | |
| CN104898681B (en) | A kind of quadrotor attitude acquisition method for approximately finishing card quaternary number using three ranks | |
| US20170123035A1 (en) | Autonomous magnetometer calibration | |
| EP1865286A2 (en) | Object locating in restricted environments using personal navigation | |
| CN104854428A (en) | Sensor fusion | |
| CN108235735A (en) | Positioning method and device, electronic equipment and computer program product | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| US9470507B2 (en) | Vehicle wheel alignment method and system based on gyroscopic sensors or angular rate sensors or MEMS angular rate sensors | |
| CN108761512A (en) | A kind of adaptive CKF filtering methods of missile-borne BDS/SINS deep combinations | |
| CN106403952A (en) | Method for measuring combined attitudes of Satcom on the move with low cost | |
| CN111189474A (en) | Autonomous calibration method of MARG sensor based on MEMS | |
| CN111238469A (en) | Unmanned aerial vehicle formation relative navigation method based on inertia/data chain | |
| EP4015993A1 (en) | Aircraft sensor system synchronization | |
| CN109781096A (en) | A kind of integrated navigation and location system and method for intelligent agricultural machinery | |
| García et al. | Analysis of sensor data and estimation output with configurable UAV platforms | |
| Coppa et al. | Accuracy enhancement of unmanned helicopter positioning with low-cost system | |
| CN111551968A (en) | Unmanned aerial vehicle alignment system and method based on deep learning motion prediction | |
| Veth et al. | Tightly-coupled ins, gps, and imaging sensors for precision geolocation | |
| Pei et al. | Methods for monitoring and controlling multi-rotor micro-UAVs position and orientation based on LabVIEW | |
| Yang et al. | Rotation estimation for mobile robot based on single-axis gyroscope and monocular camera | |
| CN101567970A (en) | Bionic eye anisotropic movement controlling method based on feedback neural integrator | |
| Dadkhah et al. | A model-aided ahrs for micro aerial vehicle application |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| C14 | Grant of patent or utility model | ||
| GR01 | Patent grant | ||
| ASS | Succession or assignment of patent right |
Owner name: QINGDAO HAILAN KANGFU EQUIPMENT CO., LTD. Free format text: FORMER OWNER: VMSENS INERTIAL TECHNOLOGY (BEIJING) CO.,LTD. Effective date: 20150413 |
|
| C41 | Transfer of patent application or patent right or utility model | ||
| COR | Change of bibliographic data |
Free format text: CORRECT: ADDRESS; FROM: 100004 CHAOYANG, BEIJING TO: 266000 QINGDAO, SHANDONG PROVINCE |
|
| TR01 | Transfer of patent right |
Effective date of registration: 20150413 Address after: Laoshan District Shazikou Street Community in Shandong city of Qingdao province river 266000 Patentee after: QINGDAO HAILANKF EQUIPMENT CO., LTD. Address before: 100004 Beijing Chaoyang District City No. 15 Jianguo Road, No. 1 North 1292 Sanjianfang creative living Park 2-414 Patentee before: Micro inertial Meissen technology development (Beijing) Co., Ltd. |