CN103975224B - Use the method for the error of fixed angles of rotary apparatus reckoning gyroscope and corresponding rotary apparatus - Google Patents

Abstract

The deviation value of the pitch axis angular-rate sensor under acquisition resting state and the deviation value of roll axis angular-rate sensor, wherein, under described resting state, it is static that the benchmark yaw axis (Z) of two-wheeled inverted pendulum vehicle body (2) is parallel to vertical direction.The deviation value of the pitch axis angular-rate sensor under acquisition rotation status and the deviation value of roll axis angular-rate sensor, wherein, under described rotation status, two-wheeled inverted pendulum vehicle (1) rotates with predetermined angular velocity of rotation when the benchmark yaw axis (Z) of two-wheeled inverted pendulum vehicle body (2) remains parallel to vertical direction.Based on deviation value under rotation status of sensor deviation value under static state, sensor and predetermined angular velocity of rotation, calculate pitch axis angular-rate sensor relative to the error of fixed angles of two-wheeled inverted pendulum vehicle body (2) and roll axis angular-rate sensor relative to the error of fixed angles of two-wheeled inverted pendulum vehicle body.

Description

Use the method for the error of fixed angles of rotary apparatus reckoning gyroscope and corresponding rotation Device

Background of invention

Technical field

The present invention relates to a kind of method calculating error of fixed angles and a kind of rotary apparatus.

Background technology

As such technology, Japan patent applicant announce No.2010-271918 (JP2010-271918A) explanation Use angular-rate sensor (gyro sensor) and acceleration transducer carry out gesture stability to two-wheeled inverted pendulum robot. Because the output valve of angular-rate sensor deteriorates over time, so in JP2010-271918A, such as by with specific Fixture supports then two-wheeled inverted pendulum robot of two-wheeled inverted pendulum robot and rotates from obliquity the most rearward with setting speed To the most forward obliquity, obtain the corrected value of angular-rate sensor.

Incidentally, if for the pitch axis angular-rate sensor phase that two-wheeled inverted pendulum vehicle is performed inverted pendulum control Established angle for the reference axis of body has error, then when two-wheeled inverted pendulum vehicle rotates (rotating around its yaw axis), by mistake The rotational component of difference can be detected by pitch axis angular-rate sensor, and therefore, inverted pendulum reference angle can be gradually deviated from.Knot Really, when when two-wheeled inverted pendulum vehicle stops the rotation, two-wheeled inverted pendulum vehicle tilts forward or backward, two-wheeled stands upside down Spot cars stands upside down.This makes occupant feel under the weather.

It addition, similarly, if roll axis angular-rate sensor has error relative to the established angle of the reference axis of body, then When two-wheeled inverted pendulum vehicle rotates, change is waved in tilting forward or backward of two-wheeled inverted pendulum vehicle.Similarly, this makes to take advantage of Member feels under the weather.

For this problem, in the prior art, take measures each angular-rate sensor relative to the benchmark of body The error of fixed angles of axle is set as zero as far as possible.In this measure, such as, since it is desired that guarantee the high precision of various degree of accuracy The framework of degree, so having problems in terms of reducing the size of body of two-wheeled inverted pendulum vehicle and weight, wherein said various Degree of accuracy e.g. each angular-rate sensor and be used as horizontal reference axle inclination sensor between installation degree of accuracy with And in the installation essence included between the sensor block of angular-rate sensor and inclination sensor and the body of two-wheeled inverted pendulum vehicle Exactness.

Due to alignment error produced by the problems referred to above be will be further illustrated.For example, it is assumed that for two-wheeled handstand spot cars Perform inverted pendulum control pitch axis angular-rate sensor relative to Plane of rotation (that is, yaw Plane of rotation) around roll axis Alignment error be β₀[radian], and vehicle is with the rotation of angular velocity γ point.In this case, γ point × sin (β₀) [radian/ Second] it is the output as error of pitch axis angular-rate sensor.Specifically, when pitch axis angular-rate sensor is relative to rotation Alignment error β of axle₀When being 0.5 [spending], then around pitch axis to rotate through that a circle 360 [spending] is realized be to just look like vehicle The cumulant of about 3 [spending] is have rotated around pitch axis.

During rotating a circle, the error of this pitch velocity occurs in relatively low frequency band, so can With it is envisioned that pass through high pass filter or by using dead band (neutral area) to eliminate error.But, such as, work as direction of rotation When changing the most to the left or to the right, high pass filter cannot use at all, so by pitch velocity integration is obtained During taking the calculating of Angle of Trim, error can be accumulated, and Angle of Trim can unrestrictedly deviate.

Summary of the invention

The present invention provides a kind of each for calculate in pitch axis angular-rate sensor and roll axis angular-rate sensor The technology of the individual error of fixed angles relative to inverted pendulum mobile unit body.

