JP2011220825A - Attitude estimating device and method, attitude controlling device and method, and program - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide an attitude estimating device and method, an attitude controlling device and method, and a program that enable efficient calculation to obtain a three-dimensional attitude angle, shorten computing time and improve computing speed.SOLUTION: The attitude controlling device estimates the attitude of a moving object using state feedback. The attitude controlling device includes an input section 101 that receives input of attitude and attitude varying rates and converts the attitude and the attitude rates into a quaternion, a filter calculating section 102 that performs a filter calculation using the attitude and the attitude rates converted into the quaternion, and an attitude controlling section 104 that controls the attitude of the moving object based upon the attitude obtained by the filter calculation. The filter calculating section 102 uses spherical linear interpolation for filter calculation to estimate the attitude.

Description

  The present invention relates to an attitude estimation apparatus and method, an attitude control apparatus and method, and a program, and more particularly, to an attitude estimation apparatus and method, an attitude control apparatus and method, and a program for attitude control using a quaternion.

  In order to express what kind of posture an object is in a certain space, three methods have been devised. The three methods are Euler angles, attitude matrices, and quaternions. The Euler angle is expressed as shown in FIG. 6 on the right-handed coordinate axis. That is, when the thumb, index finger, and middle finger are pointed 90 degrees apart, the direction extending horizontally from the front is the roll axis, the direction toward the left is the pitch axis, and the upward direction is the yaw axis. The Euler angle expression has the advantage that it is very easy to understand, but it has the disadvantages of gimbal lock.

  In the expression by the attitude matrix, the attitude (rotation) is expressed by using a 3 × 3 matrix. The representation by the attitude matrix has the disadvantages that it is difficult to understand and that it becomes an illegal format due to accumulation of calculation errors, but it has the advantage that vector rotation can be used immediately.

Quaternion extends the imaginary number to 3D, and was invented in 1843 by William Hamilton, Ireland. The quaternion includes one scalar element and one 3D vector element, and is expressed as q = (ω, x, y, z) = ω + xi + yj + zk.
i2 = j2 = k2 = -1
ij = k, ji = -k, kj = -i, ik = -j

  Since the quaternion represents rotation, when the rotation θ around a certain vector n is represented by a quaternion, the following equation is obtained.

  Although the quaternion has a drawback that it is difficult to understand, there are advantages such as no singularity, that is, no gimbal lock, high speed calculation.

  If an expression method called Euler angle or Cardan angle is used to measure and express the posture of a mobile robot, it becomes an expression that is easy to understand for humans such as roll, pitch, and yaw, but Euler angle expression is As described above, since it has a mathematical singularity, a problem of gimbal lock occurs. In view of this, as a posture expression method capable of avoiding gimbal lock, a method using the posture matrix and a method using a quaternion have been proposed.

  For example, Patent Document 1 is known as attitude control using a quaternion. In posture estimation in Patent Document 1, the posture of a tiltable object is tracked and controlled based on signals output from a gyroscope and a tilt sensor. The signals output from the gyro are converted and integrated, thereby obtaining estimated position information in the form of a modified quaternion in which the yaw component is constrained to a value of zero. In addition, the deformed quaternion information of the same form is generated from the signal output from the tilt sensor, and the error component in the estimated position information is detected and corrected using this.

