CN109108936A - A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion - Google Patents
A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion Download PDFInfo
- Publication number
- CN109108936A CN109108936A CN201811244545.1A CN201811244545A CN109108936A CN 109108936 A CN109108936 A CN 109108936A CN 201811244545 A CN201811244545 A CN 201811244545A CN 109108936 A CN109108936 A CN 109108936A
- Authority
- CN
- China
- Prior art keywords
- data
- self
- accelerometer
- angle
- magnetometer
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J5/00—Manipulators mounted on wheels or on carriages
- B25J5/007—Manipulators mounted on wheels or on carriages mounted on wheels
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
- B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a kind of self-balance robot control systems and control method based on Multiple Source Sensor data fusion, comprising the following steps: obtains accelerometer data and magnetometer data, then calculates preliminary posture according to accelerometer data and magnetometer data；Gyro data is obtained again and is converted to rotating vector, is then merged with preliminary posture, accurate robot pose is obtained, and the rotation for the speed control motor fed back finally by accurate robot pose and motor encoder is to control robot pose.The present invention is filtered the self-balancing and movement that control realize robot of the rotation realization to robot pose for carrying out merging again after noise reduction and controlling motor with the information of motor encoder feedback by attitude angle information, it obtains combining self-balance robot physical characteristic, the modified PID control method of motor response characteristic, it can be more acurrate more stable to self-balance robot realization motion control.
Description
Technical field
The invention belongs to mobile robot technology fields, and in particular to a kind of self-balancing machine of Multiple Source Sensor data fusion
Device people control system and control method.
Background technique
Mobile robot has been more and more widely used in various industries at present, is had broad application prospects.From
Balanced robot is a kind of special wheeled mobile robot, and concept is earliest by the mountain of Tokyo Department of Automation of telecommunications university
The male professor of rattan one proposes in the 1980s.Double-wheel self-balancing robot has small in size, turning radius zero, vehicle body
The advantages that flexible, therefore be suitable for using crowded urban environment as a variety of narrow scenes of representative.The technology of balanced robot is closed
Key is to continue to keep balance, and can advance in balance, retreat, turn.Therefore its movement control technology is self-balancing machine
The core of device people's technology, has great significance to the research of self-balance robot movement control technology, also there is great work
Journey application value.
In recent years, with the development of the upsurge of robot and science and technology, the movement control technology of robot there has also been
Very big raising.Movement control technology is mostly feedback control in the prior art, obtains robot by the data of sensor first
Posture information, later according to posture information carry out feedback control.Wherein mainly use the data processing technique of sensor
With PID control technology, sensing data is generally subjected to easy weighted average calculation in the prior art, and will weighted average
For value afterwards as fused final result, this method arithmetic speed is fast, but cannot accurately and maximal efficiency utilize each sensor
Data.It is in the prior art using the side SLAM for merging inertia/magnetometric sensor with monocular vision information there are also a kind of mode
Method handles data, and this method can accurately obtain the posture information of robot, but since number of sensors is more
And volume is larger, data processing and calculating need the biggish equipment of industrial personal computer etc volume, therefore are not suitable for self-balancing machine
People.PID control method is the Typical Representative in classic control algorithm, is proposed by U.S. N.Minorsky in nineteen twenty-two, PID control
Device is made of proportional unit (P), integral unit (I) and differentiation element (D).It is u (t) that it, which inputs e (t) and exports the relationship of u (t),
=kp [e (t)+1/TI ∫ e (t) dt+TD*de (t)/dt].Wherein scale parameter kp can accelerate the response speed of system, but compare
Example control has a disadvantage, can generate remaining difference, therefore introduce integral action, can eliminate accumulated error.The differential action is mainly used to
Overcome the hysteresis quality of object.
Expert PID, fuzzy, Neural network PID, genetic algorithm PID, adaptive has been derived by pid control algorithm
PID control method.Although it is not small that traditional pid control algorithm stablizes remaining, have good dynamic quality remaining little, when
When locating in a dynamic environment, traditional pid algorithm is not applicable.And other several pid algorithms derived are then due to computationally intensive,
Algorithm complexity is high, is not particularly suited for that self-balance robot is small in size in this way, may only carry the platform of small weight hardware.
Summary of the invention
It is an object of the invention to: above-mentioned deficiency in the prior art is solved, a kind of Multiple Source Sensor data fusion is provided
Self-balance robot control system and control method, by magnetometer, gyroscope, accelerometer, encoder data obtained
Fusion obtains combining self-balance robot physical characteristic, the modified PID control method of motor response characteristic, can be more acurrate
More stable realizes motion control to self-balance robot.
To achieve the goals above, the technical solution adopted by the present invention are as follows:
A kind of self-balance robot control system based on Multiple Source Sensor data fusion, including MPU6050 module and magnetic
Power meter, MPU6050 module include accelerometer and gyroscope, MPU6050 module and magnetometer and motor communication connection.
A kind of self-balance robot control method based on Multiple Source Sensor data fusion is passed using above-mentioned based on multi-source
The self-balance robot control system of sensor data fusion, comprising the following steps:
Step 1: accelerometer data and magnetometer data are obtained；
Step 2: preliminary posture is calculated according to accelerometer data and magnetometer data；
Step 3: it obtains gyro data and is converted to rotating vector, then merged with preliminary posture, obtain accurate machine
Device people's posture；
Step 4: the rotation for the speed control motor fed back by accurate robot pose and motor encoder is to control
Robot pose.
Further, above-mentioned step two further include: accelerometer data is subjected to sliding weight filtering processing, is calculated public
Formula are as follows:
Wherein, Acc (i) is accelerometer initial data, unit m/s2, ACC (i) is the counting of fused acceleration
According to unit m/s2, i is chronomere, is determined by the rate of processor acquisition sensing data.
Further, above-mentioned step two specifically:
Step 201: finding out strap-down matrix by the data of accelerometer, accelerometer data is converted according to strap-down matrix
Coordinate, calculation formula are as follows:
an=(0,0, g), ab=(ax,ay,az):
Wherein,The matrix of world coordinate system, a are converted to for body coordinatenIt is acceleration of gravity under world coordinate system
Expression way, abFor expression way of the acceleration of gravity under body coordinate system, ax,ay,azIt is sat for acceleration of gravity in body
The component of lower three axis of mark system；
Step 202: according to the accelerometer data coordinate and rough Attitude Calculation pitch angle and roll angle after conversion, calculating public
Formula are as follows:
Wherein, θ is pitch angle, and γ is roll angle, and ψ is boat parallactic angle；θ1, γ1For the pitch angle that is calculated by accelerometer and
Roll angle；
Step 203: fusion magnetometer data calculates boat parallactic angle, calculation formula are as follows:
Wherein, the data that magnetometer obtains are mb=(mx,my,mz), the magnetic field strength of magnetometer is m under absolute coordinate systemn
=[0, mN,mD], ψ1For the boat parallactic angle calculated after fusion magnetometer.
Further, above-mentioned step three specifically:
Step 301: original gyro data is converted to by rotating vector, calculation formula according to quaternary number formula are as follows:
Q=q0+q1i+q2j+q3k
Q=cos (α/2)+sin (α/2) i+sin (α/2) j+sin (α/2) k
Wherein, α=(α1,α2,α3) it is the rotation angle that reference frame is overlapped after Vector Rotation with body coordinate system, Q
For quaternary number, q0,q1,q2,q3For four components of quaternary number, i, j, k is the component of the vector form of Q；
Step 302: attitude angle, calculation formula are calculated by strap-down matrix according to rotating vector are as follows:
Wherein, θ2, γ2, ψ2For calculated pitch angle after fusion gyro data, roll angle, parallactic angle of navigating.
Further, the calculation formula merged in above-mentioned step three with preliminary posture are as follows:
Z (n)=a*X (n)+(1-a) * Y (n)
Wherein, X is accelerometer input, and Y is gyroscope input, and Z is output, and a is filter factor, and a is one much smaller than 1
Number, i.e. low-pass filter, (1-a) are high-pass filter.
Further, above-mentioned step four specifically:
Accurate robot pose is resolved and is uprightly controlled, while is logical according to the self-balance robot speed of feedback
It crosses speed closed loop controller and carries out speed control, calculation formula are as follows:
A=kp*tan(θ)+kd*[tan(θ)]'-[kp1*e(k)+ki1*∑e(k)]
Wherein, kpFor angle controller proportionality coefficient, kdFor angle controller differential coefficient, kp1For speed control ratio system
Number, ki1For speed control integral coefficient
Further, above-mentioned upright control uses PD control, and speed control is PI control.
By adopting the above-described technical solution, the beneficial effects of the present invention are:
Self-balance robot control method based on Multiple Source Sensor data fusion of the invention passes through accelerometer, gyro
Instrument and external magnetometer obtain attitude angle information, and then attitude angle information is filtered after noise reduction and is merged again,
The posture information of self-balance robot is obtained, and the information fed back by motor encoder controls the rotation realization of motor to machine
The self-balancing and movement for controlling to realize robot of people's posture, obtain combining self-balance robot physical characteristic, motor
The modified PID control method of response characteristic, can be more acurrate more stable to self-balance robot realization motion control.
Detailed description of the invention
In order to illustrate the technical solution of the embodiments of the present invention more clearly, below will be to needed in the embodiment attached
Figure is briefly described, it should be understood that the following drawings illustrates only certain embodiments of the present invention, therefore is not construed as pair
The restriction of range for those of ordinary skill in the art without creative efforts, can also be according to this
A little attached drawings obtain other relevant attached drawings.
Fig. 1 is self-balance robot structural schematic diagram of the invention.
Fig. 2 is data fusion flow diagram of the invention.
Fig. 3 is data fusion details flow diagram of the invention.
Fig. 4 is self-balance robot stress diagram of the invention.
Fig. 5 is motor response curve schematic diagram of the invention.
Fig. 6 is self-balance robot control flow schematic diagram of the invention.
Specific embodiment
In order to make the object, technical scheme and advantages of the embodiment of the invention clearer, below in conjunction with the embodiment of the present invention
In attached drawing, technical scheme in the embodiment of the invention is clearly and completely described, it is clear that described embodiment is
A part of the embodiment of the present invention, instead of all the embodiments.
Therefore, the detailed description of the embodiment of the present invention provided in the accompanying drawings is not intended to limit below claimed
The scope of the present invention, but be merely representative of selected embodiment of the invention.Based on the embodiments of the present invention, this field is common
Technical staff's every other embodiment obtained without creative efforts belongs to the model that the present invention protects
It encloses.
Referring to attached drawing 1-6, specific description is done to embodiments of the present invention.
A kind of self-balance robot control system based on Multiple Source Sensor data fusion, including MPU6050 module and magnetic
Power meter, MPU6050 module include accelerometer and gyroscope, MPU6050 module and magnetometer and motor communication connection.
A kind of self-balance robot control method based on Multiple Source Sensor data fusion is passed using above-mentioned based on multi-source
The self-balance robot control system of sensor data fusion, comprising the following steps:
Step 1: accelerometer data and magnetometer data are obtained；
Step 2: preliminary posture is calculated according to accelerometer data and magnetometer data；
Step 3: it obtains gyro data and is converted to rotating vector, then merged with preliminary posture, obtain accurate machine
Device people's posture；
Step 4: the rotation for the speed control motor fed back by accurate robot pose and motor encoder is to control
Robot pose.
Further, above-mentioned step two further include: accelerometer data is subjected to sliding weight filtering processing, is calculated public
Formula are as follows:
Wherein, Acc (i) is accelerometer initial data, unit m/s2, ACC (i) is the counting of fused acceleration
According to unit m/s2, i is chronomere, is determined by the rate of processor acquisition sensing data.
Further, above-mentioned step two specifically:
Step 201: finding out strap-down matrix by the data of accelerometer, accelerometer data is converted according to strap-down matrix
Coordinate, calculation formula are as follows:
an=(0,0, g), ab=(ax,ay,az):
Wherein,The matrix of world coordinate system, a are converted to for body coordinatenIt is acceleration of gravity under world coordinate system
Expression way, abFor expression way of the acceleration of gravity under body coordinate system, ax,ay,azIt is sat for acceleration of gravity in body
The component of lower three axis of mark system；
Step 202: according to the accelerometer data coordinate and rough Attitude Calculation pitch angle and roll angle after conversion, calculating public
Formula are as follows:
Wherein, θ is pitch angle, and γ is roll angle, and ψ is boat parallactic angle；θ1, γ1For the pitch angle that is calculated by accelerometer and
Roll angle；
Step 203: fusion magnetometer data calculates boat parallactic angle, calculation formula are as follows:
Wherein, the data that magnetometer obtains are mb=(mx,my,mz), the magnetic field strength of magnetometer is m under absolute coordinate systemn
=[0, mN,mD], ψ1For the boat parallactic angle calculated after fusion magnetometer.
Further, above-mentioned step three specifically:
Step 301: original gyro data is converted to by rotating vector, calculation formula according to quaternary number formula are as follows:
Q=q0+q1i+q2j+q3k
Q=cos (α/2)+sin (α/2) i+sin (α/2) j+sin (α/2) k
Wherein, α=(α1,α2,α3) it is the rotation angle that reference frame is overlapped after Vector Rotation with body coordinate system, Q
For quaternary number, q0,q1,q2,q3For four components of quaternary number, i, j, k is the component of the vector form of Q；
Step 302: attitude angle, calculation formula are calculated by strap-down matrix according to rotating vector are as follows:
Wherein, θ2, γ2, ψ2For calculated pitch angle after fusion gyro data, roll angle, parallactic angle of navigating.
Further, the calculation formula merged in above-mentioned step three with preliminary posture are as follows:
Z (n)=a*X (n)+(1-a) * Y (n)
Wherein, X is accelerometer input, and Y is gyroscope input, and Z is output, and a is filter factor, and a is one much smaller than 1
Number, i.e. low-pass filter, (1-a) are high-pass filter.
Further, above-mentioned step four specifically:
Accurate robot pose is resolved and is uprightly controlled, while is logical according to the self-balance robot speed of feedback
It crosses speed closed loop controller and carries out speed control, calculation formula are as follows:
A=kp*tan(θ)+kd*[tan(θ)]'-[kp1*e(k)+ki1*∑e(k)]
Wherein, kpFor angle controller proportionality coefficient, kdFor angle controller differential coefficient, kp1For speed control ratio system
Number, ki1For speed control integral coefficient
Further, above-mentioned upright control uses PD control, and speed control is PI control.
It is of the invention in one embodiment, as shown in Figure 1, showing machine in self-balance robot structural model figure
Body coordinate system and robot pose angle.Wherein, the inclination angle that robot generates when turning about the X axis is known as pitch angle (Pitch), around Y
The inclination angle that robot generates when axis rotates is known as roll angle (Roll), and the inclination angle that robot generates when turning about the Z axis is known as course
Angle (Yaw).
Self-balance robot includes MPU6050 attitude transducer 1, battery 2, motor 3, wheel 4, chassis 5 and master control electricity
Road plate 6, realized by a pair of of drive module advance, retreat, the movement such as turning, by accelerometer built in MPU6050 module,
Gyroscope and external magnetometer obtain attitude angle information, for accelerometer, gyroscope and external in the present embodiment
The attitude angle information that magnetometer obtains is merged again after being filtered noise reduction, obtains the posture information of self-balance robot,
And the rotation of the information control motor fed back by motor encoder realizes the control to robot pose to realize robot
Self-balancing and movement.
As shown in Fig. 2, self-balance robot has used three kinds of sensors to calculate postures, be respectively as follows: gyroscope, magnetometer,
Accelerometer.Gyroscope is that a kind of dynamic characteristic is preferable, responds faster sensor within a short period of time.What gyroscope was returned
Data are angular speed.But gyroscope is primarily present two kinds of noises, and one is angular speed random walk, another kind is drift.Top
Spiral shell instrument drift is a kind of zero bias unstability deviated except 0 axis to pass at any time.Magnetometer is a kind of long-term static
Can stable sensor, but dynamic property is poor, it calculates angle in the component of each axis of body coordinate system by magnetic field strength
Degree.Accelerometer has preferable long-term static performance, is a kind of inertial sensor steady in a long-term.What accelerometer utilized is
Gravity measures, therefore it can not obtain the angle of world coordinate system Z axis and navigate parallactic angle.And what magnetometer utilized is on the earth
Magnetic field strength, north by southern Hang Seng Index, the strong point of magnetometer is the direction of robot measurement to tellurian magnetic field strength.Cause
This three kinds of sensors respectively have advantage and disadvantage, by data fusion by their mutual supplement with each other's advantages, and then can more precisely obtain machine
Device people's posture.
Accelerometer data is first subjected to sliding weight filtering and obtains relatively stable 3-axis acceleration, in conjunction with magnetometer
Data tentatively calculate the posture of robot.Since accelerometer and magnetometer dynamic characteristic are bad, the robot appearance calculated at this time
State is inaccurate, after the angular velocity data integral that we recycle gyroscope to return using Kalman filtering or complementary filter into
Row data fusion (if processor performance preferably if use Kalman filtering, otherwise use complementary filter).
Data fusion in the present embodiment the following steps are included:
S1: preliminary posture is calculated according to accelerometer data and magnetometer data；
Firstly, accelerometer data is carried out sliding weight filtering processing, calculation formula are as follows:
Wherein, Acc (i) is accelerometer initial data, unit m/s2, ACC (i) is the counting of fused acceleration
According to unit m/s2, i is chronomere, is determined by the rate of processor acquisition sensing data.
Using strap-down matrix, we can convert posture for the data of accelerometer, and strap-down matrix is as shown in formula 2:
Wherein,The matrix of world coordinate system, a are converted to for body coordinatenIt is acceleration of gravity under world coordinate system
Expression way, abFor expression way of the acceleration of gravity under body coordinate system, ax,ay,azIt is sat for acceleration of gravity in body
The component of lower three axis of mark system.
The coordinate conversion of accelerometer is as shown in formula 3, wherein an=(0,0, g), ab=(ax,ay,az):
According to this formula 2 and formula 3, we can tentatively find out pitch angle and roll angle, such as formula 4,5 institute of formula
Show.
Wherein, θ is pitch angle, and γ is roll angle, and ψ is boat parallactic angle；θ1, γ1For the pitch angle that is calculated by accelerometer and
Roll angle；
Boat parallactic angle is limited to the property of accelerometer, boat parallactic angle can not be found out by accelerometer, it is therefore desirable to merge magnetic
Power meter resolves parallactic angle of sailing.The data that magnetometer obtains are mb=(mx,my,mz), the magnetic field strength of magnetometer under absolute coordinate system
For mn=[0, mN,mD], according to same principle, it can tentatively find out boat parallactic angle.It is as shown in formula 6:
Wherein, the data that magnetometer obtains are mb=(mx,my,mz), the magnetic field strength of magnetometer is m under absolute coordinate systemn
=[0, mN,mD], ψ1For the boat parallactic angle calculated after fusion magnetometer.
Above step has tentatively obtained the appearance of self-balance robot using accelerometer and magnetometer in the present embodiment
State.But since accelerometer and magnetometer dynamic characteristic are bad, the present embodiment compensates above data by gyroscope, due to
Gyro data updates comparatively fast, is calculated faster with the mode of quaternary number.
S2: it obtains gyro data and is converted to rotating vector, then merged with preliminary posture, obtain accurate robot
Posture；
Specifically: original gyro data is converted to by rotating vector, the rotational motion of a rigid body with a fixed point reason according to quaternary number formula
In, reference frame is around one angle [alpha]=(α of Vector Rotation1,α2,α3), so that it may it is overlapped with body coordinate system, then quaternary number
Q may be expressed as:
Q=q0+q1i+q2j+q3k
Q=cos (α/2)+sin (α/2) i+sin (α/2) j+sin (α/2) k
Wherein, α=(α1,α2,α3) it is the rotation angle that reference frame is overlapped after Vector Rotation with body coordinate system, Q
For quaternary number, q0,q1,q2,q3For four components of quaternary number, i, j, k is the component of the vector form of Q；
After quaternary number expression formula is found out, attitude angle is found out using the strap-down matrix that quaternary number indicates.Attitude angle at this time
As utilize attitude angle calculated by gyroscope.
Wherein, θ2, γ2, ψ2For calculated pitch angle after fusion gyro data, roll angle, parallactic angle of navigating.
At this point, obtaining one group of attitude angle data using accelerometer and magnetometer, obtained using gyroscope
One group of attitude angle data, and the dynamic characteristic of gyroscope is preferable, long-term effect is bad.Accelerometer and magnetometer long-term effect
It is good, poor dynamic.Therefore we are by the mutual supplement with each other's advantages of the two.The common mode of processing signal has high-pass filter and low pass filtered
Wave device, high-pass filter can filter out change in long term, retain of short duration variation, and low-pass filter can filter out short term variations,
Retain change in long term.We list the formula of basic filtering algorithm:
It is translated into the rudimentary algorithm of single order RC digital filtering, X is accelerometer input, and Y is gyroscope input, and Z is
Output.As shown in formula 15, wherein a is filter factor, is usually much smaller than 1 number, is equivalent to low-pass filter, (1-
A) it is equivalent to high-pass filter.
Z (n)=a*X (n)+(1-a) * Y (n)
Wherein, X is accelerometer input, and Y is gyroscope input, and Z is output, and a is filter factor, and a is one much smaller than 1
Number, i.e. low-pass filter, (1-a) are high-pass filter.
After obtaining accurate robot pose, due to the pid parameter of traditional pid algorithm be it is fixed, can not be with
The variation of robot running environment and change, therefore adaptability is bad.The present embodiment additionally provides a kind of based on self-balancing machine
The Adaptive PID Control algorithm of device people's attitudes vibration, the specific steps are as follows:
As Figure 4-Figure 6, the key problem of self-balance robot is its balance, is changed herein for this point to algorithm
Into the component of motor driving force should being made to overcome the component of gravity oliquely downward by upper figure it is found that in order to make robot self-balancing.
Thus can derive should to motor control power:
F=mgtan (θ)
The driving force of motor is indicated by acceleration, as shown in Fig. 2 motor response curve, when the self-balancing stage, motor be plus
In the fast stage, as shown in Figure 5, acceleration is substantially proportional to motor driven voltage.Therefore it can be concluded that
U=Kmgtan (θ)
In addition to this i.e. upright control of self-balancing control also needs to carry out speed control to self-balance robot, due to upright
Control needs response speed very fast, therefore uses PD control, and speed control is even more important due to eliminating steady-state error, uses
PI control.
As shown in fig. 6, the corresponding formula of the control flow are as follows:
A=kp*tan (θ)+kd* [tan (θ)] '-[kp1*e(k)+ki1*∑e(k)]。
Claims (8)
1. a kind of self-balance robot control system based on Multiple Source Sensor data fusion, it is characterised in that: including MPU6050
Module and magnetometer, the MPU6050 module include accelerometer and gyroscope, the MPU6050 module and magnetometer and electricity
Machine communication connection.
2. a kind of self-balance robot control method based on Multiple Source Sensor data fusion, using base described in claim 1
In the self-balance robot control system of Multiple Source Sensor data fusion, it is characterised in that the following steps are included:
Step 1: accelerometer data and magnetometer data are obtained；
Step 2: preliminary posture is calculated according to accelerometer data and magnetometer data；
Step 3: it obtains gyro data and is converted to rotating vector, then merged with preliminary posture, obtain accurate robot
Posture；
Step 4: the rotation for the speed control motor fed back by accurate robot pose and motor encoder is to control machine
People's posture.
3. the self-balance robot control method according to claim 2 based on Multiple Source Sensor data fusion, feature
It is: the step two further include: accelerometer data is subjected to sliding weight filtering processing, calculation formula are as follows:
Wherein, Acc (i) is accelerometer initial data, unit m/s2, ACC (i) is fused accelerometer data, unit
For m/s2, i is chronomere, is determined by the rate of processor acquisition sensing data.
4. the self-balance robot control method according to claim 2 or 3 based on Multiple Source Sensor data fusion, special
Sign is: the step two specifically:
Step 201: finding out strap-down matrix by the data of accelerometer, the seat of accelerometer data is converted according to strap-down matrix
Mark, calculation formula are as follows:
an=(0,0, g), ab=(ax,ay,az):
Wherein,The matrix of world coordinate system, a are converted to for body coordinatenFor table of the acceleration of gravity under world coordinate system
Up to mode, abFor expression way of the acceleration of gravity under body coordinate system, ax,ay,azIt is acceleration of gravity in body coordinate system
The component of lower three axis；
Step 202: according to the accelerometer data coordinate and rough Attitude Calculation pitch angle and roll angle after conversion, calculation formula
Are as follows:
Wherein, θ is pitch angle, and γ is roll angle, and ψ is boat parallactic angle；θ1, γ1For the pitch angle calculated by accelerometer and roll
Angle；
Step 203: fusion magnetometer data calculates boat parallactic angle, calculation formula are as follows:
Wherein, the data that magnetometer obtains are mb=(mx,my,mz), the magnetic field strength of magnetometer is m under absolute coordinate systemn=[0,
mN,mD], ψ1For the boat parallactic angle calculated after fusion magnetometer.
5. the self-balance robot control method according to claim 4 based on Multiple Source Sensor data fusion, feature
It is: the step three specifically:
Step 301: original gyro data is converted to by rotating vector, calculation formula according to quaternary number formula are as follows:
Q=q0+q1i+q2j+q3k
Q=cos (α/2)+sin (α/2) i+sin (α/2) j+sin (α/2) k
Wherein, α=(α1,α2,α3) it is the rotation angle that reference frame is overlapped after Vector Rotation with body coordinate system, Q tetra-
First number, q0,q1,q2,q3For four components of quaternary number, i, j, k is the component of the vector form of Q；
Step 302: attitude angle, calculation formula are calculated by strap-down matrix according to rotating vector are as follows:
Wherein, θ2, γ2, ψ2For calculated pitch angle after fusion gyro data, roll angle, parallactic angle of navigating.
6. the self-balance robot control method according to claim 5 based on Multiple Source Sensor data fusion, feature
It is: the calculation formula merged in the step three with preliminary posture are as follows:
Z (n)=a*X (n)+(1-a) * Y (n)
Wherein, X is accelerometer input, and Y is gyroscope input, and Z is output, and a is filter factor, and a is one much smaller than 1
Number, i.e. low-pass filter, (1-a) are high-pass filter.
7. the self-balance robot control method according to claim 2 based on Multiple Source Sensor data fusion, feature
It is: the step four specifically:
Accurate robot pose is resolved and is uprightly controlled, while speed is passed through according to the self-balance robot speed of feedback
It spends closed loop controller and carries out speed control, calculation formula are as follows:
A=kp*tan(θ)+kd*[tan(θ)]'-[kp1*e(k)+ki1*∑e(k)]
Wherein, kpFor angle controller proportionality coefficient, kdFor angle controller differential coefficient, kp1For speed control proportionality coefficient,
ki1For speed control integral coefficient.
8. the self-balance robot control method according to claim 7 based on Multiple Source Sensor data fusion, feature
Be: using PD control, the speed control is PI control for the upright control.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811244545.1A CN109108936A (en) | 2018-10-24 | 2018-10-24 | A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811244545.1A CN109108936A (en) | 2018-10-24 | 2018-10-24 | A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109108936A true CN109108936A (en) | 2019-01-01 |
Family
ID=64854629
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811244545.1A Pending CN109108936A (en) | 2018-10-24 | 2018-10-24 | A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109108936A (en) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110162068A (en) * | 2019-04-29 | 2019-08-23 | 广东技术师范学院天河学院 | A kind of control method of self-balance robot |
| CN110888449A (en) * | 2019-10-31 | 2020-03-17 | 高新兴物联科技有限公司 | Vehicle attitude control method, apparatus, and storage medium |
| CN112099052A (en) * | 2020-09-16 | 2020-12-18 | 广东工业大学 | Laser radar level adjustment system |
| CN113305830A (en) * | 2021-04-28 | 2021-08-27 | 吉林大学 | Humanoid robot action system based on human body posture control and control method |
| WO2021218212A1 (en) * | 2020-04-26 | 2021-11-04 | 珠海格力智能装备有限公司 | Robot control method and apparatus, and storage medium and processor |
| CN114252073A (en) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | Robot attitude data fusion method |
| CN114440871A (en) * | 2021-12-29 | 2022-05-06 | 宜昌测试技术研究所 | Nine-axis magnetic compass data fusion method based on adaptive complementary filtering |
| CN114459466A (en) * | 2021-12-29 | 2022-05-10 | 宜昌测试技术研究所 | MEMS multi-sensor data fusion processing method based on fuzzy control |
| WO2023220972A1 (en) * | 2022-05-18 | 2023-11-23 | 北京小米移动软件有限公司 | Mobile device, pose estimation method and apparatus therefor, and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009192462A (en) * | 2008-02-18 | 2009-08-27 | Seiko Epson Corp | Positioning method, program, and positioning apparatus |
| CN104914874A (en) * | 2015-06-09 | 2015-09-16 | 长安大学 | Unmanned aerial vehicle attitude control system and method based on self-adaption complementation fusion |
| CN105974935A (en) * | 2016-07-14 | 2016-09-28 | 安徽科技学院 | Four-rotor agricultural remote control aircraft and control method thereof |
| CN106249745A (en) * | 2016-07-07 | 2016-12-21 | 苏州大学 | The control method of four axle unmanned planes |
| CN108121334A (en) * | 2016-11-28 | 2018-06-05 | 沈阳新松机器人自动化股份有限公司 | A kind of self-balance robot motion control method and device |
| CN108170154A (en) * | 2017-12-19 | 2018-06-15 | 广东省航空航天装备技术研究所 | A kind of unmanned plane multisensor forward direction photography, which tilts, flies control adjustment method |
-
2018
- 2018-10-24 CN CN201811244545.1A patent/CN109108936A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2009192462A (en) * | 2008-02-18 | 2009-08-27 | Seiko Epson Corp | Positioning method, program, and positioning apparatus |
| CN104914874A (en) * | 2015-06-09 | 2015-09-16 | 长安大学 | Unmanned aerial vehicle attitude control system and method based on self-adaption complementation fusion |
| CN106249745A (en) * | 2016-07-07 | 2016-12-21 | 苏州大学 | The control method of four axle unmanned planes |
| CN105974935A (en) * | 2016-07-14 | 2016-09-28 | 安徽科技学院 | Four-rotor agricultural remote control aircraft and control method thereof |
| CN108121334A (en) * | 2016-11-28 | 2018-06-05 | 沈阳新松机器人自动化股份有限公司 | A kind of self-balance robot motion control method and device |
| CN108170154A (en) * | 2017-12-19 | 2018-06-15 | 广东省航空航天装备技术研究所 | A kind of unmanned plane multisensor forward direction photography, which tilts, flies control adjustment method |
Non-Patent Citations (2)
| Title |
|---|
| 林伟捷等: "基于四元数互补滤波和PID控制算法的两轮自平衡车系统设计 ", 《软件导刊》 * |
| 黄旭 等: "基于微机械惯性器件和磁强计的姿态航向系统", 《传感器技术》 * |
Cited By (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110162068A (en) * | 2019-04-29 | 2019-08-23 | 广东技术师范学院天河学院 | A kind of control method of self-balance robot |
| CN110888449A (en) * | 2019-10-31 | 2020-03-17 | 高新兴物联科技有限公司 | Vehicle attitude control method, apparatus, and storage medium |
| CN110888449B (en) * | 2019-10-31 | 2023-04-18 | 高新兴物联科技股份有限公司 | Vehicle attitude control method, apparatus, and storage medium |
| WO2021218212A1 (en) * | 2020-04-26 | 2021-11-04 | 珠海格力智能装备有限公司 | Robot control method and apparatus, and storage medium and processor |
| CN112099052A (en) * | 2020-09-16 | 2020-12-18 | 广东工业大学 | Laser radar level adjustment system |
| CN113305830A (en) * | 2021-04-28 | 2021-08-27 | 吉林大学 | Humanoid robot action system based on human body posture control and control method |
| CN113305830B (en) * | 2021-04-28 | 2022-08-16 | 吉林大学 | Humanoid robot action system based on human body posture control and control method |
| CN114252073A (en) * | 2021-11-25 | 2022-03-29 | 江苏集萃智能制造技术研究所有限公司 | Robot attitude data fusion method |
| CN114252073B (en) * | 2021-11-25 | 2023-09-15 | 江苏集萃智能制造技术研究所有限公司 | Robot attitude data fusion method |
| CN114440871A (en) * | 2021-12-29 | 2022-05-06 | 宜昌测试技术研究所 | Nine-axis magnetic compass data fusion method based on adaptive complementary filtering |
| CN114459466A (en) * | 2021-12-29 | 2022-05-10 | 宜昌测试技术研究所 | MEMS multi-sensor data fusion processing method based on fuzzy control |
| WO2023220972A1 (en) * | 2022-05-18 | 2023-11-23 | 北京小米移动软件有限公司 | Mobile device, pose estimation method and apparatus therefor, and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109108936A (en) | A kind of the self-balance robot control system and control method of Multiple Source Sensor data fusion | |
| CN110146077A (en) | Pose of mobile robot angle calculation method | |
| CN106292741A (en) | A kind of mobile machine user tripod head system based on brushless electric machine | |
| CN106919177A (en) | A kind of unmanned balance of bicycle control method based on rate gyroscope | |
| CN103792946B (en) | The signal processing method of motor type reversible pendulum system control and Intelligent self-balancing car whistle control system | |
| CN103557866B (en) | A kind of based on the virtual gyroscope of geomagnetic technology and the method for algorithm | |
| CN102323825B (en) | Torque compensation control method of DGMSCMG (double-gimbal magnetically suspended control moment gyroscope) system for spacecraft maneuver | |
| Feng et al. | Modeling and implementation of two-wheel self-balancing robot equipped with supporting arms | |
| CN107830861A (en) | Based on adaptive gain complementary filter moving object attitude measurement method and device | |
| CN106078744B (en) | A kind of double-wheel self-balancing robot Sliding Mode Adaptive Control system | |
| CN108731676A (en) | A kind of posture fusion enhancing measurement method and system based on inertial navigation technology | |
| CN107247459A (en) | Anti-interference flight control method and device | |
| CN111272158A (en) | Dynamic azimuth angle resolving method of MEMS electronic compass in complex magnetic disturbance scene | |
| Zhu et al. | A small low-cost hybrid orientation system and its error analysis | |
| CN110162068A (en) | A kind of control method of self-balance robot | |
| CN104682789A (en) | PID (Proportion Integration Differentiation) controller applied to two-wheeled robots | |
| Geist et al. | The wheelbot: A jumping reaction wheel unicycle | |
| Mohtasib et al. | Self-balancing two-wheel electric vehicle (STEVE) | |
| CN109202883A (en) | A kind of position control method of self-balance robot | |
| Huang et al. | Observer-based sensor fusion for power-assist electric bicycles | |
| Huang et al. | Determination of motor torque for power-assist electric bicycles using observer-based sensor fusion | |
| Campolo et al. | Attitude estimation of a biologically inspired robotic housefly via multimodal sensor fusion | |
| CN106041934B (en) | A kind of double-wheel self-balancing robot Sliding Mode Adaptive Control method | |
| Zhang et al. | Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors | |
| Campolo et al. | Multimodal sensor fusion for attitude estimation of micromechanical flying insects: A geometric approach |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190101 |
|
| RJ01 | Rejection of invention patent application after publication |