An aspect of of the present present invention provides one including inverted pendulum mobile unit body, pitch axis angular-rate sensor and side The inverted pendulum mobile unit of axis angular rate sensor of inclining calculating, pitch axis angular-rate sensor is relative to inverted pendulum mobile unit The error of fixed angles of body and roll axis angular-rate sensor are relative to the error of fixed angles of inverted pendulum mobile unit body Method.Described method includes: by obtaining output and the roll axis angular velocity of the pitch axis angular-rate sensor under resting state The output of sensor, obtains the deviation value of pitch axis angular-rate sensor under resting state and roll axis angular-rate sensor Deviation value, wherein, under described resting state, the benchmark yaw axis of inverted pendulum mobile unit body is static along vertical direction； By obtaining output and the output of roll axis angular-rate sensor of the pitch axis angular-rate sensor under rotation status, obtain institute State deviation value and the deviation value of roll axis angular-rate sensor of pitch axis angular-rate sensor under rotation status, wherein, Under described rotation status, inverted pendulum mobile unit remains parallel to vertical side at the benchmark yaw axis of inverted pendulum mobile unit body To state under rotate with predetermined angular velocity of rotation；With based on described sensor deviation value under described resting state, described Sensor deviation value under described rotation status and described predetermined angular velocity of rotation, calculate pitch axis angular-rate sensor phase Error of fixed angles and roll axis angular-rate sensor for inverted pendulum mobile unit body are relative to inverted pendulum mobile unit basis The error of fixed angles of body.Under rotation status, inverted pendulum mobile unit body can revolve while flatly being kept on the spot Turn.Inverted pendulum mobile unit can rotate under handstand state on pivot.Rotary apparatus can be used to make inverted pendulum move list Unit rotates on pivot, and described rotary apparatus includes: support base, and described support base supports inverted pendulum mobile unit；Multiple Pillar, the height of the plurality of pillar is adjustable, and the plurality of pillar is connected to support base；And rolling clamp, Described rolling clamp has the first component being connected to support base and the second component being connected to inverted pendulum mobile unit body, Wherein, the first component is connected to second component so that the first component can be parallel to support base and rotate.

Another aspect of the present invention provides a kind of rotary apparatus.Described rotary apparatus includes: support base, described support group Portion supports inverted pendulum mobile unit；Multiple pillars, the height of the plurality of pillar is adjustable, and the plurality of pillar is even Receive support base；And rolling clamp, described rolling clamp has the first component being connected to support base and is connected to stand upside down Swinging the second component of moving cell body, wherein, the first component is connected to second component so that the first component can be parallel to prop up Support group portion rotates.Above-mentioned rotary apparatus can use in the above-mentioned methods.

According to aforementioned aspect of the present invention, it is possible to calculate that pitch axis angular-rate sensor is relative to inverted pendulum mobile unit originally The error of fixed angles of body and roll axis angular-rate sensor are relative to the error of fixed angles of inverted pendulum mobile unit body.

Accompanying drawing explanation

The feature of the exemplary embodiment of the present invention, advantage and technology and industry meaning are described hereinafter with reference to the accompanying drawings Justice, the most identical reference represents identical element, and wherein:

Fig. 1 is the perspective view of the outward appearance of the two-wheeled inverted pendulum vehicle according to first embodiment；

Fig. 2 is the functional block diagram of the two-wheeled inverted pendulum vehicle according to first embodiment；

Fig. 3 is the perspective view of the horizontal rotation apparatus according to first embodiment；

Fig. 4 shows two-wheeled inverted pendulum vehicle and is set according to the state on the horizontal rotation apparatus of first embodiment Perspective view；

Fig. 5 is the control flow of the shaping modes according to first embodiment；With

Fig. 6 is the control flow of the shaping modes according to the second embodiment.

Detailed description of the invention

Hereinafter, referring to figs. 1 through Fig. 5, the first embodiment of the present invention will be described.

Two-wheeled inverted pendulum vehicle 1 (inverted pendulum mobile unit) shown in Fig. 1 is for riding along the transport of desired direction Take advantage of the vehicle of people (occupant) on two-wheeled inverted pendulum vehicle 1.Two-wheeled inverted pendulum vehicle 1 is in response to riding at two-wheeled handstand spot cars The operation of the occupant on 1 and move.

Two-wheeled inverted pendulum vehicle 1 is formed by two-wheeled inverted pendulum vehicle body 2, right wheel 3, revolver 4 and handle body 5.Right wheel 3 It is used as to be pivotally connected to the pair of wheels of two-wheeled inverted pendulum vehicle body 2 with revolver 4.Handle body 5 is with operating input Unit.

Two-wheeled inverted pendulum vehicle body 2 is formed by body lower part part 6, a pair leg rest portion 7 and control device 8.At body At low portion 6, battery or the like is installed.A pair leg rest portion 7 is arranged in body lower part part 6.Control device 8 to arrange In body lower part part 6.Handle body 5 is connected to the body lower part part 6 of two-wheeled inverted pendulum vehicle body 2.Handle body 5 It is arranged on the main direct of travel side of the body lower part part 6 of two-wheeled inverted pendulum vehicle body 2.Upper end at handle body 5 Place is provided with handle 9.Occupant catches handle 9.Right wheel motor 3a it is provided with at right wheel 3.It is provided with left wheel motor at revolver 4 4a。

Benchmark yaw axis Z as the benchmark yaw axis of two-wheeled inverted pendulum vehicle body 2 is such as defined as and leg rest portion 7 7a is vertical in leg rest face.Similarly, the benchmark pitch axis Y as the benchmark pitch axis of two-wheeled inverted pendulum vehicle body 2 such as defines For share with a pair right wheel 3 of body lower part part 6 and revolver 4 that are pivotally connected to two-wheeled inverted pendulum vehicle body 2 Pivot center.It addition, the benchmark roll axis X being used as the benchmark roll axis of two-wheeled inverted pendulum vehicle body 2 is such as defined as and base The axis that quasi-yaw axis Z is vertical and vertical with benchmark pitch axis Y.