Special table 2003-521697 gazette

  By the way, when measuring the posture of the mobile robot, there are cases where an expensive sensor having sufficient accuracy, such as an optical fiber gyroscope, cannot be used. Cost and size are the main reasons. In order to solve this problem, there is a technique that uses multiple types of inexpensive and small MEMS sensors, processes them with a digital filter, synthesizes them, and realizes posture measurement with high accuracy that cannot be obtained with a single MEMS sensor. Conventionally proposed.

  However, the conventional filtering method was based on the expression of the posture angle by Euler angles. In principle, a gimbal lock is generated. However, there is a problem in that there are postures that cannot be measured because there is only a method of once stopping posture measurement and resuming posture measurement after reaching a posture away from a singular point.

  In other words, as described above, when the Euler angle is expressed as a method for expressing the posture of the mobile robot, the problem of gimbal lock always accompanies, and gimbal lock is mathematically a discontinuous point called a singular point. Is due to the existence of In addition, when using multiple types of inexpensive and small MEMS sensors, an inclination sensor that can directly measure the attitude angle (usually a triaxial acceleration sensor is often used) and an attitude angular velocity that is a change in attitude angle. Although the gyro sensor to be measured is synthesized by filtering, it is naturally impossible to synthesize a non-continuous posture angle with singular points and a continuous posture angular velocity without singular points, and it is complicated at the time of synthesis. There is a problem that it is necessary to perform conditional branching.

  The present invention has been made to solve such problems, and can efficiently perform calculation for obtaining a three-dimensional attitude angle, can reduce calculation time, and improve calculation speed. It is an object to provide an attitude estimation apparatus and method, an attitude control apparatus and method, and a program that can be used.

  An attitude estimation apparatus according to the present invention is an attitude estimation apparatus that estimates the attitude of a moving object using state feedback, and inputs an attitude and an attitude change rate, and converts them into a quaternion; A filter operation unit that performs a filter operation using the posture converted to the quaternion and a posture change rate, and the filter operation unit uses spherical linear interpolation for the filter operation for estimating the posture. Is.

  In the present invention, slerp (spherical linear interpolation) is used for the filter calculation for estimating the posture, so that the calculation can be performed efficiently without causing gimbal lock.

  Further, the filter calculation unit can synthesize the posture converted into the quaternion and the posture change rate. Furthermore, the filter operation unit can synthesize the posture and the integral value of the posture change rate converted into the quaternion. Thereby, slerp can be used for composition of posture and posture change rate, and calculation time can be shortened.

  Furthermore, it is possible to further include an Euler conversion unit that performs Euler conversion on the posture expressed by the quaternion after the filter operation, and in the case of posture control, the quaternion can be once converted into Euler angles. .

  The posture angle before the filter calculation is a value of a tilt sensor, and the posture change rate can be a roll angular velocity, a pitch angular velocity, and a yaw angular velocity.

  Furthermore, the posture, the posture change rate, and direction information may be input to the input unit. It is also possible to calculate without using the direction information.

  Furthermore, the direction information can be information indicating the value of the azimuth sensor or the direction of the tire when the moving body has a tire. It is also possible to use the tire direction of the moving body without using the azimuth angle sensor.

  Further, a Kalman filter or a frequency filter can be used as the filter.

  An attitude control apparatus according to the present invention is an attitude control apparatus that estimates the attitude of a moving object using state feedback, and inputs an attitude and an attitude change rate, and converts these into a quaternion; A filter calculation unit that performs a filter operation using the posture converted into the quaternion and a posture change rate; and a posture control unit that performs posture control of the moving body based on the posture obtained by the filter calculation. The filter calculation unit uses spherical linear interpolation for filter calculation for estimating the posture.

  The posture estimation method according to the present invention is a posture estimation method for estimating the posture of a moving object using state feedback, and a conversion step of inputting posture and posture change rate and converting them into a quaternion, A filter calculation step of performing a filter operation using the posture converted into the quaternion and a posture change rate, and in the filter calculation step, spherical linear interpolation is used for the filter operation for estimating the posture. Is.

  The posture control method according to the present invention is a posture estimation method for estimating the posture of a moving object using state feedback, and a conversion step of inputting posture and posture change rate and converting them into a quaternion, A filter calculation step of performing a filter operation using the posture and the posture change rate converted into the quaternion, and a posture control step of performing posture control of the moving body based on the posture obtained by the filter calculation. In the filter calculation step, spherical linear interpolation is used for the filter calculation for estimating the posture.

  In the present invention, since pose estimation is performed using a quaternion, spherical linear interpolation can be used for filter calculation. Since quaternions are used, the problem of gimbal lock that occurs when Euler angle representation is used can be avoided, and there is a problem that it becomes an illegal form due to accumulation of calculation errors, such as when using an attitude matrix. Hard to occur.

  A program according to the present invention causes a computer to execute the attitude control described above.

  According to the present invention, a posture estimation device and method, a posture control device and a method capable of efficiently performing calculation for obtaining a three-dimensional posture angle, reducing the calculation time, and improving the calculation speed. As well as programs.

