Pose of mobile robot angle calculation methodTechnical field
The present invention relates to the mobile robots including unmanned plane, and in particular to merges spreading kalman based on quaternary numberThe calculation method at the pose of mobile robot angle of filter and dominant complementary filter.
Background technique
Currently, robot pose estimation is mainly calculated using attitude matrix, attitude matrix can use different numbersMode defines.And attitude matrix update method mainly has direction cosine method, Euler's horn cupping and Quaternion Method.Mobile robotThe case where will appear pitch angle in crossing over blockage close to 90 °, carries out carrier of moving robot posture renewal using Quaternion Method,Can be to avoid " singular point " problem that mobile robot occurs during crossing over blockage, and Relative Euler horn cupping calculation amount is moreSmall, algorithm is simpler.
Existing pose of mobile robot, which calculates filter, dominant complementary filter and extended Kalman filter.It is dominantComplementary filter calculation method is simple, and operand is small, and the mobile robot high suitable for requirement of real-time carries out attitude algorithm.ByIt is fixed in dominant complementary filter parameter, therefore is not particularly suited for the solution of the posture in the case of motion state changes greatly.It is transportingDuring solving mobile robot quaternary number posture with extended Kalman filter, made using accelerometer and magnetometer dataIt is measured for systematic perspective, and accelerometer output is the specific force of carrier of moving robot, i.e., carrier of moving robot relative inertness is emptyBetween absolute acceleration and the sum of gravitational acceleration, this will cause carrier of moving robot and passes through during High acceleration motionThere is very big error in the posture that accelerometer resolves.
Summary of the invention
To overcome the above-mentioned deficiency in the presence of the prior art, the present invention provides one kind to merge expansion card based on quaternary numberThe pose of mobile robot angle calculation method of Thalmann filter and dominant complementary filter, to solve existing mobile robot appearanceThe not high problem of state angle calculation accuracy, this method can obtain the attitude angle of higher precision, so as to preferably control movementThe posture of robot.
Technical solution of the present invention:
Pose of mobile robot angle calculation method, the mobile robot are driven by direct current generator；The mobile robotIt is equipped with main control chip and Inertial Measurement Unit, the Inertial Measurement Unit has nine axle sensors；
The solving of attitude method includes:
Step 1, sensor raw data acquisition: main control chip is passed by nine axis that I2C bus acquires Inertial Measurement UnitThe data of sensor information, i.e. three-axis gyroscope, three axis accelerometer and three axle magnetometer；
Step 2, sensor raw data pretreatment: using the high-frequency noise adulterated in mean filter removal data information；
Step 3 establishes the mistake based on quaternary number fusion extended Kalman filter and dominant complementary filter blending algorithmJourney model: it takes quaternary number and 7 parameters of gyroscope drift error as system state variables, establishes non-linear system status sideJourney；
Step 4 establishes the sight based on quaternary number fusion extended Kalman filter and dominant complementary filter blending algorithmSurvey model: the quaternary number resolved using dominant complementary filter recycles fusion as the observed quantity of extended Kalman filterFilter estimates optimal quaternary number；
Step 5, pose of mobile robot angle solve: optimal quaternary number obtained in step 4 is substituted into quaternary number and EulerThe conversion relational expression at angle calculates three attitude angles of mobile robot: roll angle, pitch angle and yaw angle.
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, the step 2 specifically includes following sub-stepIt is rapid:
2.1, windowing process is carried out to gyroscope, accelerometer and magnetometer initial data, takes sliding window length n=10, step-length l=1；
2.2, average value processing is carried out to the data in window, exports numerical value of the result as current time, the following institute of formulaShow:
2.3, I (n-9) exit window after sub-step 2.2, I (n+1) is into window；
2.4, sub-step 2.2 and sub-step 2.3 recycle, and to the last a data are into window, and export final result.
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, in the step 3:
System mode vector are as follows:
X (k)=(q0(k) q1(k) q2(k) q3(k) bgx(k) bgy(k) bgz(k))；
In above formula, q0(k)、q1(k)、q2(k)、q3It (k) is k moment quaternary number posture, bgx(k)、bgy(k)、bgz(k) respectivelyFor the drift error of three axis of gyroscope；
System state equation is established by system mode vector are as follows:
Ask Jacobi matrix that can obtain state-transition matrix f (x (k-1), k-1) are as follows:
Wherein: Г (k) is that noise drives matrix, and W (k-1) is zero-mean white noise sequence；It respectively indicatesGyroscope three-axis measurement value；It indicates gyroscope estimated value, is subtracted each other by gyroscope measured value and drift error amountIt obtains；I3×3For three-dimensional unit matrix；
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, the step 4 is real according to following sub-stepIt is existing:
4.1, data normalization: when mobile robot is in non-accelerating state, accelerometer is under world coordinate systemGravity vector isGravity vector is after normalizationUnder carrier of moving robot coordinate systemThe weight component that accelerometer measures is denoted as
4.2, the gravity vector under the carrier coordinate system that the sum that basis measures is calculated calculates error compensation value accelerationSpending the gravity vector after meter normalizes under reference frame isIt can be by g by attitude matrixnIt is converted toGravity vector under carrier coordinate system:
Vector multiplication cross is done in gravitational vectors estimation to the gravitational vectors under carrier coordinate system and under carrier coordinate system, is obtained pairThe rectification building-out value of gyroscope angular velocity data:
4.3, it according to the magnetic field strength under magnetometer data and the carrier of moving robot coordinate system being calculated, calculatesThe size and Orientation in error compensation value magnetometer measures earth magnetic field, magnetometer output is m in carrier coordinate systemb=(mxi myimzi)T, it is converted under world coordinate system by attitude matrix and is projected as hn=(hxi hyi hzi)T, when magnetic geographic coordinate system andWhen carrier of moving robot coordinate system is overlapped, magnetometer output is bb=(0 byi bzi)T, in XOY plane, bbBe projected asbyi, hnBe projected as (hxi+hyi)；The estimated value for obtaining magnetic field strength is:
Field value under navigational coordinate system is obtained by the estimated value of the magnetic field strength of above formula, then recycles attitude matrixIt converts and extrapolates field projection under carrier of moving robot coordinate system as wb=(wxi wyi wzi)T；According to measure and reckoningThe two is done vector multiplication cross, obtains correction-compensation value by the magnetic field strength under obtained carrier of moving robot coordinate system:
4.4, error compensation: error compensation value
4.5, gyroscope angular speed error is corrected.Gyroscope angular speed numerical value is corrected using error compensation value:
4.6, quaternary number is updated using modified gyroscope angular speed numerical value, differential side is solved using single order runge kutta methodJourney, and update quaternary number；The quaternion differential equation of t moment are as follows:
Obtain (t+T) moment updated quaternary number:
Systematic perspective measurement is set as:
Z (k)=(q0(k) q1(k) q2(k) q3(k))；
Wherein: q0(k)、q1(k)、q2(k)、q3(k) mobile robot obtained after resolving for dominant complementary filter algorithm carriesBody quaternary number posture；
Systematic observation equation are as follows:
Ask Jacobian matrix that can obtain system measurements matrix h (x (k), k) according to above formula are as follows:
The quaternary number posture at (k-1) moment is substituted into system state equation to update to obtain k moment carrier of moving robot fourFirst number posture state amount；Then it is by the quaternary number posture state amount at k moment and by what dominant complementary filter solvedOverall view measurement substitutes into state vector and estimates equation, finally obtains the optimal quaternary number Attitude estimation of k moment carrier of moving robot:
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, in the step 5, attitude angle calculating formula are as follows:
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, the main control chip is STM32F103.
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, the inertia unit is MPU9250.
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, the message transmission rate of the I2C bus is400kbit/s, the data sampling frequency of nine axle sensor are 500HZ.
As optimization, in the calculation method of pose of mobile robot angle above-mentioned, the every 2ms of main control unit is carried out periodicallyData acquisition and pretreatment 1 time.
Compared with prior art, pose of mobile robot angle calculation method of the invention achieves following significant progress:By the posture for merging the algorithm resolving mobile robot of extended Kalman filter and dominant complementary filter based on quaternary numberAngle solves the problems, such as that existing pose of mobile robot angle calculation accuracy is not high, can obtain the attitude angle of higher precision, fromAnd it can preferably control the posture of mobile robot.
The results showed that the attitude angle fluctuation that fused filter obtains is smaller, flatness is more preferable, and to yaw angleControl errors it is stronger, attitude angle tracking effect is more preferable；When mobile robot quickly moves, speed is responded using fused filtering deviceDu Genggao, and attitude angle fluctuation range is smaller.
It is found, is obtained by fusion extended Kalman filter and dominant complementary filter quiet by contrast and experimentFor state accuracy of attitude determination compared to extended Kalman filter, 71%, 74% and is can be improved in roll angle, pitch angle and yaw angle respectively66%；Dynamic accuracy of attitude determination can be improved respectively compared to extended Kalman filter, roll angle, pitch angle and yaw angle65%, 67% and 76%, significantly more advantage is shown in attitude algorithm.
Detailed description of the invention
Fig. 1 is the pose of mobile robot angle that extended Kalman filter and dominant complementary filter are merged based on quaternary numberResolve functional block diagram；
Extended Kalman filter and fusion Extended Kalman filter is respectively adopted in Fig. 2 when being the static placement of mobile robotThe roll angle that device and dominant complementary filter resolve；
Extended Kalman filter and fusion Extended Kalman filter is respectively adopted in Fig. 3 when being the static placement of mobile robotThe pitch angle that device and dominant complementary filter resolve；
Extended Kalman filter and fusion Extended Kalman filter is respectively adopted in Fig. 4 when being the static placement of mobile robotThe yaw angle that device and dominant complementary filter resolve；
Fig. 5 is that extended Kalman filter and fusion extension karr is respectively adopted in mobile robot under microinching stateThe roll angle that graceful filter and dominant complementary filter resolve；
Fig. 6 is that extended Kalman filter and fusion extension karr is respectively adopted in mobile robot under microinching stateThe pitch angle that graceful filter and dominant complementary filter resolve；
Fig. 7 is that extended Kalman filter and fusion extension karr is respectively adopted in mobile robot under microinching stateThe yaw angle that graceful filter and dominant complementary filter resolve；
Fig. 8 is that extended Kalman filter and fusion extension karr is respectively adopted in mobile robot under quick motion stateThe roll angle that graceful filter and dominant complementary filter resolve；
Fig. 9 is that extended Kalman filter and fusion extension karr is respectively adopted in mobile robot under quick motion stateThe pitch angle that graceful filter and dominant complementary filter resolve；
Figure 10 is that extended Kalman filter and fusion expansion card is respectively adopted in mobile robot under quick motion stateThe yaw angle that Thalmann filter and dominant complementary filter resolve；
Specific embodiment
With reference to the accompanying drawings and detailed description (embodiment) the present invention is further illustrated, it is described hereinSpecific embodiment is only used to explain the present invention, but is not intended as the foundation limited the present invention.
Referring to Fig. 1-10, in pose of mobile robot angle calculation method of the invention, the mobile robot is by direct currentMachine driving；The mobile robot is equipped with main control chip and Inertial Measurement Unit, and the Inertial Measurement Unit is passed with nine axisSensor.
Pose of mobile robot angle calculation method includes:
Step 1, sensor raw data acquisition: main control chip is passed by nine axis that I2C bus acquires Inertial Measurement UnitThe data of sensor information, i.e. three-axis gyroscope, three axis accelerometer and three axle magnetometer；
Step 2, sensor raw data pretreatment: using the high-frequency noise adulterated in mean filter removal data information；
Step 3 establishes the mistake based on quaternary number fusion extended Kalman filter and dominant complementary filter blending algorithmJourney model: it takes quaternary number and 7 parameters of gyroscope drift error as system state variables, establishes non-linear system status sideJourney；
Step 4 establishes the sight based on quaternary number fusion extended Kalman filter and dominant complementary filter blending algorithmSurvey model: the quaternary number resolved using dominant complementary filter recycles fusion as the observed quantity of extended Kalman filterFilter estimates optimal quaternary number；
Step 5, pose of mobile robot angle solve: optimal quaternary number obtained in step 4 is substituted into quaternary number and EulerThe conversion relational expression at angle calculates three attitude angles of mobile robot: roll angle, pitch angle and yaw angle.
It is a specific embodiment of the invention below:
The step 2 specifically includes following sub-step:
2.1, windowing process is carried out to gyroscope, accelerometer and magnetometer initial data, takes sliding window length n=10, step-length l=1；
2.2, average value processing is carried out to the data in window, exports numerical value of the result as current time, the following institute of formulaShow:
2.3, I (n-9) exit window after sub-step 2.2, I (n+1) is into window；
2.4, sub-step 2.2 and sub-step 2.3 recycle, and to the last a data are into window, and export final result.
In the step 3:
System mode vector are as follows:
X (k)=(q0(k) q1(k) q2(k) q3(k) bgx(k) bgy(k) bgz(k))；
In above formula, q0(k)、q1(k)、q2(k)、q3It (k) is k moment quaternary number posture, bgx(k)、bgy(k)、bgz(k) respectivelyFor the drift error of three axis of gyroscope；
System state equation is established by system mode vector are as follows:
Ask Jacobi matrix that can obtain state-transition matrix f (x (k-1), k-1) are as follows:
Wherein: Г (k) is that noise drives matrix, and W (k-1) is zero-mean white noise sequence；It respectively indicatesGyroscope three-axis measurement value；It indicates gyroscope estimated value, is subtracted each other by gyroscope measured value and drift error amountIt obtains；I3×3For three-dimensional unit matrix；
The step 4 is realized according to following sub-step:
4.1, data normalization: when mobile robot is in non-accelerating state, accelerometer is under world coordinate systemGravity vector isGravity vector is after normalizationUnder carrier of moving robot coordinate systemThe weight component that accelerometer measures is denoted as
4.2, the gravity vector under the carrier coordinate system that the sum that basis measures is calculated calculates error compensation value accelerationSpending the gravity vector after meter normalizes under reference frame isIt can be by g by attitude matrixnIt is converted toGravity vector under carrier coordinate system:
Vector multiplication cross is done in gravitational vectors estimation to the gravitational vectors under carrier coordinate system and under carrier coordinate system, is obtained pairThe rectification building-out value of gyroscope angular velocity data:
4.3, it according to the magnetic field strength under magnetometer data and the carrier of moving robot coordinate system being calculated, calculatesThe size and Orientation in error compensation value magnetometer measures earth magnetic field, magnetometer output is m in carrier coordinate systemb=(mxi myimzi)T, it is converted under world coordinate system by attitude matrix and is projected as hn=(hxi hyi hzi)T, when magnetic geographic coordinate system andWhen carrier of moving robot coordinate system is overlapped, magnetometer output is bb=(0 byi bzi)T, in XOY plane, bbBe projected asbyi, hnBe projected as (hxi+hyi)；The estimated value for obtaining magnetic field strength is:
Field value under navigational coordinate system is obtained by the estimated value of the magnetic field strength of above formula, then recycles attitude matrixIt converts and extrapolates field projection under carrier of moving robot coordinate system as wb=(wxi wyi wzi)T；According to measure and reckoningThe two is done vector multiplication cross, obtains correction-compensation value by the magnetic field strength under obtained carrier of moving robot coordinate system:
4.4, error compensation: error compensation value
4.5, gyroscope angular speed error is corrected.Gyroscope angular speed numerical value is corrected using error compensation value:
4.6, quaternary number is updated using modified gyroscope angular speed numerical value, differential side is solved using single order runge kutta methodJourney, and update quaternary number；The quaternion differential equation of t moment are as follows:
Obtain (t+T) moment updated quaternary number:
Systematic perspective measurement is set as:
Z (k)=(q0(k) q1(k) q2(k) q3(k))；
Wherein: q0(k)、q1(k)、q2(k)、q3(k) mobile robot obtained after resolving for dominant complementary filter algorithm carriesBody quaternary number posture；
Systematic observation equation are as follows:
Ask Jacobian matrix that can obtain system measurements matrix h (x (k), k) according to above formula are as follows:
The quantity of state quaternary number at (k-1) moment is substituted into system state equation to update to obtain k moment carrier of moving robotQuaternary number posture；Then by the status predication value at k moment and the State Viewpoint measured value generation solved by dominant complementary filterEnter system measurements matrix, finally obtain the optimal quaternary number Attitude estimation of k moment carrier of moving robot:
In the step 5, attitude angle calculating formula are as follows:
The main control chip is STM32F103.The inertia unit is MPU9250.The data of the I2C bus transmit speedRate is 400kbit/s, and the data sampling frequency of nine axle sensor is 500HZ.The every 2ms of main control unit is carried out periodicallyData acquisition and pretreatment 1 time.
Below by way of experiment, the invention will be further described:
(1) the pose of mobile robot experiment under stationary state: carrier of moving robot attitude transducer module is staticIt places 3 minutes in the horizontal plane, takes 2500 sampled points, be utilized respectively extended Kalman filter and fusion spreading kalman filterThe two methods of wave device and dominant complementary filter calculate the attitude angle under mobile robot stationary state.
By comparison, it was found that the roll and pitch angle that are resolved using fused filtering filter and actual conditions are more coincide,Difference is smaller between yaw angle and true value, but general effect is consistent.
(2) the pose of mobile robot experiment under state at a slow speed: by carrier of moving robot attitude transducer module around threeAxis does the rotation of wide-angle, takes 2500 sampled points, is utilized respectively extended Kalman filter and fusion Extended Kalman filterThe two methods of device and dominant complementary filter calculate attitude angle of the mobile robot at a slow speed under state.
The roll exported by two kinds of filters and pitch angle syncretizing effect are close, and fused filtering device smoothing capability is stronger,And the yaw angle control errors ability resolved is stronger, and attitude angle dynamically track effect is more preferable.
(3) the pose of mobile robot experiment under fast state: enable carrier of moving robot attitude transducer module around threeAxis does quick high frequency time rotation, takes 2500 sampled points, is utilized respectively extended Kalman filter and fusion spreading kalman filterTwo kinds of algorithms of wave device and dominant complementary filter calculate the attitude angle under mobile robot fast state.
From can be seen that in Fig. 8-10, the attitude angle fluctuation that fused filter obtains is smaller, and flatness is more preferable, and rightThe control errors of yaw angle are stronger, and attitude angle tracking effect is more preferable.When mobile robot quickly moves, fused filtering device is utilizedResponse speed is higher, and attitude angle fluctuation range is smaller.
By contrast and experiment, the static state obtained by fusion extended Kalman filter and dominant complementary filter is fixedRoll angle 71%, pitch angle 74% has been respectively increased compared to extended Kalman filter in appearance precision, and yaw angle 66% is dynamically determinedAppearance precision improves roll angle 65%, pitch angle 67%, yaw angle 76%, in attitude algorithm compared to extended Kalman filterIn show significantly more advantage.
The foregoing is merely illustrative of the preferred embodiments of the present invention, is not intended to limit the invention, all in essence of the inventionMade any modifications, equivalent replacements, and improvements etc., should all be included in the protection scope of the present invention within mind and principle.