As shown in Figure 2, control device 8 to include sensor unit 10, bid value computing unit 11, driver element 12, miss Difference correction unit 13 and input block 14.

Sensor unit 10 is formed by angular-rate sensor 15 and acceleration transducer 16.Angular-rate sensor 15 such as by Gyro sensor is formed.That is, angular-rate sensor 15 is by yaw axis angular-rate sensor, pitch axis angular-rate sensor and side The axis angular rate sensor that inclines is formed.The yaw velocity value of yaw axis angular-rate sensor output two-wheeled inverted pendulum vehicle body 2. The pitch velocity value of pitch axis angular-rate sensor output two-wheeled inverted pendulum vehicle body 2.Roll axis angular-rate sensor is defeated Go out the roll velocity value of two-wheeled inverted pendulum vehicle body 2.It addition, acceleration transducer 16 exports two-wheeled inverted pendulum vehicle body The 3-axis acceleration value of 2.

Input block 14 is formed by handle unit 17.It is initial that handle unit 17 exports from the handle body 5 shown in Fig. 1 The inclination value that installment state starts.That is, occupant stands upside down by making handle unit 17 tilt to control two-wheeled along desired direction The motion of spot cars 1.

Bid value computing unit 11 is formed by attitude angle computing unit 18 and inverted pendulum control computing unit 19.

Attitude angle computing unit 18 is such module, i.e. described module is by using Kalman filter or the like To the magnitude of angular velocity of each axle exported from angular-rate sensor 15 and the acceleration of each axle from acceleration transducer 16 output Angle value performs sensor and merges, and calculates the current pose angle value of two-wheeled inverted pendulum vehicle body 2.

Inverted pendulum control computing unit 19 is such module, i.e. described module is counted based on by attitude angle computing unit 18 The attitude angle value of the two-wheeled inverted pendulum vehicle body 2 calculated calculates and produces the state institute keeping two-wheeled inverted pendulum vehicle 1 to stand upside down The control information needed.It addition, inverted pendulum control computing unit 19 stands upside down based on the two-wheeled calculated by attitude angle computing unit 18 The input information that the attitude angle value of pendulum vehicle body 2 and the handle unit 17 from input block 14 receive calculates and produces and causes Two-wheeled inverted pendulum vehicle 1 performs the control needed for desired motion while keeping the handstand state of two-wheeled inverted pendulum vehicle 1 Information.

Driver element 12 is formed by right wheel driver element 20 and revolver driver element 21.Right wheel driver element 20 and revolver drive Moving cell 21 drives respectively based on the control information received from the inverted pendulum control computing unit 19 of bid value computing unit 11 Right wheel motor 3a and left wheel motor 4a.

Error correction unit 13 is by angular velocity memory element 22, error of fixed angles computing unit 23, error of fixed angles storage Unit 24 and angular velocity correction unit 25 are formed.Angular velocity memory element 22 stores each axle from angular-rate sensor 15 output Magnitude of angular velocity.Error of fixed angles computing unit 23 calculates based on each axis angular rate value being stored in angular velocity memory element 22 The error of fixed angles value of the angular-rate sensor 15 on two-wheeled inverted pendulum vehicle body 2.Error of fixed angles memory element 24 stores The error of fixed angles value calculated by error of fixed angles computing unit 23.Angular velocity correction unit 25 is based on being stored in established angle by mistake Error of fixed angles value correction in difference memory element 24 is from each axis angular rate of angular-rate sensor 15 output, and will correct The axis angular rate crossed exports attitude angle computing unit 18.

Above-mentioned control device 8 includes CPU (CPU), random access memory (RAM) and read only memory (ROM).The control program being stored in ROM is loaded by CPU and performs on CPU.So, control program causes such as The hardware of CPU plays the function of bid value computing unit 11, error correction unit 13 etc..

Fig. 3 shows the horizontal rotation apparatus 30 (rotary apparatus) in the present embodiment, and it is used for calculating angular-rate sensor 15 relative to the error of fixed angles of two-wheeled inverted pendulum vehicle body 2.Horizontal rotation apparatus 30 is by 31, four pillars 32 of support base Formed with rolling clamp 33.The height of four pillars 32 is adjustable, and these four pillars 32 are connected to support base 31. By the height of each in four pillars 32 of regulation, it is possible to support base 31 is adjusted to substantially horizontal attitude.Rotate Fixture 33 includes lower clamp 34 (the first component) and upper fixture 35 (second component).Lower clamp 34 is connected to support base 31.On Fixture 35 is connected to two-wheeled inverted pendulum vehicle body 2.Lower clamp 34 is connected to fixture 35 so that upper fixture 35 can be relative to propping up Support group portion 31 flatly rotates.

Fig. 4 shows the two-wheeled inverted pendulum vehicle 1 being arranged on horizontal rotation apparatus 30.Under state shown in the diagram, The right wheel 3 of two-wheeled inverted pendulum vehicle 1 contacts with the support base 31 of horizontal rotation apparatus 30 with suitable pressure with revolver 4.? Under this state, when right wheel 3 and revolver 4 rotate along contrary direction, two-wheeled inverted pendulum vehicle 1 is with predetermined anglec of rotation speed Degree pivotal rotation (by making two wheels rotate so that identical speed rotates along contrary direction).