It is a figure which shows the structure of the inverted motorcycle which concerns on embodiment of this invention. It is a figure for demonstrating the control apparatus which concerns on embodiment of this invention. It is a schematic diagram which shows the attitude | position control apparatus which concerns on embodiment of this invention. The block diagram which shows the detail of the attitude | position control apparatus which concerns on embodiment of this invention, It is a flowchart which shows the filtering method at the time of performing the synthesis | combination of the attitude | position angle and attitude | position angular velocity in embodiment of this invention. It is a schematic diagram which shows the right-handed coordinate axis of Euler angle expression.

  Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. In this embodiment, the present invention is applied to an inverted control of a coaxial two-wheeled vehicle as a representative example in which the posture of a mobile robot is estimated using a quaternion, and the posture of the mobile robot is stabilized using the estimated posture. It is applied. Although the present embodiment will be described as a coaxial two-wheeled vehicle, it goes without saying that the present invention can be applied to other leg-type or wheel-type robots.

  FIG. 1 is a diagram showing a configuration of an embodiment of an inverted motorcycle according to the present invention as an embodiment according to the present invention. FIG. 1A shows a front view, and FIG. 1B shows a side view. In FIG. 1, the traveling device according to the present embodiment is a coaxial two-wheeled vehicle having wheels 3A and 3B in parallel on a coaxial core line with respect to a main body 1 where a passenger stands.

  As shown in the figure, the coordinate system for the entire coaxial two-wheeled vehicle used in the following description is as follows. The vertical direction with respect to the axle is the X axis, the axle direction is the Y axis, the vertical direction is the Z axis, and the axle. The periphery (around the Y axis) is the pitch axis, and the rotation direction on the XY plane in the vehicle top view is the yaw axis.

  The traveling device according to the present embodiment includes a main body 1, a pair of drive units 2A and 2B that are coaxially attached to the main body 1, and wheels 3A and 3B that are rotationally driven by the drive units 2A and 2B, respectively. A T-shaped handle 4 for holding the occupant, a posture detecting device 4 for detecting the front / rear inclination (around the Y axis) of the main body 1, and a turning operation device 6 for instructing a turning operation are provided. The main body 1 is provided with a control device (not shown) that controls the vehicle, which will be described later. The main body 1 may incorporate a sensor or a switch (not shown) for identifying the occupant's boarding.

  This inverted two-wheeled vehicle is equipped with a posture estimation device that estimates the posture of a moving body using state feedback. The attitude control device uses spherical linear interpolation for filter calculation for estimating the attitude. Details of this will be described later.

  Next, the control apparatus for an inverted motorcycle according to the present embodiment will be described. FIG. 2 is a diagram for explaining the control device according to the present embodiment. As shown in FIG. 2, the control device 10 includes, for example, an arithmetic circuit 11 having a microcomputer (CPU (Central Processing Unit)), a program memory, a data memory, other RAM (Random Access Memory), and a ROM (Read Only Memory). The control device 10 is connected to a battery 26 and a pair of drive circuits 28L and 28R, which are also connected via an emergency stop switch 27. The drive circuits 28L and 28R individually control the rotational speed and direction of the pair of wheels, and the pair of wheel drive units 29L and 29R are individually connected to them.

  The control device 10 includes a detection signal from an angle detection sensor 23 that detects an inclination angle of 15, a detection signal from an attitude sensor unit 22, a getting-off assistance start trigger signal from a getting-off switch 24, and a foot sensor from a step sensor 25. A detection signal is supplied. In addition to the normal posture control, the control device 10 executes the dismount assistance control based on the dismount assistance start trigger signal and the foot detection signal.

  The attitude sensor unit 21 is used to detect angular velocity and acceleration during traveling of the coaxial two-wheeled vehicle and control the angular velocity and traveling acceleration, and includes, for example, a gyro sensor and an acceleration sensor. When the handle is tilted forward or backward, the step portion tilts in the same direction. This posture sensor unit 22 detects angular velocity and acceleration corresponding to the tilt. Then, the control device 10 drives and controls the wheel drive units 29L and 29R so that the vehicle moves in the inclination direction of the handle 23 based on the angular velocity and acceleration detected by the attitude sensor unit 22.

  Next, the attitude control device mounted on the control device 10 will be described. FIG. 3 is a schematic diagram showing the attitude control device. The posture estimation device estimates the posture of the moving object using state feedback. This posture estimation device is inputted with posture and posture change rate and converts them into a quaternion, and a filter calculation portion that performs filter calculation using the posture and posture change rate converted into a quaternion. 102, an Euler conversion unit 103 that performs Euler conversion of the posture expressed by the quaternion after the filter calculation, and a posture control unit 104 that performs posture control of the moving body based on the posture obtained by the filter calculation.

  The filter calculation unit 102 uses spherical linear interpolation for filter calculation for estimating the posture. The filter calculation unit 102 synthesizes the integrated value of the posture converted into the quaternion and the posture change rate. Here, the posture is a value of the angular velocity sensor, and the posture change rate is a roll angular velocity, a pitch angular velocity, a yaw angular velocity, or the like. In addition to the posture and the posture change rate, direction information is also input to the input unit 101.

  Next, a specific example of the attitude control device will be described. FIG. 4 is a block diagram showing details of the attitude control device, and FIG. 5 is a flowchart showing a filtering method when synthesizing attitude angles and attitude angular velocities about one axis by each Euler expression.

  As shown in FIG. 4, the attitude control device includes an angular velocity sensor 22a, an acceleration sensor 22b, an azimuth angle sensor 22c, a bias correction unit 31, an attitude angle calculation unit 32, an integration calculation unit 33, filters 34 and 35, and a bias estimation value calculation. And a posture angle estimated value calculation unit 37.

  The angular velocity sensor 22a, the acceleration sensor 22b, and the azimuth angle sensor 22c are included in the posture detection sensor unit 22. The angular velocity sensor 22a detects and outputs each angular velocity of roll, pitch, and yaw (step S104). The acceleration sensor 22b detects and outputs accelerations in the x, y, and z directions (step S101). The azimuth sensor 22c detects and outputs a direction. It is also possible to use the direction of the tire instead of the azimuth. Further, as described in Patent Document 1, a method that does not use an azimuth angle is also applicable.

  The bias correction unit 31 receives the angular velocity from the angle sensor 22a and corrects the deviation amount (bias) of the gyro sensor based on the estimated bias value (step S105, equation (2)). Specifically, the previous estimated bias value is subtracted from the angular velocity measurement value. The previous bias estimated value is the value of the bias estimated value calculation unit 36 of the previous cycle. This takes a sampling time to obtain a minute change rate (step S106). Then, the angular velocity measured by the gyro sensor is converted into a quaternion by converting the triaxial minute velocity into a minute rotation quaternion (step S107). Next, the initial attachment angle of the gyro is corrected (rotated). Thereby, the angular velocity observed by the gyro sensor can be converted into each displacement of the body reference coordinate system.

  The attitude angle calculation unit 32 calculates an attitude angle based on the acceleration and the azimuth angle from the acceleration sensor 22b and the azimuth angle sensor 22c. First, its own acceleration vector is canceled from the value of the acceleration sensor. Next, the rotation obtained by adding the azimuth angle to the acceleration sensor vector is converted into an attitude matrix (step S102). Then, this posture matrix is converted into a quaternion (step S103).

