WO2018139621A1 - 慣性計測方法と慣性計測装置及び慣性計測プログラム - Google Patents

慣性計測方法と慣性計測装置及び慣性計測プログラム Download PDF

Info

Publication number
WO2018139621A1
WO2018139621A1 PCT/JP2018/002599 JP2018002599W WO2018139621A1 WO 2018139621 A1 WO2018139621 A1 WO 2018139621A1 JP 2018002599 W JP2018002599 W JP 2018002599W WO 2018139621 A1 WO2018139621 A1 WO 2018139621A1
Authority
WO
WIPO (PCT)
Prior art keywords
angular velocity
acceleration
signal
bias
moving body
Prior art date
Application number
PCT/JP2018/002599
Other languages
English (en)
French (fr)
Inventor
武雄 島次
範雄 紺屋
Original Assignee
株式会社ＦＡＤｒｏｎｅ
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by 株式会社ＦＡＤｒｏｎｅ filed Critical 株式会社ＦＡＤｒｏｎｅ
Priority to JP2018564675A priority Critical patent/JP6712037B2/ja
Priority to US16/481,452 priority patent/US10816337B2/en
Publication of WO2018139621A1 publication Critical patent/WO2018139621A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
    • G01B21/22—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring angles or tapers; for testing the alignment of axes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/185—Compensation of inertial measurements, e.g. for temperature effects for gravity
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P7/00—Measuring speed by integrating acceleration