It follows that the control flow controlling device 8 that two-wheeled inverted pendulum vehicle 1 will be described with reference to Fig. 5.Two-wheeled handstand spot cars The device 8 that controls of 1 also has the regulation mould shown in Fig. 5 in addition to having the normal running mode for transporting occupant Formula.Shaping modes is for calculating the angular-rate sensor 15 two-wheeled inverted pendulum vehicle body 2 relative to two-wheeled inverted pendulum vehicle 1 The pattern of error of fixed angles.

First, by explanation resolution theory under the background of shaping modes, will be described for this theory actually used subsequently Calculate the control flow of Fig. 5 of error.

In order to measure the attitude of two-wheeled inverted pendulum vehicle, use and measure trim shaft angle and roll the inclination sensor of shaft angle (it corresponds to acceleration transducer 16) and measurement pitch velocity, roll velocity and the angular-rate sensor of yaw velocity (it corresponds to angular-rate sensor 15).Inclination sensor is generally calculated below by utilizing 3-axis acceleration sensor to perform, And then obtain trim shaft angle α_0accWith roll axis angle beta_0acc.Trim shaft angle α_0accIt is that 3-axis acceleration sensor is relative to this Body is around the error of fixed angles of pitch axis.Roll axis angle beta_0accBe 3-axis acceleration sensor relative to body around roll axis Error of fixed angles.But, if using small-sized triaxial acceleration transducer as it is, then it is adapted to mount to two-wheeled and stands upside down Small-sized triaxial acceleration transducer on spot cars is generally of bigger noise, and is uncomfortable for inverted pendulum control When.If this is because use low pass filter to reduce noise, then inverted pendulum control can be due to the Phase delay of wave filter And become unstable.

By contrast, the gyroscope (being referred to as gyro sensor) as angular-rate sensor needs to be integrated So that acquisition angle, so there is drift in the long-time inverted pendulum control performed, and reference angle is gradually deviated from.This be because of For strictly not obtained the zero point of gyro sensor, this zero point changes according to temperature etc..

Thus, the sensor of both types is performed sensor fusion, and the non-speciality part quilt of each sensor Another sensor supplements.So, the sensor unit with desired character is obtained.Such as, Kalman filter etc. are used for passing Sensor merges.When using Kalman filter, it is possible to calculate the zero point error of gyro sensor, i.e. gyro sensor Deviation value.The deviation value extrapolated automatically changes so that by the converging angular obtained by the integration of gyro sensor In the angle arrived by acceleration transducer measurement.

Here, it will be considered that the alignment error of 3-axis acceleration sensor X (roll axis direction), the peace of Y (pitch axis direction) Fill error and the alignment error of Z (yaw axis direction) and roll velocity sensor, pitch velocity sensor and yaw angle The alignment error of velocity sensor.

The deviation of the measured value of acceleration transducer is the zero migration X of each axle sensor₀、Y₀And Z₀, and acceleration Sensor is relative to vehicle body reference axis (such as, the two-wheeled handstand spot cars of two-wheeled inverted pendulum vehicle 1 of two-wheeled inverted pendulum vehicle The benchmark yaw axis Z of body 2) established angle deviation be α_0acc(it stands upside down corresponding to the two-wheeled around two-wheeled inverted pendulum vehicle 1 The deviation of benchmark roll axis X of pendulum vehicle body 2), β_0acc(it is corresponding to the two-wheeled inverted pendulum around two-wheeled inverted pendulum vehicle 1 The deviation of the benchmark pitch axis Y of vehicle body 2) and γ_0acc.So, a total of six deviations.In these six deviations, perform Inverted pendulum control only needs α_0accAnd β_0acc.Under inverted pendulum control state, the Z axis output of acceleration transducer is about 1G, and The skew Z of zero point₀Can ignore.Established angle deviation γ along direction of rotation_0accWithout in inverted pendulum control.X0 and Y0 can To pass through α_0accAnd β_0accConsider typically.The spin matrix (1) of two angles used below and (2) are acceleration transducers The frame of reference (suffix " s ") and the transformation matrix of the vehicle body frame of reference (suffix " b ").

[mathematic(al) representation 1]

$\left(\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right)=\left(\begin{array}{ccc}\cos {\mathrm{\β}}_{0\mathrm{acc}}& 0& -\sin {\mathrm{\β}}_{0\mathrm{acc}}\\ 0& 1& 0\\ \sin {\mathrm{\β}}_{0\mathrm{acc}}& 0& \cos {\mathrm{\β}}_{0\mathrm{acc}}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_{0\mathrm{acc}}& \sin {\mathrm{\α}}_{0\mathrm{acc}}\\ 0& -\sin {\mathrm{\α}}_{0\mathrm{acc}}& \cos {\mathrm{\α}}_{0\mathrm{acc}}\end{array}\right)\left(\begin{array}{c}{X}_b\\ Y_b\\ Z_b\end{array}\right)...\left(1\right)$

[mathematic(al) representation 2]

$\left(\begin{array}{c}{X}_b\\ Y_b\\ Z_b\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_{0\mathrm{acc}}& -{\sin \mathrm{\α}}_{0\mathrm{acc}}\\ 0& \sin {\mathrm{\α}}_{0\mathrm{acc}}& \cos {\mathrm{\α}}_{0\mathrm{acc}}\end{array}\right)\left(\begin{array}{ccc}\cos {\mathrm{\β}}_{0\mathrm{acc}}& 0& \sin {\mathrm{\β}}_{0\mathrm{acc}}\\ 0& 1& 0\\ -{\sin \mathrm{\β}}_{0\mathrm{acc}}& 0& \cos {\mathrm{\β}}_{0\mathrm{acc}}\end{array}\right)\left(\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right)...\left(2\right)$