The integration calculation unit 33 predicts the bias value by integrating the gyro measurement value (step S108, equation (2)). That is, the quaternion representing the posture of the current cycle is predicted by integrating the angular displacement observed in the current cycle into the quaternion representing the posture of the previous cycle. _Assuming that the predicted value α predict of the current cycle is obtained as follows.

  The filters 34 and 35 perform the Kalman filter operation using the quaternion (step S109). When using the three-dimensional posture angle expression by Euler angles, the above calculation is repeated for three orthogonal axes (typically, roll, pitch, and yaw axes). At this time, the angular velocity needs to be rotationally converted between the body reference system to which the sensor is attached after removing the bias and the horizontal reference coordinate system with reference to gravity, and integrated at the angle of the horizontal reference coordinate system. .

On the other hand, in the embodiment, this calculation is performed at once using a quaternion, rather than performing the filter calculation three times for three axes using Euler angles. To add to one axis at Euler angle means to rotate about that axis, and to add at the same time to three axes means to calculate rotation for quaternion Will do.
At this time, attention is paid to the following formula for estimating the angle.

この式を変形すると、

If this equation is transformed,

α_ｎは、α_{ｐｒｅｄｉｃｔ}とα_{ｍｅａｓｕｒｅｄ}という２つの角度を、（１−Ｋ_０）：Ｋの比率で補間する角度という意味となる。別の表現では、α_{ｐｒｅｄｉｃｔ}を始点として、α_{ｍｅａｓｕｒｅｄ}とα_{ｐｒｅｄｉｃｔ}の差分のＫ_０倍を加えるということになる。αをオイラー角により１つの軸の角度ではなく、四元数と考えると、この演算は、ｓｌｅｒｐ（球面線形補間：spherical linear interpolation）と呼ばれる演算手法と同一となる。すなわち、姿勢の推定を行うためのフィルタ演算に球面線形補間を使用することができる。