Definitions

  • the present invention relates to an inertial measurement method, an inertial measurement device, and an inertial measurement program for measuring the posture / position of a moving body using an angular velocity sensor, an acceleration sensor, or the like.
  • an inertial measurement method in which an angular velocity sensor or an acceleration sensor is attached to a moving body such as a multicopter, a robot, an automobile, etc., and the posture or position of the moving body is detected by integrating these output signals is used for various applications.
  • a moving body such as a multicopter, a robot, an automobile, etc.
  • the posture or position of the moving body is detected by integrating these output signals.
  • the white noise included in the output signal is accumulated, and the error value superimposed on the output value randomly walks (the zero point drifts randomly) Since it cannot be detected with high accuracy, various techniques for suppressing random walk have been proposed.
  • a precision angular velocity sensor that detects an angular velocity of a moving body
  • a coarse posture angle detection means that detects a posture angle of the mobile body
  • a coarse posture angle detection means that detects a posture angle of the mobile body
  • a coarse posture angle detection means and a precision angular velocity sensor
  • the fine posture angle detection means is configured using, for example, a GPS alone or a DGPS (differential positioning system) composed of a plurality of GPSs.
  • the moving body attitude angle detection device of Patent Document 1 requires a coarse attitude angle detection means, which is a second detection means, in addition to the precision angular velocity sensor, which is the main detection means, so that the configuration of the apparatus is complicated. become.
  • the coarse attitude angle detection means is configured using DGPS, it cannot be detected with high accuracy in a non-GPS environment where radio waves from the positioning satellite do not reach. Therefore, in a factory or warehouse, under a bridge, It cannot be applied to mobiles used in the middle. Furthermore, even with this mobile body attitude angle detection device using DGPS, the influence of white noise cannot be sufficiently removed, and there is a limit in attitude angle detection accuracy.
  • errors in inertial measurement include errors due to noise superimposed on the signal, integration error due to the initial bias when performing integration, etc.
  • the gravitational acceleration detected by the angular velocity sensor and the earth angular velocity are also errors.
  • the gravitational acceleration component detected by the acceleration sensor changes. It is not easy to cancel (correct) the gravitational acceleration component from the signal. The same applies to the component of the earth angular velocity.
  • the present invention has been made in view of the above background art, and can effectively cancel various error factors, and can provide a highly accurate detection result even in a non-GPS environment. It is an object of the present invention to provide an inertial measurement method, inertial measurement device, and inertial measurement program for a moving body.
  • the present invention is an inertial measurement method for deriving a posture angle and a position movement amount of the moving body based on an angular velocity signal output from an angular velocity sensor attached to the body of the moving body and an acceleration signal output from the acceleration sensor, Obtained by integrating a filtering process for reducing a noise component superimposed on the angular velocity signal, an approximate initial posture angle from the acceleration signal and the angular velocity signal, and then integrating an acceleration obtained by subtracting gravitational acceleration from the acceleration signal.
  • Alignment processing that finely adjusts the initial attitude angle so that the absolute value of the velocity is less than a certain value and the absolute value of the angle obtained by integrating the angular velocity obtained by subtracting the earth angular velocity from the angular velocity signal is a value less than a certain value.
  • Posture angle is calculated by calculating the angular velocity after subtracting And output processing, the acceleration signals gravitational acceleration and calculates the acceleration by subtracting the acceleration bias from being constituted by a position shift amount calculation processing for calculating the position movement amount this second-order integration to,
  • the filtering process, the alignment process, and the angular velocity / acceleration bias calculation process are sequentially performed, and then the filtering process, the posture angle calculation process, and the position movement amount calculation process are performed in order. Repeatedly, The filtering process when the moving body is stationary, the alignment process, the attitude angle calculating process when the moving body is stationary, and the position movement amount calculating process when the moving body is stationary.
  • the filtering process when the moving body is moving, the posture angle calculating process when the moving body is moving, and the position movement amount calculating process when the moving body is moving are signals, This is an inertial measurement method that is performed by estimation using a motion state estimation filter that reduces noise superimposed on and suppresses errors in integral values caused by noise.
  • an azimuth signal output by an electronic compass attached to the moving body is obtained, and in the alignment process, an approximate initial attitude angle is calculated from the acceleration signal, the angular velocity signal, and the azimuth signal.
  • the angular velocity bias is calculated assuming that the earth angular velocity is zero
  • the attitude angle calculation process the angular velocity is calculated assuming that the earth angular velocity is zero, and this is integrated. Then, the attitude angle may be calculated.
  • the present invention is an inertial measurement device for deriving a posture angle and a position movement amount of the moving body based on an angular velocity signal output from an angular velocity sensor attached to the body of the moving body and an acceleration signal output from the acceleration sensor,
  • a filtering unit that performs processing to reduce a noise component superimposed on the angular velocity signal, an approximate initial posture angle is calculated from the acceleration signal and the angular velocity signal, and then an acceleration obtained by subtracting gravitational acceleration from the acceleration signal is integrated.
  • the initial attitude angle is finely adjusted so that the absolute value of the obtained velocity is less than a certain value and the absolute value of the angle obtained by integrating the angular velocity obtained by subtracting the earth angular velocity from the angular velocity signal is less than a certain value.
  • An alignment unit that performs processing to calculate, an angular velocity bias obtained by subtracting the earth angular velocity from the angular velocity signal, an angular velocity / acceleration bias calculating unit that performs processing to calculate an acceleration bias obtained by subtracting gravitational acceleration from the acceleration signal, Calculate the angular velocity obtained by subtracting the earth angular velocity and the angular velocity bias from the angular velocity signal, and integrate it.
  • a position angle calculation unit that performs a process of calculating a position angle, a position movement that performs a process of calculating a position movement amount by calculating the acceleration obtained by subtracting the gravitational acceleration and the acceleration bias from the acceleration signal, and integrating the second order A quantity calculation unit,
  • the filtering unit, the alignment unit, and the angular velocity / acceleration bias calculating unit sequentially execute the processes, and then the filtering unit, the posture angle calculating unit, and the position movement.
  • the amount calculation unit repeatedly executes the above processes in order, Processing performed by the filtering unit when the moving body is stationary, processing performed by the alignment unit, processing performed by the attitude angle calculation unit when the moving body is stationary, and the moving body is stationary.
  • the processing performed by the position movement amount calculation unit sometimes reduces noise superimposed on the signal, suppresses random walk of the integral value due to noise, and suppresses increase in error of the integral value due to bias.
  • the process performed by the filtering unit when the moving body is moving, the process performed by the posture angle calculating unit when the moving body is moving, and the position movement amount calculating unit when the moving body is moving is an inertial measurement device that is performed by estimation using a motion state estimation filter that reduces noise superimposed on a signal and suppresses an error of an integral value caused by noise.
  • the alignment unit obtains an azimuth signal output by an electronic compass attached to the moving body, and calculates a rough initial attitude angle from the acceleration signal, the angular velocity signal, and the azimuth signal.
  • the angular velocity / acceleration bias calculating unit performs processing to calculate the angular velocity bias assuming that the earth angular velocity is zero, and the attitude angle calculating unit calculates the angular velocity assuming that the earth angular velocity is zero. Then, this may be integrated to calculate the posture angle.
  • the present invention is an inertial measurement program configured by a program for executing each process for executing the inertial measurement method.
  • the inertial measurement method and inertial measurement device of the present invention perform a unique alignment process and angular velocity / acceleration bias calculation process when stationary, and further calculate gravitational acceleration and earth angular velocity when calculating the attitude angle and the amount of position movement. Since the process for canceling the component is performed, errors caused by the gravitational acceleration and the earth angular velocity can be reduced.
  • the present invention is characterized in that a stationary state estimation filter and a motion state estimation filter are separately used when performing estimation in each process.
  • a stationary state estimation filter As errors in inertial measurement, (a) error due to noise superimposed on the signal, (b) random walk of the integral value caused by noise, (c) increase in error of the integral value caused by bias, (d) There are errors in the integrated value due to noise, etc., and it is difficult to suppress them all with one kind of estimation filter.
  • estimation is performed using a stationary state estimation filter that suppresses (a), (b), and (c) when stationary, and (a) and (d) are suppressed during exercise.
  • the present invention makes it possible to select a relatively inexpensive angular velocity sensor by using an electronic compass bearing signal for initial azimuth detection and ignoring the earth angular velocity, thereby reducing the overall cost of the apparatus. You can plan.
  • the inertial measurement device of the present invention can be easily configured on a general-purpose computer using the inertial measurement program of the present invention, and a random walk is performed like the mobile body attitude angle detection device of Patent Document 1. There is no need to provide a second detection means for suppression. Therefore, the apparatus can be configured very simply.
  • FIG. 3 is a slow chart showing the contents of angular velocity / acceleration bias calculation processing in FIG. 2. It is a flowchart which shows the content of the attitude angle calculation process in FIG. 2, Comprising: The process (a) performed at the time of a stillness, and the process (b) performed at the time of exercise
  • FIG. 1 It is a figure (a) which shows the conditions of the simulation performed in order to confirm the effect of a movement state estimation filter, and the figure (b) which shows the conditions of the simulation of a comparative example. It is the waveforms (a) and (b) which show the simulation result performed on condition of Fig.16 (a) and (b). It is a system configuration figure of a second embodiment of the inertial measurement device of the present invention. It is a flowchart which shows the content of the alignment process which the inertial measurement apparatus of 2nd embodiment performs. It is a flowchart which shows the content of the initial attitude angle calculation step in FIG.
  • the inertial measurement device 10 of this embodiment is based on an angular velocity signal ⁇ B output from an angular velocity sensor 14 attached to a body 12 a of a moving body 12 and an acceleration signal a B output from an acceleration sensor 16.
  • An apparatus for deriving the posture angles ⁇ (roll), ⁇ (pitch), ⁇ (yaw) and position movement amount r G of the moving body 12, and the inertial measurement method of this embodiment causes the inertial measurement method to be executed.
  • An inertial measurement program configured by an execution program for each process is installed in the inertial measurement apparatus 10 and executed.
  • the symbols ⁇ B and ⁇ G are vectors, “B” indicates that each axis component is expressed in the aircraft coordinate system, and “G” indicates each axis component in the geographic coordinate system.
  • the inertial measurement apparatus 10 includes a filtering unit 18, an alignment unit 20, an angular velocity / acceleration bias calculation unit 22, an attitude angle calculation unit 24, and a position movement amount calculation unit 26. First, the function of each block will be briefly described.
  • Filtering unit 18 performs processing for reducing the noise component superimposed on the angular velocity signal omega B (the filtering process S1s, S1u).
  • Angular velocity and acceleration bias calculation unit 22 calculates the angular velocity bias [omega] b B from the angular velocity signal omega B minus the earth angular velocity .omega.p B, the process of calculating an acceleration bias ab B from the acceleration signal a B minus the acceleration of gravity gp B (Angular velocity / acceleration bias calculation processing S3) is performed.
  • Attitude angle calculating section 24 calculates a corrected angular velocity [omega] h B minus the earth angular velocity .omega.p B and the angular velocity bias [omega] b B from the angular velocity signals omega B, attitude angle ⁇ by integrating it, theta, processing for calculating the [psi (posture Corner calculation processing S4s, S4u) is performed.
  • Position movement amount calculating section 26 calculates the correction acceleration ah G from the acceleration signal a B minus the acceleration of gravity gp B and acceleration bias ab B, calculates the position movement amount r G this second-order integration and process ( Position movement amount calculation processing S5s, S5u) is performed.
  • the inertial measurement apparatus 10 sequentially performs the filtering process S1s, the alignment process S2, and the angular velocity / acceleration bias calculation process S3 when the moving body 12 is stationary, and then the filtering process S1s or S1u, posture angle calculation processing S4s or S4u, and position movement amount calculation processing S5s or S5u are repeatedly executed in order.
  • “stationary” refers to, for example, a state where the mobile object 12 is on the ground and is in a standby state.
  • the determination as to whether the vehicle is stationary or moving is performed by the inertial measurement device 10 analyzing the angular velocity signal ⁇ B and the acceleration signal a B by an appropriate method.
  • a filtering process S1s is performed.
  • the filtering is performed by estimating the noise component superimposed on the angular velocity signal ⁇ B by using the stationary state estimation filter As1.
  • the stationary state estimation filter As1 is a filter that is set so as to reduce noise superimposed on a signal and suppress an error of an integration value caused by noise when performing integration. I will explain later.
  • the alignment process S2 includes an initial posture angle calculation step S21 and three initial posture angle adjustment steps S22. Perform S23 and S24. As shown in FIG. 3, when the alignment process S2 is started and the initial attitude angles ⁇ 0, ⁇ 0, and ⁇ 0 have not been calculated, an initial attitude angle calculation step S21 is performed.
  • step S211 the acceleration signal a B and the angular velocity signal ⁇ B after filtering are acquired, and the average value and variance of each signal at the specified time are calculated. To do. Then, after the lapse of the specified time, the process proceeds to step S212, where it is assumed that the average value of the acceleration signal a B calculated in step S211 is equal to gp B of the true gravitational acceleration, and the average value of the angular velocity signal ⁇ B is true. Assuming that it is equal to the earth angular velocity ⁇ p B , initial attitude angles ⁇ 0, ⁇ 0, ⁇ 0 are calculated (step S212).
  • This assumption is based on the premise that the alignment process S2 is performed based on the acceleration signal a B and the angular velocity signal ⁇ B at rest. Based on the above assumptions, the approximate initial posture angles ⁇ 0, ⁇ 0, and ⁇ 0 are set as shown in FIG. It can be easily obtained by the simple formula described in the inside.
  • the first initial posture angle adjustment step S22 is a step for finely adjusting the initial posture angle ⁇ 0.
  • the process proceeds to step S221, where the gravity acceleration gp G in the geographic coordinate system is converted into the gravity acceleration gp B in the body coordinate system using the coordinate conversion matrix C (GB) shown in Equation (1).
  • the coordinate transformation matrix C (GB) is a known matrix, C ⁇ , C ⁇ , C ⁇ on the right side are cos ⁇ , cos ⁇ , cos ⁇ , respectively, and S ⁇ , S ⁇ , S ⁇ are sin ⁇ , sin ⁇ , sin ⁇ , respectively.
  • step S221 After performing step S221, the process proceeds to step S222, and the corrected acceleration is canceled by subtracting the X-axis component gp (x) of the gravitational acceleration gp B from the X-axis component a (x) of the acceleration signal a B. Calculate ah (x). Then, the stationary state estimation filter As1 is applied to the corrected acceleration ah (x) to estimate the velocity vh (x) in the X-axis direction, which is an integral value (step S223). This stationary state estimation filter As1 is the same as that used in the filtering process S1s.
  • step S224 the average value of the speed vh (x) at the specified time is calculated (step S224).
  • the process proceeds to step S225, and the initial posture angle ⁇ 0 is changed (increased / decreased) from the immediately preceding value. If the initial posture angle ⁇ 0 is changed and steps S221 to S224 are executed repeatedly, the absolute value of the velocity vh (x) will eventually become below a certain value ( ⁇ 0), so the process proceeds to S226 and the initial posture after adjusting ⁇ 0 at that time The angle is ⁇ 0. This completes the adjustment of ⁇ 0.
  • the second initial posture angle adjustment step S23 is a step of finely adjusting the initial posture angle ⁇ 0.
  • the process proceeds to step S231, where the gravitational acceleration gp G in the geographic coordinate system is converted into the gravitational acceleration gp B in the body coordinate system using the coordinate transformation matrix C (GB) shown in equation (1).
  • Step S231 is the same processing as step S221 described above.
  • step S231 After performing step S231, the process proceeds to step S232, and the corrected acceleration in which the influence of the gravitational acceleration is canceled by subtracting the Y-axis component gp (y) of the gravitational acceleration gp B from the Y-axis component a (y) of the acceleration signal a B. Calculate ah (y). Then, the stationary state estimation filter As1 is applied to the corrected acceleration ah (y), and the velocity vh (y) in the Y-axis direction, which is an integral value, is estimated (step S233). This stationary state estimation filter As1 is also the same as that used in the filtering process S1s.
  • step S234 the average value of the speed vh (y) at the specified time is calculated (step S234). If the absolute value of the velocity vh (y) is larger than a certain value, the process proceeds to step S235, and the initial posture angle ⁇ 0 is changed (increased / decreased) from the immediately preceding value. If the initial posture angle ⁇ 0 is changed and steps S231 to S234 are executed repeatedly, the absolute value of the speed vh (y) will eventually become below a certain value ( ⁇ 0), so the process proceeds to S236, and the initial posture after adjusting ⁇ 0 at that time The angle is ⁇ 0. This completes the adjustment of ⁇ 0.
  • the third initial posture angle adjustment step S24 is a step of finely adjusting the initial posture angle ⁇ 0. As shown in FIG. 7, first, the process proceeds to step S241, and the earth angular velocity ⁇ p G in the geographic coordinate system is converted into the earth angular velocity ⁇ p B in the body coordinate system using the coordinate transformation matrix C (GB) shown in Expression (1). .
  • step S241 the process proceeds to step S242, the X-axis component of the angular velocity signal ⁇ B ⁇ (x) by subtracting the X axis component .omega.p (x) of the earth angular velocity .omega.p B, corrected angular velocity canceling the influence of the earth angular velocity ⁇ h (x) is calculated. Then, the stationary state estimation filter As1 is applied to the corrected angular velocity ⁇ h (x), and the angle ⁇ h (x) in the X-axis direction, which is an integral value, is estimated (step S243). This stationary state estimation filter As1 is also the same as that used in the filtering process S1s.
  • step S244 the average value of the angle ⁇ h (x) at the specified time is calculated (step S244). If the absolute value of the angle ⁇ h (y) is larger than a certain value, the process proceeds to step S245, and the initial posture angle ⁇ 0 is changed (increased or decreased) from the immediately preceding value. If the initial posture angle ⁇ 0 is changed and steps S241 to S244 are repeated, the absolute value of the angle ⁇ h (x) will eventually become below a certain value ( ⁇ 0), so the process proceeds to S246, and the initial posture after adjustment of ⁇ 0 at that time The angle is ⁇ 0. This completes the adjustment of ⁇ 0, and the alignment process S2 ends.
  • the angular velocity / acceleration bias calculation process S3 is performed when the angular velocity / acceleration bias ⁇ b B and ab B have not been calculated in the stationary state.
  • the process first proceeds to step S31, and the global angular velocity ⁇ p G of the geographic coordinate system is converted to the coordinate conversion shown in the equation (1).
  • the matrix C (GB) is used to convert to the Earth's angular velocity ⁇ p B in the aircraft coordinate system.
  • the process proceeds to step S32, and the angular velocity error ⁇ B is calculated by subtracting the earth angular velocity ⁇ p B from the angular velocity signal ⁇ B.
  • step S35 the gravitational acceleration gp G of the geographic coordinate system is converted into the coordinate transformation matrix C (GB) shown in the equation (1). Used to convert to gravity acceleration gp B in the body coordinate system.
  • step S36 the acceleration error ⁇ a B is calculated by subtracting the gravitational acceleration ⁇ p B from the acceleration signal a B.
  • the posture angle calculation processing S4s is performed through filtering processing S1s as shown in FIG.
  • the attitude angle calculation process S4s first proceeds to step S41s, where the earth angular velocity ⁇ p G of the geographic coordinate system is calculated using the coordinate transformation matrix C (GB) shown in Expression (1). Convert to the global angular velocity ⁇ p B of the coordinate system.
  • step S42s by subtracting the earth angular velocity .omega.p B and the angular velocity bias [omega] b B from the angular velocity signals omega B, calculates a corrected angular velocity [omega] h B which cancels the error factor of the earth angular velocity or the like.
  • step S43s where the corrected angular velocity ⁇ h B of the body coordinate system is converted into the posture angular velocities ⁇ (dots) and ⁇ (dots) using the coordinate transformation matrix R (BA) shown in Equation (2). , ⁇ (dot).
  • the coordinate transformation matrix R (BA) is a known matrix.
  • step S44s the stationary state estimation filter As1 is applied to each of the posture angular velocities ⁇ (dots), ⁇ (dots), and ⁇ (dots), and the posture angles ⁇ , ⁇ , and ⁇ that are integral values are applied. Is estimated.
  • This stationary state estimation filter As1 is the same as that used in the filtering process S1s.
  • the coordinate transformation matrix C (G-B) is updated, and the attitude angle calculation process S4s ends (step S45s).
  • a position movement amount calculation process S5s is performed.
  • the position movement amount calculation process S5s first proceeds to step S51s, where the gravitational acceleration gp G of the geographic coordinate system is calculated using the coordinate transformation matrix C (GB) shown in Expression (1). Convert to gravity acceleration gp B in the body coordinate system. Then, the process proceeds to step S52s, by subtracting the gravitational acceleration gp B and acceleration bias ab B from the acceleration signal a B, calculates a correction acceleration ah B canceling the error factor of the gravitational acceleration and the like.
  • step S53s the corrected acceleration ah B in the body coordinate system is converted into the acceleration ah G in the geographic coordinate system using the coordinate conversion matrix C (GB) shown in Expression (1).
  • step S53s proceeds to step S54s, applying a quiescent state estimation filter As2 to each axis component of acceleration ah G, estimates the a second-order integration value position movement amount r G.
  • the stationary state estimation filter As2 is set so as to reduce the noise superimposed on the signal and suppress the error of the integral value due to the noise when performing the integration, like the previous stationary state estimation filter As1. The specific mode will be described later. This completes the attitude angle calculation process S5s.
  • the posture angle calculation process S4u is performed through a filtering process S1u as shown in FIG. Filtering S1u, unlike the above-described filtering process S1s, the processing for reducing the noise component superimposed on the angular velocity signal omega B, performed by estimation with motion state estimation filter Au1.
  • the motion state estimation filter Au1 is a filter that is set so as to reduce noise superimposed on a signal and suppress an error of an integrated value caused by noise when performing integration. I will explain later.
  • steps S41u to S45u are sequentially performed as shown in FIG. 9B.
  • steps S41u to S43u and S45u have the same contents as steps S41s to S43s and S45s at rest, and the difference is step S44u (underlined portion).
  • step S44u the motion state estimation filter Au1 is applied to the posture angular velocities ⁇ (dots), ⁇ (dots), and ⁇ (dots) that are the processing results of step S43u, and the posture angles ⁇ , ⁇ , and ⁇ that are integral values are applied. Is estimated.
  • This motion state estimation filter Au1 is the same as that used in the filtering process S1u.
  • the coordinate transformation matrix C (G-B) is updated, and the attitude angle calculation process S4u ends (step S45u).
  • step S51u to S54u are sequentially performed.
  • steps S51u to S53u are the same as steps S51s to S53s at rest, and the difference is step S54u (underlined portion).
  • step S54u the motion state estimation filter Au2 is applied to each axis component of the acceleration ah G , which is the processing result of step S53u, to estimate the position movement amount r G that is a second-order integral value.
  • This motion state estimation filter Au2 is set so as to reduce the noise superimposed on the signal and suppress the error of the integral value due to the noise when performing the integration, like the previous motion state estimation filter Au1. A specific mode will be described later. This completes the attitude angle calculation process S5u.
  • the inertial measurement device 10 sequentially executes the filtering process S1s, the alignment process S2, and the angular velocity / acceleration bias calculation process S3 when the moving body 12 is stationary, and then the filtering process S1s or S1u, and the posture.
  • the angle calculation process S4s or S4u and the position movement amount calculation process S5s or S5u in order, the attitude angles ⁇ (roll), ⁇ (of the moving body 12 based on the angular velocity signal ⁇ B and the acceleration signal a B Pitch), ⁇ (yaw) and position movement amount r G are derived.
  • the stationary state estimation filter As1 is a filter used when performing first-order integration in the stationary alignment process S2 and the attitude angle calculation process S4s, and is also used in the stationary filtering process S1s.
  • the following equations (3) and (4) are set in the stationary state estimation filter As1 as calculation equations for performing estimation.
  • Equation (3) the state variables are v (k), v (k-1), a (k) and a (k-1), and the output Y (k) is a (k).
  • the observation noise W (k) is added.
  • v is an integral value of a. For example, if a is an acceleration, v is a velocity, and if a is an angular velocity, v is an angle. Then, the 4 ⁇ 4 matrix in Equation (3) reduces the noise superimposed on the stationary signal, and the random walk of the integration value due to the noise and the integration when performing the integration. It is set so as to suppress an increase in the error of the integral value due to the bias when performing.
  • the motion state estimation filter As1 is a filter used when performing first-order integration in the posture angle calculation process S4u during exercise, and is also used in the filtering process S1u during exercise.
  • the above formula (4) and the following formula (5) are set as calculation formulas for estimation.
  • the state variables are v (k), v (k-1), a (k) and a (k-1), and the output Y (k) is a (k).
  • the observation noise W (k) is added.
  • the matrix of 4 rows and 4 columns in equation (5) reduces the noise superimposed on the signal at the time of movement, and suppresses the error of the integrated value due to noise when performing integration. Is set.
  • Equation (3) the contents of each matrix are different in the second row.
  • the second row of the stationary state estimation filter As1 is integrated for a while after the timing k-1 (for example, until the intermediate point between the timing k-1 and the timing k). It is set based on the idea that the value v is held constant.
  • the second row of the motion state estimation filter Au1 is set based on the idea that the integral value v changes with a constant slope from timing k-1 to timing k. Yes. Due to the difference in the setting method in the second row, the stationary state estimation filter As1 can perform estimation suitable for the stationary state, and the motion state estimation filter Au1 can perform estimation suitable for the motion state.
  • the inventor conducted a simulation test to confirm the operation of the stationary state estimation filter As1.
  • the signal a is white noise (a signal with an average value 0 and variance 1 generated by a statistically controlled noise generating means), and a stationary state estimation filter
  • the signal a and the integral value v were estimated by applying As1.
  • an integrated value v of the signal a was calculated using a general integrator 28.
  • a second test was performed to confirm the operation of the stationary state estimation filter As1.
  • the signal a is superimposed on the bias (signal level 1) with the above white noise, and the stationary state estimation filter As1 is applied to integrate the signal a.
  • the value v was estimated.
  • an integrated value v of the signal a was calculated using a general integrator 28.
  • the results of the second test are as shown in FIGS. 15 (a) and 15 (b).
  • the estimation a of the stationary state estimation filter As1 it can be seen that the white noise included in the signal a is reduced. Also, looking at the calculation v of the integrator 28 (comparative example), the zero point drifts upward with time, and an increase in the error of the integral value due to the bias occurs when performing the above-described integration. I understand that On the other hand, when the estimation v of the stationary state estimation filter As1 is viewed, the zero point hardly drifts, and an increase in error due to the bias is effectively suppressed.
  • a third test was performed to confirm the operation of the motion state stationary state estimation filter Au1.
  • the third test as shown in FIG. 16A, the above-mentioned white noise is superimposed on an ideal sine wave (amplitude 1, frequency 4 Hz), and a motion state estimation filter Au1 is applied.
  • the signal a and the integral value v were estimated.
  • an integrated value v of the signal a was calculated using a general integrator 28.
  • the results of the third test are as shown in FIGS. 17 (a) and 17 (b). Looking at the estimation a of the motion state estimation filter Au1, it can be seen that the white noise included in the signal a is reduced. Further, when looking at the calculation v (comparative example) of the integrator 28, an error in the integrated value due to noise is seen, but when looking at the estimated v of the motion state estimation filter Au1, the error due to noise due to this integration is effective. Is suppressed.
  • the stationary state estimation filter As2 is a filter used when performing second-order integration in the stationary position movement amount calculation process S5s.
  • the following equations (6) and (7) are set in the stationary state estimation filter As2 as calculation equations for performing estimation.
  • the state variables are r (k), r (k-1), v (k), v (k-1), a (k) and a (k-1),
  • the output Y (k) is obtained by adding the observation noise W (k) to a (k).
  • v is an integral value of a
  • r is an integral value of v. That is, if a is an acceleration, v is a speed and r is a position movement amount.
  • the matrix of 6 rows and 6 columns in Equation (6) reduces noise superimposed on the stationary signal, and increases the random walk of the integral value caused by noise and the error of the integral value caused by bias. Is set to suppress.
  • the motion state estimation filter Au2 is a filter used when performing second-order integration in the position movement amount calculation S5u during motion.
  • the above formula (7) and the following formula (8) are set as calculation formulas for estimation.
  • Equations (7) and (8) the state variables are r (k), r (k-1), v (k), v (k-1), a (k) and a (k-1),
  • the output Y (k) is obtained by adding the observation noise W (k) to a (k).
  • the matrix of 6 rows and 6 columns in the equation (8) reduces noise superimposed on the signal during movement, and suppresses an error in the integrated value due to noise when performing integration. Is set.
  • Equation (6) and Equation (8) the contents of each matrix differ between the second row and the fourth row.
  • the difference in the fourth row is the same as described above with reference to FIGS. 11 (a) and 11 (b). That is, the fourth row of the stationary state estimation filter As2 is set based on the idea that the integral value v is held constant for a while after the timing k ⁇ 1, and the fourth row of the motion state estimation filter Au2 is As shown in FIG. 11B, it is set based on the idea that the integral value v changes with a constant slope from timing k-1 to timing k.
  • the concept of the second line is the same as that of the fourth line. That is, the second row of the stationary state estimation filter As2 is set based on the idea that the integral value r is held constant for a while after the timing k ⁇ 1, and the second row of the motion state estimation filter Au2 is It is set based on the idea that the integral value r changes with a constant slope from timing k-1 to timing k.
  • the stationary state estimation filter As2 can perform estimation suitable for the stationary state, and the motion state estimation filter Au2 performs estimation suitable for the motion state. Can do.
  • the inertial measurement method 10 according to this embodiment and the inertial measurement apparatus 10 installed with the inertial measurement program perform the alignment process S2 and the angular velocity / acceleration bias calculation process S3 when stationary, Since the process of canceling the components of gravitational acceleration and earth angular velocity is performed when calculating the amount of position movement, errors due to gravitational acceleration and earth angular velocity can be reduced.
  • the stationary state estimation filters As1 and As2 and the motion state estimation filters Au1 and As2 are selectively used. Therefore, (a) an error due to noise superimposed on the signal, which is a problem in inertial measurement, (b) a random walk of an integral value due to noise, (c) an increase in an error of an integral value due to bias, (d) noise Estimate the errors caused by the integral value using stationary state estimation filters As1, As2 when stationary, and effectively suppress (a), (b), and (c), while exercising, (A) and (d) can be effectively suppressed by performing estimation using the estimation filters Au1 and Au2, and the posture angles ⁇ , ⁇ , ⁇ , and the position movement amount r G can be measured with high accuracy. it can.
  • the inertial measurement device 10 can be easily configured with a general-purpose computer, and, like the moving body posture angle detection device of Patent Document 1, it is necessary to provide a second detection means in order to suppress a random walk. Nor. Therefore, the apparatus can be configured very simply.
  • the inertial measurement device 30 (and the inertial measurement method) of this embodiment is obtained by changing a part of the configuration of the inertial measurement device 10 (and the inertial measurement method).
  • the inertial measurement device 10 is the same as the inertial measurement device 10.
  • the configuration is denoted by the same reference numeral and description thereof is omitted.
  • the inertial measurement device 10 of the first embodiment has a feature that the number of necessary sensor elements is small (only two types of the angular velocity sensor 14 and the acceleration sensor 16), but in order to effectively perform the above alignment, It is necessary to use a high-performance angular velocity sensor 14 that can accurately detect the earth angular velocity, and the apparatus may be expensive.
  • the inertial measurement device 30 of the second embodiment uses the azimuth signal H of the electronic compass 32 to calculate the initial attitude angle ⁇ 0, so that the inexpensive angular velocity sensor 14 is It can be used, and the cost of the entire apparatus can be suppressed.
  • the overall flow of the inertial measurement method performed by the inertial measurement device 30 is the same as the flow of the inertial measurement device 10 (FIG. 2), but the alignment processing S2, the acceleration / angular velocity bias calculation processing S3, and the posture angular velocity calculation processing S4s and S4u. Since there are slightly different parts, the different parts will be described in order.
  • the alignment processing S2 performed by the inertial measurement device 30 is the same as the inertial measurement device 10 in the three initial posture angles ⁇ 0, ⁇ 0, and ⁇ 0, but the initial posture is the same.
  • the angle ⁇ 0 is calculated based on the direction signal H of the electronic compass 32 and is not adjusted. Therefore, in the initial posture angle calculation step S61, as shown in FIG. 20, only the initial posture angles ⁇ 0 and ⁇ 0 are calculated (step S611), and the adjustment is made (step S612).
  • the initial attitude angle ⁇ 0 can be calculated by using the azimuth signal H of the electronic compass 32, and alignment processing is performed. Can do. Since an angular velocity sensor that is faster than the earth angular velocity can be detected by an inexpensive angular velocity sensor, the cost of the angular velocity sensor 14 can be reduced.
  • the same effect as in the case of the inertial measurement device 10 can be obtained, and although the number of necessary sensors increases, the cost of the entire device can be reduced.
  • the inertial measurement method, inertial measurement device, inertial measurement device, and inertial measurement program of the present invention are not limited to the above embodiment.
  • the setting of the stationary state estimation filter is not limited to the setting of the stationary state estimation filter As1 (Equations (3) and (4)) and As2 (Equations (6) and (7)).
  • the content of the formula may be changed within a range where the effect obtained is obtained.
  • the stationary state estimation filter As1 is shared by a plurality of processes. However, a filter whose setting is changed according to the contents of each process may be used.
  • the setting of the motion state estimation filter is not limited to the setting of the motion state estimation filter Au1 (Equations (4) and (5)) and Au2 (Equations (7) and (8)), and the effects described above.
  • the content of the expression may be changed within the range where is obtained.
  • the motion state estimation filter Au1 is shared by a plurality of processes. However, it is also possible to use a filter whose settings are changed according to the contents of each process.
  • the inertial measurement method and inertial measurement device of the present invention are suitable for inertial measurement of a multicopter or the like flying in the air. Needless to say, the inertial measurement method and inertial measurement device of the present invention can be applied to a moving body other than a multicopter. Can do.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

様々な誤差要因を効果的にキャンセルすることができ、非ＧＰＳ環境下でも高精度な検出結果が得られる移動体の慣性計測方法及び慣性計測装置である。角速度信号ωのフィルタリング処理S1s，S1uを行う。加速度信号及び角速度信号から概略の初期姿勢角を算出した後、これらを微調整するアライメント処理S2を行う。角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理S3を行う。角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理S4s，S4uを行う。加速度信号から重力加速度及び加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理S5s，S5uを行う。

Description

慣性計測方法と慣性計測装置及び慣性計測プログラム
　本発明は、角速度センサや加速度センサ等を使用して移動体の姿勢・位置を計測する慣性計測方法と慣性計測装置及び慣性計測プログラムに関する。
　従来、マルチコプタ、ロボット、自動車等の移動体に角速度センサや加速度センサを取り付け、これらの出力信号を積分して移動体の姿勢や位置等を検出する慣性計測方法が種々の用途に用いられている。この慣性計測方法において、各センサの出力信号を単純積分すると、出力信号に含まれる白色雑音が累積されて、出力値に重畳する誤差の値がランダムウォークし（ゼロ点がランダムにドリフトし）、高精度に検出することができないため、従来から、ランダムウォークを抑制するための様々な技術が提案されている。
　従来、例えば特許文献１に開示されているように、移動体の角速度を検出する精角速度センサと、移動体の姿勢角を検出する粗姿勢角検出手段と、粗姿勢角検出手段及び精角速度センサの出力信号に基づいて移動体の姿勢角を推定する姿勢角推定器とを備えた移動体姿勢角検出装置があった。姿勢角推定器は、粗姿勢角信号と精加速度信号に動的カルマンフィルタを適用することにより、移動体の姿勢角及び精角速度センサのドリフト値を推定し、ドリフトをキャンセルした姿勢角推定値を出力する。粗姿勢角検出手段は、例えばＧＰＳ単体又は複数のＧＰＳからなるＤＧＰＳ（ディファレンシャル測位方式）を用いて構成される。
特開２０００－２９２１７０号公報
　特許文献１の移動体姿勢角検出装置は、メインの検出手段である精角速度センサに加え、第二の検出手段である粗姿勢角検出手段を別個に設ける必要があるので、装置の構成が複雑になる。また、ＤＧＰＳを用いて粗姿勢角検出手段を構成した場合、測位衛星からの電波が届かない非ＧＰＳ環境下では高精度な検出ができなくなるので、工場や倉庫の中、橋梁の下、トンネルの中等で使用される移動体には適用することができない。さらに、ＤＧＰＳを用いたこの移動体姿勢角検出装置でも、白色雑音による影響を十分に除去することはできず、姿勢角の検出精度には限界があった。
　また、慣性計測における誤差は、上述したランダムウォーク以外に、信号に重畳しているノイズによる誤差、積分を行う際に、初期のバイアスに起因する積分値の誤差等があり、その他、加速度センサと角速度センサによって検出される重力加速度と地球角速度（地球の自転や公転によって発生する角速度）も誤差となる。例えば、マルチコプタ等の慣性計測を行う場合、図２３、図２４に示すように、空中で移動体１２が傾いたり揺動したりすると加速度センサで検出される重力加速度の成分が変化するので、出力信号から重力加速度の成分をキャンセルする（補正する）のは容易ではない。地球角速度の成分についても同様である
　本発明は、上記背景技術に鑑みて成されたものであり、様々な誤差要因を効果的にキャンセルすることができ、非ＧＰＳ環境下でも高精度な検出結果が得られる移動体の慣性計測方法と慣性計測装置及び慣性計測プログラムを提供することを目的とする。
　本発明は、移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測方法であって、
　前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理とで構成され、
　前記移動体が静止している時に前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
　前記移動体が静止している時の前記フィルタリング処理、前記アライメント処理、前記移動体が静止している時に前記姿勢角算出処理、及び前記移動体が静止している時に前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行い、
　前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行う慣性計測方法である。
　この慣性計測方法は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出するようにしてもよい。
　本発明は、移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測装置であって、
　前記角速度信号に重畳しているノイズ成分を低減する処理を行うフィルタリング部と、前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整する処理を行うアライメント部と、前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する処理を行う角速度・加速度バイアス算出部と、前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する処理を行う姿勢角算出部と、前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する処理を行う位置移動量算出部とで構成され、
　前記移動体が静止している時に前記フィルタリング部、前記アライメント部及び前記角速度・加速度バイアス算出部が前記の各処理を順番に実行し、その後、前記フィルタリング部、前記姿勢角算出部及び前記位置移動量算出部が前記の各処理を順番に繰り返し実行し、
　前記移動体が静止している時に前記フィルタリング部が行う処理、前記アライメント部が行う処理、前記移動体が静止している時に前記姿勢角算出部が行う処理、及び前記移動体が静止している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行われ、
　前記移動体が運動している時に前記フィルタリング部が行う処理、前記移動体が運動している時に前記姿勢角算出部が行う処理、及び前記移動体が運動している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行われる慣性計測装置である。
　この慣性計測装置は、前記アライメント部は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出する処理を行い、前記角速度・加速度バイアス算出部は、地球角速度をゼロと仮定して前記角速度バイアスを算出する処理を行い、前記姿勢角算出部は、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出するようにしてもよい。
　本発明は、前記慣性計測方法を実行させるための各処理の実行用プログラムにより構成された慣性計測プログラムである。
　本発明の慣性計測方法及び慣性計測装置は、静止している時に独特なアライメント処理と角速度・加速度バイアス算出処理とを行い、さらに姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくすることができる。
　さらに、本発明は、各処理で推定を行うとき、静止状態推定フィルタと運動状態推定フィルタを使い分けているという点にも特徴がある。慣性計測の誤差としては、（ａ）信号に重畳しているノイズによる誤差、（ｂ）ノイズに起因する積分値のランダムウォーク、（ｃ）バイアスに起因する積分値の誤差の増大、（ｄ）ノイズに起因する積分値の誤差等があり、これらを１種類の推定フィルタですべて抑制することは難しい。しかし、本発明のように、静止時は（ａ）、（ｂ）、（ｃ）を抑制する静止状態推定フィルタを使用して推定を行い、運動時は（ａ）、（ｄ）を抑制する運動状態推定フィルタを使用して推定を行うことにより、慣性計測の誤差を効果的に抑制し、姿勢角及び位置移動量を高精度に計測することができる。
　本発明により、初期の方位角の検出用に電子コンパスの方位信号を利用し、地球角速度を無視することによって、比較的安価な角速度センサを選択することが可能になり、装置全体のコストダウンを図ることができる。
　さらに、本発明の慣性計測装置は、汎用のコンピュータに本発明の慣性計測プログラムを用いて容易に構成することができ、しかも、特許文献１の移動体姿勢角検出装置のように、ランダムウォークを抑制するために第二の検出手段を付設する必要もない。従って、装置を非常にシンプルに構成することができる。
本発明の慣性計測装置の第一の実施形態のシステム構成図である。 第一の実施形態の慣性計測装置が行う慣性計測方法（本発明の慣性計測方法の第一の実施形態）を示すフローチャートである。 図２の中のアライメント処理の内容を示すフローチャートである。 アライメント処理で行う初期姿勢角算出ステップの内容を示すフローチャートである。 アライメント処理で行う第１の初期姿勢角調整ステップの内容を示すフローチャートである。 アライメント処理で行う第２の初期姿勢角調整ステップの内容を示すフローチャートである。 アライメント処理で行う第３の初期姿勢角調整ステップの内容を示すフローチャートである。 図２の中の角速度・加速度バイアス算出処理の内容を示すスローチャートである。 図２の中の姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。 図２の中の位置移動量算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。 静止状態推定フィルタと運動状態推定フィルタの設定方法の違いを説明するタイミングチャート（ａ）、（ｂ）である。 静止状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。 図１２（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。 静止状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。 図１４（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。 運動状態推定フィルタの効果を確認するために行ったシミュレーションの条件を示す図（ａ）、比較例のシミュレーションの条件を示す図（ｂ）である。 図１６（ａ）、（ｂ）の条件で行ったシミュレーション結果を示す波形（ａ）、（ｂ）である。 本発明の慣性計測装置の第二の実施形態のシステム構成図である。 第二の実施形態の慣性計測装置が行うアライメント処理の内容を示すフローチャートである。 図１９の中の初期姿勢角算出ステップの内容を示すフローチャートである。 第二の実施形態の慣性計測装置が行う角速度・加速度バイアス算出処理の内容を示すフローチャートである。 第二の実施形態の慣性計測装置が行う姿勢角算出処理の内容を示すフローチャートであって、静止時に行う処理（ａ）、運動時に行う処理（ｂ）である。 地理座標系と機体座標系を示す図である。 移動体が傾いた時の重力加速度の成分の変化を示す図（ａ）、（ｂ）である。
　以下、本発明の慣性計測装置と慣性計測装置及び慣性計測プログラムの第一の実施形態について、図１～図１７に基づいて説明する。この実施形態の慣性計測装置１０は、図１に示すように、移動体１２の機体１２ａに取り付けた角速度センサ１４が出力する角速度信号ωB及び加速度センサ１６が出力する加速度信号ａBに基づいて、移動体１２の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒGを導出する装置であり、この実施形態の慣性計測方法は、この慣性計測方法を実行させるための各処理の実行用プログラムにより構成された慣性計測プログラムを、慣性計測装置１０にインストールして実行される。なお、□B，□Gという表記の記号はベクトルであり、「B」は各軸成分を機体座標系で表したものであることを示し、「G」は各軸成分を地理座標系で表したものであることを示す。
　慣性計測装置１０は、フィルタリング部１８、アライメント部２０、角速度・加速度バイアス算出部２２、姿勢角算出部２４、位置移動量算出部２６とで構成されている。まず、各ブロックの機能を簡単に説明する。
　フィルタリング部１８は、角速度信号ωBに重畳しているノイズ成分を低減する処理（フィルタリング処理S1s，S1u）を行う。
　アライメント部２０は、加速度信号ａB及び角速度信号ωBから概略の初期姿勢角φ0，θ0，ψ0を算出した後、加速度信号ａBから重力加速度ｇpBを差し引いた加速度ahBを積分して求まる速度VhBの絶対値が一定以下になり、且つ角速度信号ωBから地球角速度ωpBを差し引いた角速度αhBを積分して求まる角度の絶対値が一定以下の値になるように、初期姿勢角φ0，θ0，ψ0を微調整する処理（アライメント処理S2）を行う。
　角速度・加速度バイアス算出部２２は、角速度信号ωBから地球角速度ωpBを差し引いた角速度バイアスωbBを算出するとともに、加速度信号ａBから重力加速度ｇpBを差し引いた加速度バイアスａbBを算出する処理（角速度・加速度バイアス算出処理S3）を行う。
　姿勢角算出部２４は、角速度信号ωBから地球角速度ωpB及び角速度バイアスωbBを差し引いた補正角速度ωhBを算出し、これを積分して姿勢角φ，θ，ψを算出する処理（姿勢角算出処理S4s，S4u）を行う。
　位置移動量算出部２６は、加速度信号ａBから重力加速度ｇpB及び加速度バイアスａbBを差し引いた補正加速度ａhGを算出し、これを２階積分して位置移動量ｒGを算出する処理（位置移動量算出処理S5s，S5u）を行う。
　慣性計測装置１０は、図２に示すように、移動体１２が静止している時に、フィルタリング処理S1s、アライメント処理S2、角速度・加速度バイアス算出処理S3を順番に実行し、その後、フィルタリング処理S1s又はS1u、姿勢角算出処理S4s又はS4u、位置移動量算出処理S5s又はS5uを順番に繰り返し実行する。
　移動体１２がマルチコプタの場合、「静止」とは、例えば、地面に着地してスタンバイしている状態等のことであり、飛行を開始すると「運動」の状態になる。静止状態か運動状態かの判断は、慣性計測装置１０が角速度信号ωBと加速度信号ａBを適宜の方法で分析することによって行う。
　以下、各処理の詳細な内容について順番に説明する。図２に示すように、まず静止状態か運動状態かの判断を行い、静止状態と判断した場合、フィルタリング処理S1sを行う。フィルタリングは、角速度信号ωBに重畳しているノイズ成分を低減する処理を、静止状態推定フィルタＡs1を使用した推定によって行う。静止状態推定フィルタＡs1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。
　フィルタリング処理S1sが終了した後、静止状態でアライメントが未終了の場合は、アライメント処理S2を行う。アライメント処理S2は、初期姿勢角算出ステップS21と３つの初期姿勢角調整ステップS22．S23，S24を行う。図３に示すように、アライメント処理S2を開始し、初期姿勢角φ0，θ0，ψ0が未算出の場合、初期姿勢角算出ステップS21を行う。
　初期姿勢角算出ステップS21では、図４に示すように、まず、ステップS211で、加速度信号ａBとフィルタリング処理後の角速度信号ωBを取得し、規定時間における各信号の平均値と分散を計算する。そして、規定時間の経過後、ステップS212に進み、ステップS211で算出した加速度信号ａBの平均値が真の重力加速度のｇpBと等しいと仮定し、さらに角速度信号ωBの平均値が真の地球角速度ωpBと等しいと仮定して、初期姿勢角φ0，θ0，ψ0を算出する（ステップS212）。この仮定は、アライメント処理S2は静止時の加速度信号ａB及び角速度信号ωBに基づいて行うことが前提であり、上記の仮定により、概略の初期姿勢角φ0，θ0，ψ0を、図４の中に記載したシンプルな式で簡単に求めることができる。
　初期姿勢角φ0，θ0，ψ0を算出した後、初期姿勢角θ0が未調整の場合は、第１の初期姿勢角調整ステップS22に進む。第１の初期姿勢角調整ステップS22は、初期姿勢角θ0を微調整するステップである。図５に示すように、まずステップS221に進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。座標変換行列Ｃ(G-B)は公知の行列であり、右辺のCφ，Cθ，Cψは各々cosφ，cosθ，cosψであり、Sφ，Sθ，Sψは各々sinφ，sinθ，sinψである。
Figure JPOXMLDOC01-appb-M000001
　ステップS221を行った後はステップS222に進み、加速度信号ａBのＸ軸成分a(x)から重力加速度ｇpBのＸ軸成分gp(x)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(x)を算出する。そして、補正加速度ah(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の速度vh(x)を推定する（ステップS223）。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。
　速度vh(x)を推定すると、規定時間における速度vh(x)の平均値を計算する（ステップS224）。そして、速度vh(x)の絶対値が一定以上に大きい場合はステップS225に進み、初期姿勢角θ0を直前の値から変更（増減）させる。初期姿勢角θ0を変更してステップS221～S224を繰り返し実行すると、やがて速度vh(x)の絶対値が一定以下（≒0）になるので、S226に進み、その時のθ0を調整後の初期姿勢角θ0とする。これでθ0の調整が終了する。
　初期姿勢角θ0の調整が終了した後、初期姿勢角φ0が未調整の場合は、図３に示すように、第２の初期姿勢角調整ステップS23に進む。第２の初期姿勢角調整ステップS23は、初期姿勢角φ0を微調整するステップである。図６に示すように、まずステップS231に進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。ステップS231は、先に説明したステップS221と同様の処理である。
　ステップS231を行った後はステップS232に進み、加速度信号ａBのＹ軸成分a(y)から重力加速度ｇpBのＹ軸成分gp(y)を差し引いて、重力加速度の影響をキャンセルした補正加速度ah(y)を算出する。そして、補正加速度ah(y)に静止状態推定フィルタＡs1を適用し、積分値であるＹ軸方向の速度vh(y)を推定する（ステップS233）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。
　速度vh(y)を推定すると、規定時間における速度vh(y)の平均値を計算する（ステップS234）。そして、速度vh(y)の絶対値が一定以上に大きい場合はステップS235に進み、初期姿勢角φ0を直前の値から変更（増減）させる。初期姿勢角φ0を変更してステップS231～S234を繰り返し実行すると、やがて速度vh(y)の絶対値が一定以下（≒0）になるので、S236に進み、その時のφ0を調整後の初期姿勢角φ0とする。これでφ0の調整が終了する。
　初期姿勢角φ0の調整が終了した後、初期姿勢角ψ0が未調整の場合は、図３に示すように、第３の初期姿勢角調整ステップS24に進む。第３の初期姿勢角調整ステップS24は、初期姿勢角ψ0を微調整するステップである。図７に示すように、まずステップS241に進み、地理座標系の地球角速度ωpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωpBに変換する。
　ステップS241を行った後はステップS242に進み、角速度信号ωBのＸ軸成分ω(x)から地球角速度ωpBのＸ軸成分ωp(x)を差し引いて、地球角速度の影響をキャンセルした補正角速度ωh(x)を算出する。そして、補正角速度ωh(x)に静止状態推定フィルタＡs1を適用し、積分値であるＸ軸方向の角度αh(x)を推定する（ステップS243）。この静止状態推定フィルタＡs1も、フィルタリング処理S1sで使用したのと同じものである。
　角度αh(x)を推定すると、規定時間における角度αh(x)の平均値を計算する（ステップS244）。そして、角度αh(y)の絶対値が一定以上に大きい場合はステップS245に進み、初期姿勢角ψ0を直前の値から変更（増減）させる。初期姿勢角ψ0を変更してステップS241～S244を繰り返し実行すると、やがて角度αh(x)の絶対値が一定以下（≒0）になるので、S246に進み、その時のψ0を調整後の初期姿勢角ψ0とする。これでψ0の調整が終了し、アライメント処理S2が終了する。
　アライメント処理S2が終了した後、静止状態で角速度・加速度バイアスωbB，ａbB未算出の場合、角速度・加速度バイアス算出処理S3を行う。角速度・加速度バイアス算出処理S3は、図８に示すように、角速度バイアスωbBが未算出の場合、まずステップS31に進み、地理座標系の地球角速度ωpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωpBに変換する。そして、ステップS32に進み、角速度信号ωBから地球角速度ωpBを差し引いて、角速度誤差ΔωBを算出する。
　角速度誤差ΔωBを算出すると、規定時間における角速度誤差ΔωBの平均値を計算し、その結果を角速度バイアスωbBとし（ステップS34）、これで角速度バイアスωbBの算出が終了する。
　角速度バイアスωbBの算出が終了した後、加速度バイアスａbBが未算出の場合はステップS35に進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。そして、ステップS36に進み、加速度信号ａBから重力加速度ωpBを差し引いて、加速度誤差ΔａBを算出する。
　加速度誤差ΔａBを算出すると、規定時間における加速度誤差ΔａBの平均値を計算し、その結果を加速度バイアスａbBとする（ステップS34）。これで加速度バイアスａbBの算出が終了し、角速度・加速度バイアス算出処理S3が終了する。
　角速度・加速度バイアス算出処理S3が終了した後、静止状態の時は、図２に示すように、フィルタリング処理S1sを経て、姿勢角算出処理S4sを行う。姿勢角算出処理S4sは、図９（ａ）に示すように、まずステップS41sに進み、地理座標系の地球角速度ωpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の地球角速度ωpBに変換する。そして、ステップS42sに進み、角速度信号ωBから地球角速度ωpBと角速度バイアスωbBを差し引いて、地球角速度等の誤差要因をキャンセルした補正角速度ωhBを算出する。
　補正角速度ωhBを算出すると、ステップS43sに進み、機体座標系の補正角速度ωhBを、式（２）に示す座標変換行列Ｒ(B-A)を用いて姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に変換する。座標変換行列Ｒ(B-A)は公知の行列である。
Figure JPOXMLDOC01-appb-M000002
　ステップS43sを行った後はステップS44sに進み、姿勢角速度φ（ドット），θ（ドット），ψ（ドット）にそれぞれ静止状態推定フィルタＡs1を適用し、積分値である姿勢角φ，θ，ψを推定する。この静止状態推定フィルタＡs1は、フィルタリング処理S1sで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新して姿勢角算出処理S4sが終了する（ステップS45s）。
　姿勢角算出処理S4sが終了した後、静止状態の時は、図２に示すように、位置移動量算出処理S5sを行う。位置移動量算出処理S5sは、図１０（ａ）に示すように、まずステップS51sに進み、地理座標系の重力加速度ｇpGを、式（１）に示す座標変換行列Ｃ(G-B)を用いて機体座標系の重力加速度ｇpBに変換する。そして、ステップS52sに進み、加速度信号ａBから重力加速度ｇpBと加速度バイアスａbBを差し引いて、重力加速度等の誤差要因をキャンセルした補正加速度ａhBを算出する。
　補正加速度ａhBを算出すると、ステップS53sに進み、機体座標系の補正加速度ａhBを、式（１）に示す座標変換行列Ｃ(G-B)を用いて地理座標系の加速度ａhGに変換する。
　ステップS53sを行った後はステップS54sに進み、加速度ａhGの各軸成分に静止状態推定フィルタＡs2を適用し、２階積分値である位置移動量ｒGを推定する。静止状態推定フィルタＡs2は、先の静止状態推定フィルタＡs1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで姿勢角算出処理S5sが終了する。
　角速度・加速度バイアス算出処理S3が終了した後、運動状態の時は、図２に示すようにフィルタリング処理S1uを経て、姿勢角算出処理S4uを行う。フィルタリング処理S1uは、上記のフィルタリング処理S1sとは異なり、角速度信号ωBに重畳しているノイズ成分を低減する処理を、運動状態推定フィルタＡu1を使用した推定によって行う。運動状態推定フィルタＡu1は、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。
　姿勢角算出処理S4uは、図９（ｂ）に示すように、ステップS41u～S45uを順に行う。この中のステップS41u～S43u，S45uは静止時のステップS41s～S43s，S45sと同じ内容であり、異なるのはステップS44u（下線部）である。
　ステップS44uでは、ステップS43uの処理結果である姿勢角速度φ（ドット），θ（ドット），ψ（ドット）に対して運動状態推定フィルタＡu1を適用し、積分値である姿勢角φ，θ，ψを推定する。この運動状態推定フィルタＡu1は、フィルタリング処理S1uで使用したのと同じものである。そして、座標変換行列Ｃ(G-B)を更新し、姿勢角算出処理S4uが終了する（ステップS45u）。
　姿勢角算出処理S4uが終了した後、運動状態の時は、図２に示すように、位置移動量算出処理S5uを行う。位置移動量算出処理S5uは、図１０（ｂ）に示すように、ステップS51u～S54uを順に行う。この中のステップS51u～S53uは静止時のステップS51s～S53sと同じ内容であり、異なるのはステップS54u（下線部）である。
　ステップS54uでは、ステップS53uの処理結果である加速度ａhGの各軸成分に対して運動状態推定フィルタＡu2を適用し、２階積分値である位置移動量ｒGを推定する。この運動状態推定フィルタＡu2は、先の運動状態推定フィルタＡu1と同様に、信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されたフィルタであり、具体的な態様については後で説明する。これで姿勢角算出処理S5uが終了する。
　このように、慣性計測装置１０は、移動体１２が静止している時に、フィルタリング処理S1s、アライメント処理S2、角速度・加速度バイアス算出処理S3を順番に実行し、その後、フィルタリング処理S1s又はS1u、姿勢角算出処理S4s又はS4u、位置移動量算出処理S5s又はS5uを順番に繰り返し実行することによって、角速度信号ωB及び加速度信号ａBに基づいて、移動体１２の姿勢角φ（ロール），θ（ピッチ），ψ（ヨー）及び位置移動量ｒGを導出する。
　次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs1と運動状態推定フィルタＡu1について説明する。
　静止状態推定フィルタＡs1は、静止時のアライメント処理S2、姿勢角算出処理S4sで１階積分を行う際に使用されるフィルタであり、静止時のフィルタリング処理S1sでも使用されている。静止状態推定フィルタＡs1には、推定を行うための演算式として次の式（３）、（４）が設定されている。
Figure JPOXMLDOC01-appb-M000003
Figure JPOXMLDOC01-appb-M000004
　式（３）、（４）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはａの積分値であり、例えばａを加速度とすればｖが速度になり、ａを角速度とすればｖが角度になる。そして、式（３）の中の４行４列の行列が、静止時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値のランダムウォーク、及び積分を行う際の、バイアス起因の積分値の誤差の増大を抑制するように設定されている。
　運動状態推定フィルタＡs1は、運動時の姿勢角算出処理S4uで１階積分を行う際に使用されるフィルタであり、運動時のフィルタリング処理S1uでも使用されている。運動状態推定フィルタＡu1には、推定を行うための演算式として、上記の式（４）と次の式（５）とが設定されている。
Figure JPOXMLDOC01-appb-M000005
　式（４）、（５）では、状態変数をｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（５）の中の４行４列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。
　式（３）と式（５）を比較して分かるように、各行列の内容が異なるのは第２行である。静止状態推定フィルタＡs1の第２行は、図１１（ａ）に示すように、タイミングk-1の後のしばらくの間（例えば、タイミンブk-1とタイミングkの中間点までの間）、積分値ｖが一定に保持されるという考え方で設定されている。一方、運動状態推定フィルタＡu1の第２行は、図１１（ｂ）に示すように、タイミングk-1からタイミングkまでの間、積分値ｖが一定の傾きで変化するという考え方で設定されている。この第２行の設定方法の違いにより、静止状態推定フィルタＡs1は静止状態に適した推定を行うことができ、運動状態推定フィルタＡu1は、運動状態に適した推定を行うことができる。
　発明者は、静止状態推定フィルタＡs1の動作を確認するため、シミュレーションによる試験を行った。第１の試験は、図１２（ａ）に示すように、信号ａを白色雑音（統計的に制御されたノイズ発生手段が発生させた平均値０、分散１の信号）とし、静止状態推定フィルタＡs1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１２（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。
　第１の試験の結果は、図１３（ａ）、（ｂ）に示す通りである。静止状態推定フィルタＡs1の推定ａを見ると、白色雑音（＝信号ａ）が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、時間とともにゼロ点がランダムにドリフトしており、上述した積分を行う際の、ノイズ起因の積分値のランダムウォークが発生していることが分かる。これに対して、静止状態推定フィルタＡs1の推定ｖを見ると、ゼロ点がほとんどドリフトせず、ランダムウォークが効果的に抑制されている。
　また、静止状態推定フィルタＡs1の動作を確認するため、第２の試験を行った。第２の試験は、図１４（ａ）に示すように、信号ａをバイアス（信号レベル１）に上記の白色雑音を重畳させたものとし、静止状態推定フィルタＡs1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１４（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。
　第２の試験の結果は、図１５（ａ）、（ｂ）に示す通りである。静止状態推定フィルタＡs1の推定ａを見ると、信号ａに含まれる白色雑音が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、時間とともにゼロ点が右肩上がりにドリフトしており、上述した積分を行う際の、バイアス起因の積分値の誤差の増大が発生していることが分かる。これに対して、静止状態推定フィルタＡs1の推定ｖを見ると、ゼロ点がほとんどドリフトせず、バイアス起因の誤差の増大が効果的に抑制されている。
　さらに、運動状態静止状態推定フィルタＡu1の動作を確認するため、第３の試験を行った。第３の試験は、図１６（ａ）に示すように、信号ａを理想正弦波（振幅１、周波数４Hz）に上記の白色雑音を重畳させたものとし、運動状態推定フィルタＡu1を適用して信号ａと積分値ｖの推定を行った。また、比較例として、図１６（ｂ）に示すように、一般的な積分器２８を用いて信号ａの積分値ｖを計算した。
　第３の試験の結果は、図１７（ａ）、（ｂ）に示す通りである。運動状態推定フィルタＡu1の推定ａを見ると、信号ａに含まれる白色雑音が低減していることが分かる。また、積分器２８の計算ｖ（比較例）を見ると、ノイズ起因による積分値の誤差が見られるが、運動状態推定フィルタＡu1の推定ｖを見ると、この積分によるノイズ起因の誤差が効果的に抑制されている。
　次に、４つの状態推定フィルタＡs1，Ａs2，Ａu1，Ａu2の中の、静止状態推定フィルタＡs2と運動状態推定フィルタＡu2について説明する。
　静止状態推定フィルタＡs2は、静止時の位置移動量算出処理S5sで２階積分を行う際に使用されるフィルタである。静止状態推定フィルタＡs2には、推定を行うための演算式として次の式（６）、（７）が設定されている。
Figure JPOXMLDOC01-appb-M000006
Figure JPOXMLDOC01-appb-M000007
　式（６）、（７）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。ｖはaの積分値であり、ｒはｖの積分値である。つまり、ａを加速度とすれば、ｖが速度でｒが位置移動量になる。そして、式（６）の中の６行６列の行列が、静止時の信号に重畳しているノイズを低減し、且つノイズ起因の積分値のランダムウォーク及びバイアス起因の積分値の誤差の増大を抑制するように設定されている。
　運動状態推定フィルタＡu2は、運動時の位置移動量算出S5uで２階積分を行う際に使用されるフィルタである。運動状態推定フィルタＡu2には、推定を行うための演算式として、上記の式（７）と次の式（８）とが設定されている。
Figure JPOXMLDOC01-appb-M000008
　式（７）、（８）では、状態変数をｒ(k)、ｒ(k-1)、ｖ(k)、ｖ(k-1)、ａ(k)及びａ(k-1)とし、出力Ｙ(k)は、a(k)に観測雑音Ｗ(k)を加算したものとしている。そして、式（８）の中の６行６列の行列は、運動時の信号に重畳しているノイズを低減し、且つ積分を行う際の、ノイズ起因の積分値の誤差を抑制するように設定されている。
　式（６）と式（８）を比較して分かるように、各行列の内容が異なるのは第２行と第４行である。第４行の違いは、図１１（ａ）、（ｂ）を用いて先に説明した通りである。すなわち、静止状態推定フィルタＡs2の第４行は、タイミングk-1の後のしばらくの間、積分値ｖが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第４行は、図１１（ｂ）に示すように、タイミングk-1からタイミングkまでの間、積分値ｖが一定の傾きで変化するという考え方で設定されている。
　第２行の考え方も、第４行の考え方と同じである。すなわち、静止状態推定フィルタＡs2の第２行は、タイミングk-1の後のしばらくの間、積分値ｒが一定に保持されるという考え方で設定され、運動状態推定フィルタＡu2の第２行は、タイミングk-1からタイミングkまでの間、積分値ｒが一定の傾きで変化するという考え方で設定されている。
　この第２行及び第４行の設定方法の違いにより、静止状態推定フィルタＡs2は、静止状態に適した推定を行うことができ、運動状態推定フィルタＡu2は、運動状態に適した推定を行うことができる。
　以上説明したように、この実施形態の慣性計測方法及びその慣性計測プログラムがインストールされた慣性計測装置１０は、静止している時にアライメント処理S2と角速度・加速度バイアス算出処理S3を行い、姿勢角及び位置移動量を算出する時に、重力加速度及び地球角速度の成分をキャンセルする処理を行うので、重力加速度及び地球角速度に起因する誤差を小さくすることができる。
　さらに、各処理で推定を行うとき、静止状態推フィルタＡs1，Ａs2と運動状態推定フィルタＡu1，Ａs2とを使い分けている。従って、慣性計測で問題になる（ａ）信号に重畳しているノイズによる誤差、（ｂ）ノイズ起因の積分値のランダムウォーク、（ｃ）バイアス起因の積分値の誤差の増大、（ｄ）ノイズ起因の積分値の誤差等に対し、静止時は静止状態推定フィルタＡs1，Ａs2を使用した推定を行って（ａ）、（ｂ）、（ｃ）を効果的に抑制し、運動時は運動状態推定フィルタＡu1，Ａu2を使用した推定を行って（ａ）、（ｄ）を効果的に抑制することができ、姿勢角φ，θ，ψ及び位置移動量ｒGを高精度に計測することができる。
　慣性計測装置１０は、汎用のコンピュータで容易に構成することができ、しかも、特許文献１の移動体姿勢角検出装置のように、ランダムウォークを抑制するために第二の検出手段を付設する必要もない。従って、装置を非常にシンプルに構成することができる。
　次に、以下、本発明の慣性計測装置及び慣性計測方法の第二の実施形態について、図１８～図２１に基づいて説明する。この実施形態の慣性計測装置３０（及び慣性計測方法）は、上記の慣性計測装置１０（及び慣性計測方法）の構成の一部を変更したものであり、ここでは、慣性計測装置１０と同様の構成は同一の符号を付して説明を省略する。
　第一の実施形態の慣性計測装置１０は、必要なセンサ素子の種類が少ない（角速度センサ１４及び加速度センサ１６の２種類だけ）という特徴があるが、上記のアライメントを効果的に行うには、地球角速度を精度よく検出できる高性能な角速度センサ１４を使用する必要があり、装置が高価になる可能性がある。これに対して、第二の実施形態の慣性計測装置３０は、図１８に示すように、初期姿勢角ψ0の算出に電子コンパス３２の方位信号Ｈを利用することにより、安価な角速度センサ１４が使用可能になり、装置全体のコストを抑えることができるという特徴がある。
　慣性計測装置３０が行う慣性計測方法の全体の流れは、慣性計測装置１０の流れ（図２）と同様であるが、アライメント処理S2、加速度・角速度バイアス算出処理S3及び姿勢角速度算出処理S4s，S4uの内容に少し異なる部分があるので、異なる部分について順番に説明する。
　慣性計測装置３０が行うアライメント処理S2は、図１９に示すように、３つの初期姿勢角φ0，θ0，ψ0のうち、φ0，θ0についての処理は慣性計測装置１０と同じであるが、初期姿勢角ψ0は、ステップS62に示すように、電子コンパス３２の方位信号Ｈに基づいて算出し、調整は行わない。従って、初期姿勢角算出ステップS61では、図２０に示すように、初期姿勢角φ0，θ0だけを算出し(ステップS611)、これを調整する（ステップS612）という処理になっている。安価な角速度センサは、微小な地球角速度を高精度に検出することが難しいが、電子コンパス３２の方位信号Ｈを利用することによって、初期姿勢角ψ0を算出することができ、アライメント処理を行うことができる。地球角速度よりも高速の角速度の検出は、安価な角速度センサでも可能なので、角速度センサ１４のコストダウンを図ることができる。
　その他、角速度・加速度バイアス算出処理S3では、図２１のステップS71，S72に示すように、角速度誤差ΔωBを算出する時、地球角速度ωpBをゼロと仮定して行う。同様に、姿勢角算出処理S4s，S4uでは、図２２のステップS81s，S82sとステップS81u，S82uに示すように、角速度誤差ΔωBを算出する時、地球角速度ωpBをゼロと仮定して行う。
　慣性計測装置３０及びその慣性計測方法によれば、慣性計測装置１０の場合と同様の効果を得ることができ、必要なセンサの種類は増えるものの、装置全体のコストダウンを図ることができる。
　なお、本発明の慣性計測方法と慣性計測装置と慣性計測装置及び慣性計測プログラムは、上記実施形態に限定されるものではない。例えば、静止状態推定フィルタの設定は、上記の静止状態推定フィルタＡs1（式（３）と（４））やＡs2（式（６）と（７））の設定に限定されるものではなく、上述した効果が得られる範囲で、式の内容は変更してもよい。また、上記実施形態では、静止状態推定フィルタＡs1を複数の処理で兼用しているが、個々の処理内容に応じて設定を変えたものを使用してもよい。
　運動状態推定フィルタの設定は、上記の運動状態推定フィルタＡu1（式（４）と（５））やＡu2（式（７）と（８））の設定に限定されるものではなく、上述した効果が得られる範囲で、式の内容は変更してもよい。また、上記実施形態では、運動状態推定フィルタＡu1を複数の処理で兼用しているが、個々の処理内容に応じて設定を変えたものを使用してもよい。
　また、本発明の慣性計測方法及び慣性計測装置は、空中を飛行するマルチコプタ等の慣性計測に好適であるが、マルチコプタ以外の移動体にも適用できることは言うまでもなく、上記の優れた効果を得ることができる。
１０，３０　慣性計測装置
１２　移動体
１２ａ　機体
１４　角速度センサ
１６　加速度センサ
１８　フィルタリング部
２０　アライメント部
２２　加速度・角速度バイアス算出部
２４　姿勢角算出部
２６　位置移動量算出部
２８　積分器
Ａs1，Ａs2　静止状態推定フィルタ
Ａu1，Ａu2　運動状態推定フィルタ
Ｓ1s，Ｓ1u　フィルタリング処理
Ｓ2　アライメント処理
Ｓ3　角速度・加速度バイアス算出処理
Ｓ4s，Ｓ4u　姿勢角算出処理
Ｓ5s，Ｓ5u　位置移動量算出処理

Claims (5)

  1. 　移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測方法であって、
    　前記角速度信号に重畳しているノイズ成分を低減するフィルタリング処理と、
    　前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整するアライメント処理と、
    　前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する角速度・加速度バイアス算出処理と、
    　前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する姿勢角算出処理と、
    　前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する位置移動量算出処理とで構成され、
    　前記移動体が静止している時に前記フィルタリング処理、前記アライメント処理及び前記角速度・加速度バイアス算出処理を順番に実行し、その後、前記フィルタリング処理、前記姿勢角算出処理及び前記位置移動量算出処理を順番に繰り返し実行し、
    　前記移動体が静止している時の前記フィルタリング処理、前記アライメント処理、前記移動体が静止している時に前記姿勢角算出処理、及び前記移動体が静止している時に前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行い、
    　前記移動体が運動している時の前記フィルタリング処理、前記移動体が運動している時の前記姿勢角算出処理、及び前記移動体が運動している時の前記位置移動量算出処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行うことを特徴とする慣性計測方法。
  2. 　前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記アライメント処理では、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出し、前記角速度・加速度バイアス算出処理では、地球角速度をゼロと仮定して前記角速度バイアスを算出し、前記姿勢角算出処理では、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する請求項１記載の慣性計測方法
  3. 　移動体の機体に取り付けた角速度センサが出力する角速度信号及び加速度センサが出力する加速度信号に基づいて、前記移動体の姿勢角及び位置移動量を導出する慣性計測装置であって、
    　前記角速度信号に重畳しているノイズ成分を低減する処理を行うフィルタリング部と、
    　前記加速度信号及び前記角速度信号から概略の初期姿勢角を算出した後、前記加速度信号から重力加速度を差し引いた加速度を積分して求まる速度の絶対値が一定以下になり、且つ前記角速度信号から地球角速度を差し引いた角速度を積分して求まる角度の絶対値が一定以下の値になるように、前記初期姿勢角を微調整する処理を行うアライメント部と、
    　前記角速度信号から地球角速度を差し引いた角速度バイアスを算出するとともに、前記加速度信号から重力加速度を差し引いた加速度バイアスを算出する処理を行う角速度・加速度バイアス算出部と、
    　前記角速度信号から地球角速度及び前記角速度バイアスを差し引いた角速度を算出し、これを積分して姿勢角を算出する処理を行う姿勢角算出部と、
    　前記加速度信号から重力加速度及び前記加速度バイアスを差し引いた加速度を算出し、これを２階積分して位置移動量を算出する処理を行う位置移動量算出部とで構成され、
    　前記移動体が静止している時に前記フィルタリング部、前記アライメント部及び前記角速度・加速度バイアス算出部が前記の各処理を順番に実行し、その後、前記フィルタリング部、前記姿勢角算出部及び前記位置移動量算出部が前記の各処理を順番に繰り返し実行し、
    　前記移動体が静止している時に前記フィルタリング部が行う処理、前記アライメント部が行う処理、前記移動体が静止している時に前記姿勢角算出部が行う処理、及び前記移動体が静止している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値のランダムウォークを抑制するとともに、バイアスに起因する積分値の誤差の増大を抑制する静止状態推定フィルタを使用した推定により行われ、
    　前記移動体が運動している時に前記フィルタリング部が行う処理、前記移動体が運動している時に前記姿勢角算出部が行う処理、及び前記移動体が運動している時に前記位置移動量算出部が行う処理は、信号に重畳しているノイズを低減し、且つノイズに起因する積分値の誤差を抑制する運動状態推定フィルタを使用した推定により行われることを特徴とする慣性計測装置。
  4. 　前記アライメント部は、前記移動体の機体に取り付けた電子コンパスが出力する方位信号を取得し、前記加速度信号、前記角速度信号及び前記方位信号から概略の初期姿勢角を算出する処理を行い、前記角速度・加速度バイアス算出部は、地球角速度をゼロと仮定して前記角速度バイアスを算出する処理を行い、前記姿勢角算出部は、地球角速度をゼロと仮定して前記角速度を算出し、これを積分して姿勢角を算出する処理を行う請求項３記載の慣性計測装置。
  5. 　請求項１又は２記載の慣性計測方法を実行させるための前記各処理の実行用プログラムにより構成された慣性計測プログラム。
PCT/JP2018/002599 2017-01-27 2018-01-27 慣性計測方法と慣性計測装置及び慣性計測プログラム WO2018139621A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2018564675A JP6712037B2 (ja) 2017-01-27 2018-01-27 慣性計測方法と慣性計測装置及び慣性計測プログラム
US16/481,452 US10816337B2 (en) 2017-01-27 2018-01-27 Inertial measurement method, inertial measurement apparatus, and inertial measurement program

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2017012961 2017-01-27
JP2017-012961 2017-06-16

Publications (1)

Publication Number Publication Date
WO2018139621A1 true WO2018139621A1 (ja) 2018-08-02

Family

ID=62979562

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2018/002599 WO2018139621A1 (ja) 2017-01-27 2018-01-27 慣性計測方法と慣性計測装置及び慣性計測プログラム

Country Status (3)

Country Link
US (1) US10816337B2 (ja)
JP (1) JP6712037B2 (ja)
WO (1) WO2018139621A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPWO2018230316A1 (ja) * 2017-06-12 2020-04-02 富士フイルム株式会社 ブレ検出装置、撮像装置、レンズ装置、撮像装置本体、ブレ検出方法及びブレ検出プログラム

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11747144B2 (en) * 2017-03-29 2023-09-05 Agency For Science, Technology And Research Real time robust localization via visual inertial odometry
IL253770B2 (en) * 2017-07-31 2023-02-01 Israel Aerospace Ind Ltd Determination of azimuth during flight
US11835544B2 (en) * 2018-08-27 2023-12-05 Sony Semiconductor Solutions Corporation Wind speed measuring device and wind speed measuring method
CN114964230B (zh) * 2022-05-12 2023-11-03 北京自动化控制设备研究所 一种车载组合导航陀螺漂移修正方法
CN114889843B (zh) * 2022-05-17 2024-06-07 中国工程物理研究院总体工程研究所 一种离心机轴向与切向过载输出高精度测算方法
CN116945195B (zh) * 2023-09-19 2024-01-12 成都飞机工业（集团）有限责任公司 全向测量设备系统装置、配准方法、电子设备和存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000292170A (ja) * 1999-04-13 2000-10-20 Nec Corp 移動体姿勢角検出装置
JP2001153680A (ja) * 1999-11-26 2001-06-08 Japan Aviation Electronics Industry Ltd 慣性装置
JP2013178230A (ja) * 2012-02-02 2013-09-09 Asahi Kasei Electronics Co Ltd 物理量計測装置及び物理量計測方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
ZA957639B (en) * 1994-10-24 1996-05-24 Caterpillar Inc System and method for precisely determining an operating point for an autonomous vehicle
US6473713B1 (en) * 1999-09-20 2002-10-29 American Gnc Corporation Processing method for motion measurement
AU2003302144A1 (en) * 2002-11-15 2004-06-15 The Regents Of The University Of California Dynamically amplified dual mass mems gyroscope
US7860651B2 (en) * 2005-08-30 2010-12-28 Honeywell International Inc. Enhanced inertial system performance
JP5742450B2 (ja) * 2011-05-10 2015-07-01 セイコーエプソン株式会社 位置算出方法及び位置算出装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2000292170A (ja) * 1999-04-13 2000-10-20 Nec Corp 移動体姿勢角検出装置
JP2001153680A (ja) * 1999-11-26 2001-06-08 Japan Aviation Electronics Industry Ltd 慣性装置
JP2013178230A (ja) * 2012-02-02 2013-09-09 Asahi Kasei Electronics Co Ltd 物理量計測装置及び物理量計測方法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPWO2018230316A1 (ja) * 2017-06-12 2020-04-02 富士フイルム株式会社 ブレ検出装置、撮像装置、レンズ装置、撮像装置本体、ブレ検出方法及びブレ検出プログラム

Also Published As

Publication number Publication date
US20190360802A1 (en) 2019-11-28
JP6712037B2 (ja) 2020-06-17
JPWO2018139621A1 (ja) 2019-11-14
US10816337B2 (en) 2020-10-27

Similar Documents

Publication Publication Date Title
WO2018139621A1 (ja) 慣性計測方法と慣性計測装置及び慣性計測プログラム
Ahmed et al. Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors
CN106679649B (zh) 一种手部运动追踪系统及追踪方法
JP5328252B2 (ja) ナビゲーションシステムの位置検出装置および位置検出方法
EP1653194B1 (en) Azimuth/attitude detecting sensor
EP2951529B1 (en) Inertial device, method, and program
JP6922641B2 (ja) 角速度導出装置および角速度導出方法
JP6409346B2 (ja) 移動距離推定装置
CN104880189B (zh) 一种动中通天线低成本跟踪抗干扰方法
JP6201762B2 (ja) 速度推定装置
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN106153069B (zh) 自主导航系统中的姿态修正装置和方法
JP6413946B2 (ja) 測位装置
CN111896007A (zh) 一种补偿足地冲击的四足机器人姿态解算方法
Gao et al. An integrated land vehicle navigation system based on context awareness
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
US10466054B2 (en) Method and system for estimating relative angle between headings
JP4149913B2 (ja) 慣性オドメータ補正による統合慣性ｖｍｓ航法
CN112985384A (zh) 一种抗干扰磁航向角优化系统
CN110567456B (zh) 基于抗差卡尔曼滤波的bds/ins组合列车定位方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
Singh et al. Attitude estimation for dynamic legged locomotion using range and inertial sensors
Milanchian et al. Magnetic calibration of three-axis strapdown magnetometers for applications in MEMS attitude-heading reference systems
JP6981459B2 (ja) センサ誤差補正装置
JP6859917B2 (ja) 角速度導出装置および角速度導出方法

Legal Events

Date Code Title Description
ENP Entry into the national phase

Ref document number: 2018564675

Country of ref document: JP

Kind code of ref document: A

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18744093

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 18744093

Country of ref document: EP

Kind code of ref document: A1