When in view of the β in above mathematic(al) representation (2)_0accAnd α_0accThe fact that both is the smaller value being close to zero Time, above mathematic(al) representation (2) can be revised as following mathematic(al) representation (3).

[mathematic(al) representation 3]

$\left(\begin{array}{c}{X}_b\\ Y_b\\ Z_b\end{array}\right)=\left(\begin{array}{ccc}1& 0& \sin {\mathrm{\β}}_{0\mathrm{acc}}\\ 0& 1& -{\sin \mathrm{\α}}_{0\mathrm{acc}}\\ -{\sin \mathrm{\β}}_{0\mathrm{acc}}& \sin {\mathrm{\α}}_{0\mathrm{acc}}& 1\end{array}\right)\left(\begin{array}{c}{X}_S\\ Y_S\\ Z_S\end{array}\right)=\left(\begin{array}{c}{X}_S+Z_S\·\sin {\mathrm{\β}}_{0\mathrm{acc}}\\ Y_S-Z_S\·\sin {\mathrm{\α}}_{0\mathrm{acc}}\\ Z_S-X_S\·\sin {\mathrm{\β}}_{0\mathrm{acc}}+Y_S\·\sin {\mathrm{\α}}_{0\mathrm{acc}}\end{array}\right)...\left(3\right)$

Similarly, six installation deviation of a total of gyro sensor (α indicates roll axis, and β indicates pitch axis, and γ indicates yaw axis), i.e. installation Angle of Trim deviation α of roll axis gyroscope_0βWith installation yaw angle deviation α_0γ, pitch axis gyro The installation side inclination deviation β of instrument_0αWith installation yaw angle deviation β_0γAnd the installation side inclination deviation γ of yaw axis gyroscope_0aWith Angle of Trim deviation γ is installed_0β.But, two-wheeled inverted pendulum vehicle will not rotate continuously around pitch axis, if so α_0γOr γ_0α The middle error that exists also is no problem, thus, it is supposed that this error is zero to be no problem.Similarly, two-wheeled inverted pendulum Vehicle will not rotate continuously around roll axis, so β_0γAnd γ_0βAlso assume that being zero is no problem.Thus, it will be considered that Angular displacement is only α_0βAnd β_0α.Use α_0βAnd β_0αThe gyro sensor frame of reference (suffix " s ") is rotated to be vehicle body base The transformation matrix of conventional coordinates (suffix " b ") is following mathematic(al) representation (4) and (5).In following mathematic(al) representation (4) etc., α_sPoint is the output valve of roll axis angular-rate sensor.β_sPoint is the output valve of pitch axis angular-rate sensor.γ_sPoint is yaw The output valve of axis angular rate sensor.α_bPoint is the roll velocity of two-wheeled inverted pendulum vehicle, β_bPoint is two-wheeled inverted pendulum vehicle Pitch velocity.γ_bPoint is the yaw velocity of two-wheeled inverted pendulum vehicle.

[mathematic(al) representation 4]

$\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_S\\ {\stackrel{\·}{\mathrm{\β}}}_S\\ {\stackrel{\·}{\mathrm{\γ}}}_S\end{array}\right)=\left(\begin{array}{ccc}\cos {\mathrm{\β}}_{0\mathrm{\α}}& 0& -\sin {\mathrm{\β}}_{0\mathrm{\α}}\\ 0& 1& 0\\ \sin {\mathrm{\β}}_{0\mathrm{\α}}& 0& \cos {\mathrm{\β}}_{0\mathrm{\α}}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_{0\mathrm{\β}}& \sin {\mathrm{\α}}_{0\mathrm{\β}}\\ 0& -\sin {\mathrm{\α}}_{0\mathrm{\β}}& \cos {\mathrm{\α}}_{0\mathrm{\β}}\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_b\\ {\stackrel{\·}{\mathrm{\β}}}_b\\ {\stackrel{\·}{\mathrm{\γ}}}_b\end{array}\right)...\left(4\right)$

[mathematic(al) representation 5]

$\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_b\\ {\stackrel{\·}{\mathrm{\β}}}_b\\ {\stackrel{\·}{\mathrm{\γ}}}_b\end{array}\right)=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_{0\mathrm{\β}}& -{\sin \mathrm{\α}}_{0\mathrm{\β}}\\ 0& \sin {\mathrm{\α}}_{0\mathrm{\β}}& \cos {\mathrm{\α}}_{0\mathrm{\β}}\end{array}\right)\left(\begin{array}{ccc}\cos {\mathrm{\β}}_{0\mathrm{\α}}& 0& \sin {\mathrm{\β}}_{0\mathrm{\α}}\\ 0& 1& 0\\ -{\sin \mathrm{\β}}_{0\mathrm{\α}}& 0& \cos {\mathrm{\β}}_{0\mathrm{\α}}\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_S\\ {\stackrel{\·}{\mathrm{\β}}}_S\\ {\stackrel{\·}{\mathrm{\γ}}}_S\end{array}\right)...\left(5\right)$

When in view of the α in above mathematic(al) representation (5)_0βAnd β_0αDuring the fact that both is the smaller value being close to zero, Above mathematic(al) representation (5) can be revised as following mathematic(al) representation (6).

[mathematic(al) representation 6]