α _n means an angle for interpolating two angles of α _predict and α _{measured at} a ratio of (1−K ₀ ): K. In other words, starting at an alpha _predict, it comes to adding _{K 0} times difference alpha _Measured and alpha _predict. If α is regarded as a quaternion rather than an angle of one axis by Euler angles, this calculation is the same as a calculation method called slerp (spherical linear interpolation). That is, spherical linear interpolation can be used for the filter calculation for estimating the posture.

The function slerp (q0, q1, t) that interpolates between the two quaternions q ₀ and q ₁ with a variable t is theoretically defined by the following equation.

計算機で実際に演算を行う際には、ｑ_０とｑ_１との間の弧の長さであるωを使用して、更に効率のよい下記のテクニックを利用することができる。

When the calculation is actually performed by the computer, the following technique that is more efficient can be used by using ω that is the length of the arc between q ₀ and q ₁ .

２つの四元数間の角度のサインとコサインは、下記の式で簡単に求めることができる。
ｑ_０＝（ｗ_０，ｘ_０，ｙ_０，ｚ_０）＝（ｗ_１，ｘ_１，ｙ_１，ｚ_１）とした時、
ｃｏｓω＝ｗ_０・ｗ_１＋ｘ_０・ｘ_１＋ｙ_０・ｙ_１＋ｚ_０・ｚ_１
ｓｉｎω＝ｓｑｒｔ（１−ｃｏｓ^２ω）
したがって、ｓｌｅｒｐの答えをｑ＝（ｗ，ｘ，ｙ，ｚ）とすると下記のような演算を行うことができる。
ω＝αｔａｎ２（ｓｉｎω，ｃｏｓω）
ｋ_０＝ｓｉｎ（（１．０−ｔ）ω）／ｓｉｎω
ｋ_１＝ｓｉｎ（ｔω）／ｓｉｎω
ｗ＝ｗ_０・ｋ_０＋ｗ_１・ｋ_１
ｘ＝ｘ_０・ｋ_０＋ｘ_１・ｋ_１
ｙ＝ｙ_０・ｋ_０＋ｙ_１・ｋ_１
ｚ＝ｚ_０・ｋ_０＋ｚ_１＋ｋ_１
なお、最後に、浮動小数点演算の誤差が蓄積しないように、正規化を行う（ステップＳ１１１）。

The sine and cosine of the angle between two quaternions can be easily obtained by the following formula.
When q ₀ = (w ₀ , x ₀ , y ₀ , z ₀ ) = (w ₁ , x ₁ , y ₁ , z ₁ ),
cos ω = w ₀ · w ₁ + x ₀ · x ₁ + y ₀ · y ₁ + z ₀ · z ₁
sin ω = sqrt (1-cos ² ω)
Therefore, if the answer of slerp is q = (w, x, y, z), the following calculation can be performed.
ω = α tan2 (sin ω, cos ω)
k ₀ = sin ((1.0−t) ω) / sinω
k ₁ = sin (tω) / sinω
w = w ₀ · k ₀ + w ₁ · k ₁
x = x ₀ · k ₀ + x ₁ · k ₁
y = y ₀ · k ₀ + y ₁ · k ₁
z = z ₀ · k ₀ + z ₁ + k ₁
Finally, normalization is performed so as not to accumulate errors in floating point arithmetic (step S111).

  The bias estimated value 36 temporarily returns the quaternion to the Euler angle in order to feed back to the bias (step S112). Note that if the bias is held in the quaternion instead of the Euler angle, slerp can be used and the calculation efficiency can be improved.

  The posture angle estimated value calculation unit 37 stores values for the next cycle calculation and posture control calculation. When a certain time has elapsed, the next cycle starts from the assumption of the acceleration sensor and the gyro sensor (step S113).

  As described above, the signals of the angle sensor (acceleration sensor) with a large noise and the angular velocity sensor (gyro) with a large drift are combined with an appropriate filter, and the estimated values of the posture angle and the bias (drift) with less noise are obtained. obtain. Applicable filter types include frequency filters (LPF (low pass filter) combined with HPF (high pass filter)) and Kalman filters. In this embodiment, the accuracy of the filter processing and the efficiency of the filter operation can be improved by using spherical linear interpolation (slerp) when the quaternion is filtered by the Kalman filter.

  It should be noted that the present invention is not limited to the above-described embodiments, and various modifications can be made without departing from the scope of the present invention.

  For example, in the above-described embodiment, the hardware configuration has been described. However, the present invention is not limited to this, and arbitrary processing may be realized by causing a CPU (Central Processing Unit) to execute a computer program. Is possible. In this case, the computer program can be stored using various types of non-transitory computer readable media and supplied to the computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included. The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