$\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_b\\ {\stackrel{\·}{\mathrm{\β}}}_b\\ {\stackrel{\·}{\mathrm{\γ}}}_b\end{array}\right)=\left(\begin{array}{ccc}1& 0& \sin {\mathrm{\β}}_{0\mathrm{\α}}\\ 0& 1& -{\sin \mathrm{\α}}_{0\mathrm{\β}}\\ -{\sin \mathrm{\β}}_{0\mathrm{\α}}& \sin {\mathrm{\α}}_{0\mathrm{\β}}& 1\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_S\\ {\stackrel{\·}{\mathrm{\β}}}_S\\ {\stackrel{\·}{\mathrm{\γ}}}_S\end{array}\right)=\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\α}}}_S+{\stackrel{\·}{\mathrm{\γ}}}_S\·\sin {\mathrm{\β}}_{0\mathrm{\α}}\\ {\stackrel{\·}{\mathrm{\β}}}_S-{\stackrel{\·}{\mathrm{\γ}}}_S\·\sin {\mathrm{\α}}_{0\mathrm{\β}}\\ {\stackrel{\·}{\mathrm{\γ}}}_S-{\mathrm{\α}}_S\·\sin {\mathrm{\β}}_{0\mathrm{\α}}+{\stackrel{\·}{\mathrm{\β}}}_S\·\sin {\mathrm{\α}}_{0\mathrm{\β}}\end{array}\right)...\left(6\right)$

From learning above, the parameter needed for correcting mounting error is: enclosing between acceleration transducer and body reference axis Established angle deviation α around roll axis_0accWith established angle deviation β around pitch axis_0acc；The measurement axis of roll axis gyroscope and basis Established angle deviation α around pitch axis between body benchmark roll axis_0β；And, the measurement axis of pitch axis gyroscope and body base Established angle deviation β around roll axis between quasi-pitch axis_0α.When this four deviations can be accurately measured, even if when passing When sensor has installation deviation, it is also possible to obtain inclining in the body frame of reference exactly by calculating to rotate backward simply Oblique angle and rotational angular velocity.It means that in above mathematic(al) representation (6), when keeping intact in installation deviation Around yaw axis rotate time, the roll velocity α of roll axis gyroscope_sPoint and the pitch velocity β of pitch axis gyroscope_s Point change, and result, as roll velocity α_sPoint and pitch velocity β_sThe angle of heel α of the integrated value of point and trim angle beta quilt Change.

It is explained above the resolution theory under the background of shaping modes.

Hereinafter, by the control flow of explanatory diagram 5 successively.

First, as shown in Figure 4, two-wheeled inverted pendulum vehicle 1 is set on horizontal rotation apparatus 30, and is set at Under resting state, wherein, under described resting state, the benchmark yaw axis of two-wheeled inverted pendulum vehicle body 2 is parallel to vertical side To.Subsequently, the control model controlling device 8 changes into shaping modes (S300) from stop mode.Then, control device 8 to obtain Output valve X of the acceleration transducer 16 under above-mentioned resting state₀And Y₀(S310).Now, by the longest time period Interior equalization output valve X₀And Y₀, obtain accurate output valve X not having noise₀And Y₀.Similarly, control device 8 to obtain above-mentioned Output valve α of the angular-rate sensor 15 under resting state₀Point (output valve of roll axis angular-rate sensor), β₀Point (pitch axis The output valve of angular-rate sensor) and γ₀Point (S310).These output valves are stored in the angular velocity of error correction unit 13 and deposit In storage unit 22.Now, by equalization output valve α within the longest time period₀Point, β₀Point and γ₀Point, obtains and does not make an uproar Accurate output valve α of sound₀Point, β₀Point and γ₀Point.Control device 8 by acquired output valve α₀Point, β₀Point and γ₀Point is stored in In the angular velocity memory element 22 of error correction unit 13.It should be noted that under above-mentioned resting state, output valve α₀Point, β₀Point And γ₀Point should be essentially zero；But, due to the drift of angular-rate sensor, these values are actually not zero.Anticipate at this In justice, output valve β₀Point is the deviation value of the pitch axis angular-rate sensor of angular-rate sensor 15 under static state.Class As, output valve α₀Point is the deviation value of the roll axis angular-rate sensor of angular-rate sensor 15 under static state.After Sew " 0 " and represent resting state.

Subsequently, two-wheeled inverted pendulum vehicle 1 is along counterclockwise rotating on pivot on horizontal rotation apparatus 30 (S320).More specifically, be parallel to by the benchmark yaw axis of the two-wheeled inverted pendulum vehicle body 2 at two-wheeled inverted pendulum vehicle 1 With predetermined angular velocity of rotation γ under the state of vertical direction₁Point rotates two-wheeled inverted pendulum vehicle 1 (S320), two-wheeled inverted pendulum on the spot Vehicle 1 is placed in rotation status.Subsequently, control device 8 and obtain the trim shaft angle under the rotation status of two-wheeled inverted pendulum vehicle 1 The output of velocity sensor and the output (S330) of roll axis angular-rate sensor.Rotation status at two-wheeled inverted pendulum vehicle 1 Under, the output valve of pitch axis angular-rate sensor is by β₁Point represents, and similarly, the output valve of roll axis angular-rate sensor By α₁Point represents.Suffix " 1 " represents rotation status.It is desirable that, by equalization α within the longest time period₁Point and β₁ Point, obtains the α accurately not having noise₁Point and β₁Point.α₁Point and β₁Point and α₀Point and β₀Point is slightly different, this is because α₁Point and β₁Point is containing the rotation composition around yaw axis.β₁Put the deviation value corresponding to the pitch axis angular-rate sensor under rotation status, And similarly, α₁Point is corresponding to the deviation value (S330) of the roll axis angular-rate sensor under rotation status.

Then, based on sensor deviation value α under static state₀Point and β₀Point, the sensor deviation under rotation status Value α₁Point and β₁Point and predetermined angular velocity of rotation γ₁Point, error of fixed angles computing unit 23 calculates pitch axis angular-rate sensor Relative to the error of fixed angles of two-wheeled inverted pendulum vehicle body 2 and roll axis angular-rate sensor relative to two-wheeled inverted pendulum vehicle The error of fixed angles (S340) of body 2.In above reckoning, use the following mathematical table derived from above-mentioned mathematic(al) representation (6) Reach formula (7) and (8).

[mathematic(al) representation 7]

${\stackrel{\·}{\mathrm{\α}}}_1-{\stackrel{\·}{\mathrm{\α}}}_0={\stackrel{\·}{\mathrm{\γ}}}_1\·\sin {\mathrm{\β}}_{0\mathrm{\α}}\⇒{\mathrm{\β}}_{0\mathrm{\α}}={\sin }^{-1}\left(\frac{{\stackrel{\·}{\mathrm{\α}}}_1-{\stackrel{\·}{\mathrm{\α}}}_0}{{\stackrel{\·}{\mathrm{\γ}}}_1}\right)...\left(7\right)$

[mathematic(al) representation 8]

${\stackrel{\·}{\mathrm{\β}}}_1-{\stackrel{\·}{\mathrm{\β}}}_0={-\stackrel{\·}{\mathrm{\γ}}}_1\·\sin {\mathrm{\α}}_{0\mathrm{\β}}\⇒{\mathrm{\α}}_{0\mathrm{\β}}={\sin }^{-1}\left(\frac{{\stackrel{\·}{\mathrm{\β}}}_1-{\stackrel{\·}{\mathrm{\β}}}_0}{{\stackrel{\·}{\mathrm{\γ}}}_1}\right)...\left(8\right)$

It addition, following mathematic(al) representation (9) and (10) are for acceleration transducer 16 relative to two-wheeled inverted pendulum vehicle originally The error of fixed angles (S340) of body 2.Wherein, the variable g in following mathematic(al) representation (9) and (10) is acceleration of gravity,

[mathematic(al) representation 9]

$X_0=g\·\sin {\mathrm{\β}}_{0\mathrm{\α}}\⇒{\mathrm{\β}}_{0\mathrm{\α}}={\sin }^{-1}\left(\frac{X_0}{g}\right)...\left(9\right)$

[mathematic(al) representation 10]

$Y_0=-g\·\sin {\mathrm{\α}}_{0\mathrm{acc}}\⇒{\mathrm{\α}}_{0\mathrm{acc}}={\sin }^{-1}(-\frac{Y_0}{g})...\left(10\right)$

Subsequently, control device 8 and make the pivotal rotation of two-wheeled inverted pendulum vehicle 1 stop (S350), then make two-wheeled inverted pendulum Vehicle 1 rotates (S360) on horizontal rotation apparatus 30 on pivot clockwise.

The control flow of S370 to S390 is identical with the control flow of S330 to S350, so no longer will say it Bright.

Finally, error of fixed angles memory element 24 obtain the value calculated in S340 and the value that calculates in S380 it Between meansigma methods (S400), and such as meansigma methods is stored in and to include error of fixed angles memory element 24 shown in Fig. 2 Control in the RAM of device 8 (S410).Hereafter, control device 8 and complete shaping modes (S420), and will control from shaping modes Return to normal running mode.

After returning to normal running mode, the angular velocity correction unit 25 of the error correction unit 13 shown in Fig. 2 From error of fixed angles memory element 24 acquired value, and the output valve of correction angle velocity sensor 15 as required, then will value Output is to attitude angle computing unit 18.Such that make when angular-rate sensor 15 is relative to the peace of two-wheeled inverted pendulum vehicle body 2 When there is error in dress angle, it is also possible to error is removed in the calculating by low cost significantly.

Similarly, due to value acquired in above-mentioned mathematic(al) representation (9) to (10), it is possible to by the calculating of low cost Remove the acceleration transducer 16 error relative to the established angle of two-wheeled inverted pendulum vehicle body 2 significantly.

It is explained above the first embodiment of the present invention, and the advantage that the following is first embodiment.

That is, a kind of at the angular velocity including two-wheeled inverted pendulum vehicle body 2 be arranged on two-wheeled inverted pendulum vehicle body 2 The two-wheeled inverted pendulum vehicle 1 of sensor 15 (pitch axis angular-rate sensor and roll axis angular-rate sensor) calculates pitch axis Angular-rate sensor relative to the error of fixed angles of two-wheeled inverted pendulum vehicle body 2 and roll axis angular-rate sensor relative to two The method of the error of fixed angles of wheel inverted pendulum vehicle body 2 comprises the following steps.

(1) by obtaining the output of the pitch axis angular-rate sensor under resting state and roll axis angular-rate sensor Output obtains deviation value and the deviation value of roll axis angular-rate sensor of the pitch axis angular-rate sensor under resting state Step, wherein, under described resting state, the benchmark yaw axis Z of two-wheeled inverted pendulum vehicle body 2 is parallel to vertical direction.