22a Angular velocity sensor 22b Acceleration sensor 22c Azimuth angle sensor 31 Bias correction unit 32 Attitude angle calculation unit 33 Integration calculation unit 34 Filter 35 Filter 36 Bias estimation value calculation unit 37 Attitude angle estimation value calculation unit

Claims (18)

A posture estimation device that estimates the posture of a moving object using state feedback,
An input unit that inputs posture and posture change rate and converts them into a quaternion;
A filter operation unit that performs a filter operation using the posture converted into the quaternion and the posture change rate;
The said filter calculating part is an attitude | position estimation apparatus which uses spherical linear interpolation for the filter calculation for estimating an attitude | position.   The posture estimation apparatus according to claim 1, wherein the filter calculation unit synthesizes the posture converted into the quaternion and a posture change rate.   The posture estimation device according to claim 1, wherein the filter calculation unit synthesizes an integrated value of the posture and the posture change rate converted into the quaternion.   The posture estimation apparatus according to claim 1, further comprising an Euler conversion unit configured to perform Euler conversion on the posture expressed by the quaternion after the filter operation.   The posture estimation device according to any one of claims 1 to 4, wherein the posture angle before the filter calculation is a value of a tilt sensor, and the posture change rate is a roll angular velocity, a pitch angular velocity, and a yaw angular velocity.   The posture estimation apparatus according to claim 1, wherein the posture and posture change rate, and direction information are input to the input unit.   The posture estimation apparatus according to claim 6, wherein the direction information is information indicating a value of an azimuth angle sensor or a direction of a tire when the moving body has a tire. An attitude control device that estimates the attitude of a moving object using state feedback,
An input unit that inputs posture and posture change rate and converts them into a quaternion;
A filter operation unit that performs a filter operation using the posture converted into the quaternion and the posture change rate;
A posture control unit that performs posture control of the moving body based on the posture obtained by the filter operation;
The said filter calculating part is an attitude | position control apparatus which uses spherical linear interpolation for the filter calculation for estimating an attitude | position.   The posture estimation apparatus according to claim 8, further comprising an Euler conversion unit that performs Euler conversion on the posture expressed by the quaternion after the filter operation.   The posture control apparatus according to claim 8 or 9, wherein the posture angle before the filter calculation is a value of a tilt sensor, and the posture change rate is a roll angular velocity, a pitch angular velocity, and a yaw angular velocity.   The posture estimation apparatus according to claim 8, wherein the posture and posture change rate and direction information are input to the input unit.   The posture estimation apparatus according to any one of claims 1 to 11, wherein the filter is a Kalman filter or a frequency filter. A posture estimation method for estimating the posture of a moving object using state feedback,
A conversion process in which the posture and the posture change rate are input and convert them into a quaternion;
A filter operation step of performing a filter operation using the posture converted into the quaternion and the posture change rate,
A posture estimation method in which spherical linear interpolation is used for filter calculation for estimating a posture in the filter calculation step.   The posture estimation method according to claim 13, further comprising an Euler conversion step of performing Euler conversion on a posture expressed by a quaternion after the filter calculation step.   The posture estimation method according to claim 13 or 14, wherein the posture angle before the filter calculation is a value of a tilt sensor, and the posture change rate is a roll angular velocity, a pitch angular velocity, and a yaw angular velocity.   The posture estimation method according to claim 13, wherein direction information is input together with the posture and the posture change rate. A posture estimation method for estimating the posture of a moving object using state feedback,
A conversion process in which the posture and the posture change rate are input and convert them into a quaternion;
A filter operation step of performing a filter operation using the posture converted into the quaternion and the posture change rate;
A posture control step of performing posture control of the moving body based on the posture obtained by the filter calculation,
A posture control method in which spherical linear interpolation is used for filter calculation for estimating a posture in the filter computation step. A program for causing a computer to execute a posture estimation process for a moving object using state feedback,
A posture estimation method for performing
A conversion process in which the posture and the posture change rate are input and convert them into a quaternion;
A filter operation step of performing a filter operation using the posture converted into the quaternion and the posture change rate,
In the filter calculation step, a program that uses spherical linear interpolation for filter calculation for estimating a posture.

Images