(2) by obtaining the output of the pitch axis angular-rate sensor under rotation status and roll axis angular-rate sensor Output obtains deviation value and the deviation value of roll axis angular-rate sensor of the pitch axis angular-rate sensor under rotation status Step, wherein, under described rotation status, two-wheeled inverted pendulum vehicle 1 is at the benchmark yaw axis of two-wheeled inverted pendulum vehicle body 2 With predetermined angular velocity of rotation γ under the state that Z is parallel to vertical direction₁Point rotates.

(3) based on deviation value under rotation status of sensor deviation value under static state, sensor and predetermined Angular velocity of rotation γ₁Point calculate pitch axis angular-rate sensor relative to two-wheeled inverted pendulum vehicle body 2 error of fixed angles and Roll axis angular-rate sensor is relative to the step of the error of fixed angles of two-wheeled inverted pendulum vehicle body 2.

By said method, it is possible to calculate the pitch axis angular-rate sensor peace relative to two-wheeled inverted pendulum vehicle body 2 Fill angle error and the roll axis angular-rate sensor error of fixed angles relative to two-wheeled inverted pendulum vehicle body 2.

It follows that the second embodiment of the present invention will be described with reference to Fig. 6.Here, will be specifically explained this second embodiment The point different from above-mentioned first embodiment, and eliminate repeat specification in the appropriate case.

In the present embodiment, replace horizontal rotation apparatus 30, use simple level gauge to produce resting state, hereafter, Occupant actually rides two-wheeled inverted pendulum vehicle 1, and makes two-wheeled inverted pendulum vehicle 1 at pivot by operation handle body 5 Upper rotation.Illustrate using the control flow shown in Fig. 6.In S310, by using level gauge to produce resting state. Then, in S315, occupant rides on two-wheeled inverted pendulum vehicle 1.Also by occupant operate handle body 5 to perform S350, S360 and S390, and in S415 occupant from two-wheeled inverted pendulum vehicle 1.

Claims (7)

1. one kind is including inverted pendulum mobile unit body, pitch axis angular-rate sensor (15) and roll axis angular-rate sensor (15) inverted pendulum mobile unit (1) calculates described pitch axis angular-rate sensor relative to described inverted pendulum mobile unit originally The error of fixed angles of body and described roll axis angular-rate sensor are relative to the installation of described inverted pendulum mobile unit body (2) The method of angle error, described method includes:

By obtaining the output of the described pitch axis angular-rate sensor under resting state and described roll axis angular-rate sensor Output, the deviation value and the described roll axis angular velocity that obtain described pitch axis angular-rate sensor under described resting state pass The deviation value of sensor, wherein, under described resting state, the benchmark yaw axis of described inverted pendulum mobile unit body is along vertically Direction is static；

By obtaining the output of the described pitch axis angular-rate sensor under rotation status and described roll axis angular-rate sensor Output, the deviation value and the described roll axis angular velocity that obtain described pitch axis angular-rate sensor under described rotation status pass The deviation value of sensor, wherein, under described rotation status, described inverted pendulum mobile unit is at described inverted pendulum mobile unit body Described benchmark yaw axis remain parallel to described vertical direction state under rotate with predetermined angular velocity of rotation；With

The deviation under described resting state based on described pitch axis angular-rate sensor and described roll axis angular-rate sensor Deviation value under rotation status of value, described pitch axis angular-rate sensor and described roll axis angular-rate sensor and described Predetermined angular velocity of rotation, calculates the described pitch axis angular-rate sensor established angle relative to described inverted pendulum mobile unit body Error and described roll axis angular-rate sensor are relative to the error of fixed angles of described inverted pendulum mobile unit body.

Method the most according to claim 1, wherein,

Under described rotation status, described inverted pendulum mobile unit body rotates while flatly being kept on the spot.

Method the most according to claim 2, wherein,

Described inverted pendulum mobile unit rotates under handstand state on pivot.

4. according to the method described in any one in claims 1 to 3, wherein,

Under described resting state, at predetermined amount of time or in the longer time period, pitch axis angular velocity sensing described in equalization Each in the output of device and the output of described roll axis angular-rate sensor.

5. according to the method described in any one in claims 1 to 3, wherein,

Under described rotation status, at predetermined amount of time or in the longer time period, pitch axis angular velocity sensing described in equalization Each in the output of device and the output of described roll axis angular-rate sensor.

Method the most according to claim 2, wherein,

Utilizing rotary apparatus (30) to make described inverted pendulum mobile unit at pivots, described rotary apparatus includes: a support group Portion (31), described support base supports described inverted pendulum mobile unit；Multiple pillars (32), the height of the plurality of pillar can Regulate, and the plurality of pillar is connected to described support base；With rolling clamp (33), described rolling clamp has and is connected to First component (34) of described support base and be connected to the second component (35) of described inverted pendulum mobile unit body, wherein, Described first component is connected to described second component so that described first component can be parallel to described support base and rotate.

7. the rotary apparatus (30) used in method according to claim 2, described rotary apparatus includes:

Support base (31), described support base supports described inverted pendulum mobile unit；

Multiple pillars (32), the height of the plurality of pillar can regulate, and the plurality of pillar is connected to described support group Portion；With

Rolling clamp (33), described rolling clamp has and is connected to first component (34) of described support base and is connected to described The second component (35) of inverted pendulum mobile unit body, wherein, described first component is connected to described second component so that described First component can be parallel to described support base and rotate.