JP7391616B2 - Mobile body pose estimation device - Google Patents

Description

The present invention relates to a device for estimating the posture of a predetermined part of a moving body.

Conventionally, as this type of technology, an inertial navigation method using inertial sensors including an acceleration sensor and an angular speed reading sensor is generally known.

Furthermore, for mobile robots, for example, as seen in Patent Document 1, a method has been proposed for estimating the posture of the mobile robot's body using a method that incorporates the desired gait of the mobile robot into an inertial navigation method.

Patent No. 4181113

BACKGROUND ART Mobile bodies that carry people and luggage, and mobile bodies that guide and guide people in public facilities, etc., need to move flexibly while avoiding various obstacles. For this reason, such a moving body is susceptible to changes in its motion state, and it is difficult to set a target motion in advance.

In addition, with conventional inertial navigation methods, when the motion acceleration of a part whose attitude is to be estimated changes in response to changes in the motion state of a moving body, it is difficult to estimate the attitude of the part stably and accurately. . Furthermore, the technique disclosed in Patent Document 1 cannot be applied to a moving object for which it is difficult to set a target motion in advance.

The present invention has been made in view of this background, and provides a posture estimation device that is capable of stably and accurately estimating the posture of a predetermined part of a moving object, even if the motion state is likely to change. The purpose is to provide.

In order to achieve the above object, the mobile body posture estimation device of the present invention is a device for estimating the posture of a predetermined part of a mobile body, comprising:
an angular velocity estimation unit that generates an estimated value of the angular velocity of the predetermined portion from the output of an angular velocity sensor mounted on the predetermined portion;
a translational acceleration estimation unit that generates an estimated value of the translational acceleration at the mounting position of the acceleration sensor from the output of the acceleration sensor mounted at the predetermined portion, the estimated value including a gravitational acceleration component;
The estimated value of the motion acceleration generated at the mounting position of the acceleration sensor due to the motion of the predetermined region is calculated by calculating the motion acceleration that has a correlation with the motion acceleration of the predetermined region without using a target value regarding the motion of the predetermined region. a motion acceleration estimation unit that generates from observed values of body motion state quantities;
a posture estimation unit that generates an estimated value of the posture of the predetermined part from the estimated value of the angular velocity, the estimated value of the translational acceleration, and the estimated value of the motion acceleration;
The motion acceleration estimation unit is configured to generate an estimated value of motion acceleration including translational motion acceleration and centripetal acceleration generated at the mounting position of the acceleration sensor,
The posture estimating unit includes a reference posture estimating unit that estimates a reference posture of the predetermined part based on the difference between the estimated value of the translational acceleration and the estimated value of the motion acceleration; The difference between the latest orientation estimate and the reference orientation estimate is estimated as an error in the orientation estimate.
a feedback control unit that generates a feedback operation amount according to a feedback control law so that the error approaches zero at a predetermined period according to the estimated value of the error; an integral processing unit that generates an estimated value of the posture of the predetermined part by integrating an input obtained by correcting the estimated value,
The observed value of the motion state quantity is a motion state quantity other than the translational acceleration detected by the acceleration sensor, and is an observed value of the motion state quantity observed in accordance with the motion of the moving object in the predetermined period. characterized by something.

According to the present invention, although the estimated value of the translational acceleration based on the output of the acceleration sensor includes a component of motion acceleration due to the motion of the moving body in addition to a component of gravitational acceleration, the motion acceleration is , is estimated by the motion acceleration estimation unit from the observed value of the motion state quantity (motion state quantity other than the translational acceleration detected by the acceleration sensor) of the moving body . Further, the motion acceleration is generated without using a target value regarding the motion of the predetermined portion.

Note that the "observed value" of the motion state quantity is determined from the detected value of the motion state quantity by an appropriate sensor or the detected value of one or more other state quantities that have a certain correlation with the motion state quantity. It means an estimated value based on correlation.

The difference between the estimated value of the translational acceleration and the estimated value of the motion acceleration mainly depends on the gravitational acceleration. Therefore, by estimating the reference posture of the predetermined part based on the difference, the reference posture is estimated so that the direction of the gravitational acceleration included in the estimated value of the translational acceleration is in the correct direction (vertical direction). value can be determined. Therefore, by estimating the difference between the latest posture estimate of the generated predetermined part and the reference posture estimate as the error of the posture estimate, the error can be estimated with high reliability. becomes possible. Further, by integrating an input obtained by correcting the estimated angular velocity based on the output of the angular velocity sensor using a feedback operation amount generated by a feedback control law so as to bring this error close to zero, the error approaches zero. Thus, the posture of a predetermined portion of the moving object is estimated.

As a result, according to the present invention, it is possible to stably and accurately estimate the posture of a predetermined portion of a moving object, even if the moving object is likely to change its motion state.

Furthermore , since the translational motion acceleration and the centripetal acceleration are estimated as the motion acceleration, it is possible to improve the accuracy of the estimated value of the posture of a predetermined part of the movable body, regardless of the motion state of the movable body. can.

FIG. 1 is a diagram showing a schematic configuration of a moving body in a first embodiment of the present invention. The block diagram which shows the function of the arithmetic processing device with which the moving object was equipped. The figure which shows the typical structure of the moving body in 2nd Embodiment of this invention.

[First embodiment]
A first embodiment of the present invention will be described with reference to FIGS. 1 and 2. Referring to FIG. 1, a moving body 1A in this embodiment is, for example, an inverted pendulum type moving body. Although detailed illustrations are omitted, this moving body 1A includes a wheel-shaped or spherical moving unit 2 that rotates on a floor surface by the driving force of an actuator (not shown), and a moving unit 2. The base body 3 is tiltably supported by the base body 3, and the movement of the moving unit 2 is controlled so as to maintain the balance of the center of gravity G1 of the base body 3, like an inverted pendulum. Note that, when an object such as a person or luggage is mounted on the base body 3, the center of gravity G1 is the center of gravity including the object.

Such a moving body 1A may be a known one. For example, a structure having a wheel-shaped moving motion part as seen in Japanese Patent Application Publication No. 2015-093651 may be adopted as the moving body 1A. Further, for example, a structure having a spherical moving part as seen in Japanese Patent Application Laid-Open No. 4-201793 may be employed as the moving body 1A. Furthermore, for example, a structure proposed by the applicant in Japanese Patent Application Laid-Open No. 2019-166863 (a moving body in which a spherical moving unit is driven via a plurality of rollers in contact with the moving unit) is proposed. , it can also be employed as the mobile body 1A.

The base body 3 of the moving body 1A includes an inertial sensor 8 including an acceleration sensor 6 capable of detecting three-axis (three-dimensional) acceleration and an angular velocity sensor 7 capable of detecting three-axis (three-dimensional) angular velocity. An arithmetic processing device 10 that can perform various arithmetic processing is installed.

The arithmetic processing unit 10 is composed of an electronic circuit unit including, for example, a microcomputer or a processor, a memory (RAM, ROM, etc.), an interface circuit, etc. Detection outputs of various sensors are input.

In this embodiment, the arithmetic processing device 10 includes a function as an attitude estimation device as a function realized by the implemented hardware configuration and program (software), and by this function, a predetermined It is possible to successively estimate the actual posture of the base body 3 as a part. Here, the "attitude" of the base body 3 more specifically means the inclined posture (tilt) of the base body 3 in a direction around a horizontal or approximately horizontal axis. The tilted posture is expressed by, for example, Euler angles. Hereinafter, the Euler angle representing the attitude of the base body 3 will be referred to as an attitude angle.

Next, the process of estimating the posture of the base body 3 executed by the arithmetic processing device 10 will be specifically described with reference to FIG. Here, as a supplement regarding the estimation process, in this embodiment, as shown in FIG. The moving environment of the moving body 1A is defined as the X-axis of the moving body 1A in the front-rear direction (horizontal direction of the paper in FIG. 1), and the Y-axis of the moving body 1A in the left-right direction (direction perpendicular to the paper in FIG. 1). A three-axis orthogonal coordinate system set (defined) is used as a coordinate system (hereinafter referred to as a reference coordinate system) that expresses motion state quantities such as the attitude of the base body 3. In this case, the direction around the X axis, the direction around the Y axis, and the direction around the Z axis of the reference coordinate system correspond to the roll direction, pitch direction, and yaw direction of the moving body 1A, respectively.

Hereinafter, the X-axis, Y-axis, and Z-axis mean the coordinate axes of the reference coordinate system unless otherwise specified. In addition, in the following explanation, the component in the X-axis direction (or around the X-axis), the component in the Y-axis direction (or around the Y-axis), and the component in the Z-axis direction (or around the When representing a component in the circumferential direction), subscripts "_x", "_y", and "_z" are added to the reference code of the state quantity. For example, a component of the state quantity Q in the X-axis direction (or around the X-axis) is expressed as Q_x. Furthermore, a set of two specific coordinate components of an arbitrary state quantity Q, for example, a set of a component in the X-axis direction and a component in the Y-axis direction (or a set of a component in the direction around the X-axis and a component in the direction around the Y-axis). When expressed, the set of coordinate components is expressed as Q_x,y. Furthermore, in the following explanation, the estimated value of an arbitrary state quantity Q may be simply expressed as a state quantity estimated value Q.

Referring to FIG. 2, the arithmetic processing device 10 has an angular velocity estimation unit 11 that estimates the angular velocity of the base 3 as seen in the reference coordinate system, and an acceleration sensor 6 for the base 3 as functions for estimating the orientation of the base 3. A translational acceleration estimating unit 12 that estimates the translational acceleration (translational acceleration seen in the reference coordinate system) at the mounting position (hereinafter simply referred to as the sensor position) of A motion acceleration estimating unit 13 that estimates the motion acceleration (motion acceleration seen in a reference coordinate system) that is generated acceleration, and a posture that represents the posture of the base body 3 using the respective estimated values of the angular velocity, translational acceleration, and motion acceleration. It includes a posture estimating section 14 that estimates the angle, and processes of each of these functional sections are sequentially executed at a predetermined calculation processing cycle. Note that the attitude angle estimated by the attitude estimation unit 14 is a set of an attitude angle in a direction around the X axis (roll direction) and an attitude angle in a direction around the Y axis (pitch direction).

The detection output of the angular velocity sensor 7 is input to the angular velocity estimation section 11 . The angular velocity estimating unit 11 then converts the detected angular velocity value (detected value of the angular velocity seen in the sensor coordinate system set for the angular velocity sensor 7) indicated by the input detection output to the angular velocity detected value that has already been generated by the attitude estimation unit 14. By performing coordinate transformation using the latest value of the estimated attitude angle θb_x,y of the base body 3 (estimated value in the previous calculation processing cycle, hereinafter referred to as the previous estimated attitude angle θb_p_x,y), An estimated angular velocity value ωb (a set of ωb_x, ωb_y, ωb_z) of the viewed base body 3 is generated and output.

The detection output of the acceleration sensor 6 is input to the translational acceleration estimation section 12 . The translational acceleration estimating unit 12 then uses the posture estimating unit 14 to detect the translational acceleration detected value (the detected value of the translational acceleration seen in the sensor coordinate system set for the acceleration sensor 6) indicated by the input detection output. By performing coordinate transformation using the previously generated previously estimated attitude angle value θb_p_x,y of the base body 3, the estimated translational acceleration value Ab (Ab_x, Ab_y, Ab_z) of the base body 3 at the sensor position as seen in the reference coordinate system is calculated. ) is generated and output. Note that since the translational acceleration detected by the acceleration sensor 6 includes a gravitational acceleration component, the above-mentioned translational acceleration estimated value Ab also includes a gravitational acceleration component.

The observed value (detected value or estimated value) of the motion state quantity of the moving body 1A that is related to (has a correlation with) the motion acceleration of the base body 3 is input to the motion acceleration estimation unit 13 . The observed value of the motion state quantity does not include the detected value of the translational acceleration detected by the acceleration sensor 6 or the estimated translational acceleration value Ab generated by the translational acceleration estimation unit 12.

Here, in this embodiment, the motion acceleration estimated by the motion acceleration estimation unit 13 is, in detail, the translation in the horizontal direction (X-axis direction and Y-axis direction) that occurs at the sensor position of the base body 3 due to the movement of the moving body 1A. There are two types of motion acceleration: motion acceleration and centripetal acceleration in the horizontal direction (X-axis direction and Y-axis direction) generated in the base body 3 due to the turning motion of the moving body 1A.

In order to estimate these translational acceleration and centripetal acceleration, the motion acceleration estimator 13 receives the estimated angular velocity ωb (a set of ωb_x, ωb_y, ωb_z) generated by the angular velocity estimator 11, and the moving object. Estimated rotational angular velocities ωw_x,y in the roll direction (direction around the X-axis) and pitch direction (direction around the Y-axis) of the wheel-shaped or spherical moving unit 2 of 1A are input.

In this case, if the moving body 1A is a moving body having a wheel-shaped moving unit as seen in, for example, JP2015-093651A, the rotational angular velocity of the moving unit 2 is determined by the rotational angular velocity of the moving unit 2. It can be estimated from the observed value of the rotational angular velocity of the output shaft of each motor (not shown) that drives 2. In this case, the rotational angular velocity of the output shaft of each motor can be detected using, for example, a rotary encoder, a resolver, or the like.

Further, when the moving body 1A is a moving body having a spherical moving movement part as seen in, for example, JP-A-4-201793 (or a moving body with a structure as seen in JP-A-2019-166863) , an estimated rotational angular velocity value ωw_x of the moving motion unit 2 from the observed value of the rotational angular velocity of the output shaft of each motor (not shown) that drives the spherical moving motion unit 2 and the observed value of the attitude angle of the base body 3. y can be estimated. In this case, the rotational angular velocity of the output shaft of each motor can be detected using, for example, a rotary encoder, a resolver, or the like. Further, as the observed value of the attitude angle of the base body 3, the previous attitude angle estimate θb_p_x,y obtained by the attitude estimation unit 14 can be used.

In this embodiment, as described above, the estimated angular velocity ωb of the base body 3 and the estimated rotational angular velocity ωw_x,y of the wheel-shaped or spherical moving unit 2 are used as the observed values of the motion state quantity by the motion acceleration estimation unit. 13.

In order to estimate the centripetal acceleration of the motion acceleration, the motion acceleration estimating section 13 uses the estimated angular velocity ωb_x,y of the base body 3 in the roll direction and pitch direction among the input motion state quantities, and the moving motion section. The estimated translational velocity Vbg_x,y of the center of gravity G1 of the base body 3 is calculated from the rotational angular velocity estimated value ωw_x,y of 2 by calculating the kinematic model shown in the following equations (1a) and (1b). Note that in this specification, "*" is an arithmetic symbol meaning multiplication.

Vbg_x=r1*(ωw_y+ωb_y)+h1*ωb_y...(1a)
Vbg_y=-r1*(ωw_x+ωb_x)-h1*ωb_x...(1b)

Here, with reference to FIG. 1, in equations (1a) and (1b), r1 is the radius of rotation of the moving operation section 2, and h1 is the tilting fulcrum C1 of the base body 3 located on the rotation axis of the moving operation section 2. It is the distance from the center of gravity G1 to the center of gravity G1, and these values are predetermined setting values. In addition, when the moving motion section 2 is a wheel-shaped moving motion section as seen in JP-A No. 2015-093651 etc., the respective values of r1 and h1 above are expressed by equations (1a) and (1b). can be set to different values.

The first term on the right side of equations (1a) and (1b) is the translational speed (X-axis direction and The second term on the right-hand side of equations (1a) and (1b) means the relative tilting of the base 3 with respect to the moving unit 2. It means the translational speed of the center of gravity G1 (translational speed in the X-axis direction and the Y-axis direction) that occurs relative to the center of gravity G1.

The motion acceleration estimation unit 13 calculates the direction from the estimated translational velocity Vbg_x,y of the center of gravity G1 of the base body 3 obtained as described above and the estimated angular velocity ωb_z in the yaw direction of the base body 3 among the input motion state quantities. Calculate the cardiac acceleration estimated value Abmr_x,y. Specifically, the centripetal acceleration estimated value Abmr_x,y is calculated from the translational velocity estimated value Vbg_x,y and the angular velocity estimated value ωb_z using the following equations (2a) and (2b).

Abmr_x=-ωb_z＊Vbg_y...(2a)
Abmr_y＝ωb_z＊Vbg_x……(2b)

Note that the centripetal acceleration estimated value Abmr_x,y is an estimated centripetal acceleration value that occurs in the entire base body 3, so it also has a meaning as an estimated value of the centripetal acceleration at the sensor position of the base body 3.

In addition, the motion acceleration estimating unit 13 uses the estimated angular velocity ωb (a set of ωb_x, ωb_y, ωb_z) of the base body 3 among the input motion state quantities, and the movement From the estimated rotational angular velocity ωw_x,y of the operating unit 2, calculate the estimated translational acceleration Abmt _x,y at the sensor position of the base body 3 by calculating the kinematic model shown by the following equations (3a) and (3b). do. Note that in this specification, "x" is an arithmetic symbol meaning a vector product operation (cross product operation).

Abmt_x＝r1＊(ωw_dot_y＋ωb_dot_y)＋(↑ωb_dot×↑p1)_x ...(3a)
Abmt_y＝-r1＊(ωw_dot_x＋ωb_dot_x)＋(↑ωb_dot×↑p1)_y ...(3b)

Here, r1 in equations (3a) and (3b) is the same as that shown in equations (1a) and (1b) above, and ωw_dot_x and ωw_dot_y are the first-order differential values of the estimated rotational angular velocity values ωw_x and ωw_y, respectively. (temporal rate of change). In addition, ↑ωb_dot is the first derivative value (temporal rate of change) of the vector ↑ωb (vector with ωb_x, ωb_y, ωb_z as three components) of the estimated angular velocity value ωb, and ↑p1 is the tilting fulcrum C1 of the base body 3. This is the position vector (see FIG. 1) of the sensor position (mounting position of the acceleration sensor 6) relative to the sensor position. Then, (↑ωb_dot×↑p1)_x is the X-axis direction component of the vector product (↑ωb_dot×↑p1), and (↑ωb_dot×↑p1)_y is the Y-axis direction component of the vector product (↑ωb_dot×↑p1). It is an ingredient.

In the present embodiment, as described above, the motion acceleration estimating unit 13 calculates the centripetal acceleration estimated value Abmr_x,y and the translational motion acceleration estimated value Abmt_x,y from the base body 3 due to the motion of the moving body 1A. It is determined as the motion acceleration generated at the sensor position.

Next, the posture estimating section 14 receives the estimated angular velocity ωb_x,y in the roll direction and the pitch direction of the angular velocity estimated value ωb obtained by the angular velocity estimating section 11, and the translational velocity obtained by the translational acceleration estimating section 12. The estimated acceleration value Ab (a set of Ab_x, Ab_y, Ab_z), the estimated centripetal acceleration value Abmr_x,y and the estimated translational acceleration value Abmt_x,y obtained by the motion acceleration estimation unit 13 are input. Then, the posture estimating unit 14 calculates the estimated posture angle θb_x,y of the base body 3 from these input values, as described below.

The posture estimating unit 14 first calculates the base body 3 based on the input translational acceleration estimated value Ab, the centripetal acceleration estimated value Abmr_x,y, and the translational acceleration estimated value Abmt _x,y obtained by the motion acceleration estimation unit 13. The process of estimating the reference attitude angle of is executed by the reference attitude angle estimation unit 14a. The reference attitude angle estimation section 14a corresponds to the reference attitude estimation section in the present invention.

Here, if there is an error in the previous attitude angle estimate θb_p_x,y used to obtain the translational acceleration estimated value Ab in the translational acceleration estimating unit 12, the gravitational acceleration component included in the obtained translational acceleration estimated value Ab A deviation occurs between the direction of the reference coordinate system and the Z-axis direction of the reference coordinate system.

Therefore, the reference attitude angle estimation unit 14a calculates the attitude angle estimation value θb_xy (hereinafter referred to as the current attitude angle estimation value θb_x,y) newly obtained by the attitude estimation unit 14 in the current (current) calculation processing cycle based on the above deviation. The attitude angle of the base body 3, which is used as a reference for correction to suppress the error, is determined as a reference attitude angle estimate θbb_x,y.

Specifically, the reference posture angle estimation unit 14a calculates the estimated translational acceleration values Ab_x in the X-axis direction and the Y-axis direction of the inputted translational acceleration estimated value Ab, as shown in the following equations (4a) and (4b). By subtracting the centripetal acceleration estimated value Abmr_x,y and the translational acceleration estimated value Abmt _x,y from y, the X-axis direction component g_x and Y-axis direction component g_y of the gravitational acceleration vector seen in the reference coordinate system are obtained. seek.

g_x＝Ab_x－Abmr_x－Abmt_x...(4a)
g_y＝Ab_y－Abmr_y－Abmt_y...(4b)

g_x and g_y obtained by these equations (4a) and (4b) respectively correspond to the X-axis direction component and the Y-axis direction component of the gravitational acceleration vector seen in the reference coordinate system. The reference attitude angle estimate θbb_x,y can be approximately estimated using the following equations (5a) and (5b).

θbb_ x=-g_y/gc...(5a)
θbb_y=g_x/gc...(5b)

Here, θbb_x is a reference attitude angle in the roll direction (direction around the X-axis), θbb_y is a reference attitude angle in the pitch direction (direction around the Y-axis), and gc is a gravitational acceleration constant. Based on the above equations (5a) and (5b), the reference attitude angle estimate θbb_x,y as the attitude angle estimate of the base body 3 is determined based on the gravitational acceleration vector seen in the reference coordinate system. Thereby, the reference attitude angle estimate θbb_x,y can be determined so that the direction of the gravitational acceleration vector included in the translational acceleration estimate Ab is the correct direction (vertical direction).

The attitude estimating unit 14 then uses the calculation unit 14b to subtract the reference attitude angle estimates θbb_x and θbb_y obtained as described above from the previous attitude angle estimates θb_x and θb_y of the base body 3, respectively. An estimated attitude angle error value θb_err_x,y is calculated as an estimated value of the error of the previous estimated attitude angle value θb_p_xy (hereinafter referred to as attitude angle error). The calculation unit 14b corresponds to the error estimation unit in the present invention. Then, this attitude angle error estimate θb_err_x,y is input to the feedback control unit 14c, and the feedback control unit 14c controls the base body so that each of the attitude angle error estimates θb_err_x, θb_err_y approaches zero according to the feedback control law. The feedback operation amounts δω_x and δω_y for correction of the estimated angular velocities ωb_x and ωb_x of No. 3 are determined, respectively.

The feedback control unit 14c may use, for example, the PI law (proportional/integral law) as the feedback control law. In this case, the feedback operation amounts δω_x and δω_y are calculated using the following equations (6a) and (6b).

δω_x＝k1_x＊θb_err_x＋k2_x＊∫θb_err_x dt...(6a)
δω_y＝k1_y＊θb_err_y＋k2_y＊∫θb_err_y dt...(6b)

k1_x, k2_x, k1_y, and k2_y in these equations (6a) and (6b) are gains of predetermined values. According to the above equations (6a) and (6b), the posture caused by the offset of the estimated angular velocity values ωb_x and ωb_x of the base body 3 and the error in the orientation of the local coordinate system and the coordinate axes set for the base body 3 of the moving body 1A. Feedback operation amounts δω_x and δω_y are calculated so as to reduce the angular error. Supplementally, the feedback control law used by the feedback control unit 14c is not limited to the PI law, but may be, for example, a PID law (proportional, differential, integral law) or the like.

The posture estimation unit 14 then uses the calculation unit 14d to calculate the feedback operation amounts δω_x and δω_y obtained by the feedback control unit 14c from the estimated angular velocities ωb_x and ωb_x of the base body 3 obtained by the angular velocity estimation unit 11, respectively. By subtracting , each of the estimated angular velocity values ωb_x and ωb_x is corrected. Then, by integrating this corrected angular velocity estimated value by the integrator 14e, the current attitude angle estimated value θb_x,y of the base body 3 is obtained. Note that the integrator 14e corresponds to an integration processing section in the present invention.

In this embodiment, the arithmetic processing device 10 sequentially executes the orientation designation process of the base body 3 at a predetermined arithmetic processing cycle, as described above. According to this embodiment, the estimated centripetal acceleration Abmr_x,y of the base 3 estimated from the actual motion state quantity of the moving body 1A is calculated from the estimated translational acceleration Ab based on the detected output of the acceleration sensor 6, and the estimated centripetal acceleration Abmr_x,y of the base 3 By subtracting the estimated translational motion acceleration Abmt_x,y at the sensor position in step 3, a highly reliable gravitational acceleration vector (gravitational acceleration vector seen in the reference coordinate system) can be obtained in various motion states of the moving body 1A. I can do it. Furthermore, the reference attitude angle can be estimated with high reliability from the X-axis direction component g_x and Y-axis direction component g_y of the gravitational acceleration vector, which indicate the amount of deviation between the direction of this gravitational acceleration vector and the Z-axis direction of the reference coordinate system. The value θbb_x,y can be found.

Then, the feedback operation amount is determined by the feedback control law so that the estimated attitude angle error θb_err_x,y, which is determined as the deviation between the reference attitude angle estimate θbb_x,y and the previous specified attitude angle value θb_p_x,y, converges to zero. After correcting the estimated angular velocity ωb_x,y based on the detection output of the angular velocity sensor 7 using δω_x and δω_y, the estimated attitude angle θb_x,y of the base body 3 is calculated by integrating the corrected estimated angular velocity. Ru. Thereby, highly reliable attitude angle estimation values θb_x,y can be continuously obtained in various motion states of the mobile body 1A.

[Second embodiment]
Next, a second embodiment of the present invention will be described with reference to FIG. Note that in this embodiment, detailed explanations of the same items as in the first embodiment will be omitted. In this embodiment, the configuration of a mobile body 1B is different from that of the mobile body 1A of the first embodiment, and includes a plurality of wheels 22 and a base body (vehicle body) supported by these wheels 22 via a suspension mechanism (not shown). 23. For example, the wheels 22 include a front wheel 22f and a rear wheel 22r, which are arranged one at a time in the front and rear, and an intermediate position between the front wheel 22f and the rear wheel 22r (an intermediate position in the longitudinal direction of the moving body 1B). In FIG. 3, two intermediate wheels 22m are provided with a gap between them in the direction (perpendicular to the plane of the paper). For example, the front wheel 22f and the rear wheel 22r are driven wheels, and the intermediate wheel 22m is a driving wheel. It should be noted that each wheel 22 may be similar to the wheels of a normal automobile, but, for example, it may be of a structure similar to a wheel-shaped moving unit as seen in Japanese Patent Application Laid-Open No. 2015-093651. Good too.

A suspension mechanism between the base body 23 and the plurality of wheels 22 is configured such that the base body 23 can tilt in the roll direction and the pitch direction using a point C2 on the rotation axis of the intermediate wheel 22m as a fulcrum. .

Furthermore, an inertial sensor 8 (including an acceleration sensor 6 and an angular velocity sensor 7) configured similarly to that of the first embodiment and an arithmetic processing unit 10 are mounted on the base body 23. Similar to the first embodiment, the arithmetic processing device 10 includes a function as a posture estimating device, and by this function, it is possible to successively estimate the actual posture of the base body 23 as a predetermined part of the mobile body 1B. It is possible.

Next, the process of estimating the posture of the base body 23 executed by the arithmetic processing device 10 of this embodiment will be specifically described. In addition, in the processing of the arithmetic processing device 10 of this embodiment, the three-axis orthogonal coordinate system (shown in FIG. 3) set similarly to the first embodiment is a reference for expressing the motion state quantity such as the attitude of the base body 3. Used as a coordinate system.

The processing of the arithmetic processing device 10 of this embodiment includes the angular velocity estimating section 11, the translational acceleration estimating section 12, the motion acceleration estimating section 13, and the posture estimating section 14 shown in FIG. 2, as in the first embodiment. In this case, in this embodiment, only a part of the processing of the motion acceleration estimation unit 13 is different, and the other processing is the same as the first embodiment. Therefore, the processing of the motion acceleration estimation unit 13 will be explained below, focusing on the differences from the first embodiment.

In this embodiment, the motion acceleration estimating unit 13 includes the estimated angular velocity ωb (a set of ωb_x, ωb_y, ωb_z) generated by the angular velocity estimating unit 11, and the angular velocity estimated value ωb (a set of ωb_x, ωb_y, ωb_z) of the intermediate wheel 22m of the wheels 22 of the moving body 1B. The estimated rotational angular velocity value ωw_x,y is input as the observed value of the motion state quantity of the moving body 1B. The estimated rotational angular velocity value ωw_x,y can be estimated, for example, from the detection output of a rotary encoder, resolver, etc. that can detect the rotation of the intermediate wheel 22m.

Note that if the intermediate wheel 22m is a wheel that can rotate only in the pitch direction, the estimated rotational angular velocity value ωw_x in the roll direction may be zero.

In order to estimate the centripetal acceleration of the motion acceleration, the motion acceleration estimating unit 13 uses the estimated angular velocity ωb_x,y of the base body 23 in the roll direction and pitch direction and the intermediate wheel 22m among the input motion state quantities. From the estimated rotational angular velocity ωw_x,y, the estimated translational velocity Vbg_x,y of the center of gravity G2 of the base body 23 is calculated by calculating the kinematic model shown in the following equations (11a) and (11b). Note that in equations (11a) and (11b), the average value of the estimated rotational angular velocity values ωw_x,y of the two intermediate wheels 22m may be used as the value of ωw_x,y.

Vbg_x=r2*ωw_y+h2*ωb_y...(11a)
Vbg_y=-r2*ωw_x-h2*ωb_x...(11b)

Here, with reference to FIG. 2, r2 in equations (11a) and (11b) is the rotation radius of the intermediate wheel 22m, h2 is the distance from the tilting fulcrum C2 of the base body 23 to the center of gravity G2, and these values is a predetermined setting value. In addition, when the intermediate wheel 22m is a wheel-shaped moving motion part as seen in JP-A No. 2015-093651 etc., the respective values of r2 and h2 above are expressed by equation (11a) and equation (11b). They can be set to different values.

The first term on the right side of equations (11a) and (11b) means the translational speed of the center of the intermediate wheel 22m (translational speed in the X-axis direction and the Y-axis direction, respectively), and the first term on the right side of equations (11a) and ( The second term on the right side of 11b) is the translation speed of the center of gravity G2 (X-axis direction and Y-axis direction) that occurs relative to the intermediate wheel 22m due to the relative tilting of the base body 23 with respect to the intermediate wheel 22m. ).

The motion acceleration estimating unit 13 calculates the estimated translational velocity Vbg_x,y of the center of gravity G1 of the base body 3 obtained as described above and the estimated angular velocity ωb_z of the base body 3 in the yaw direction among the input motion state quantities. As in the first embodiment, the centripetal acceleration estimated value Abmr_x,y is calculated using the equations (2a) and (2b).

Furthermore, in order to estimate the translational acceleration among the motion accelerations, the motion acceleration estimation unit 13 uses the estimated angular velocity ωb (a set of ωb_x, ωb_y, ωb_z) of the base body 23 among the input motion state quantities, and the intermediate The estimated translational acceleration Abmt _x,y at the sensor position of the base body 23 is calculated from the estimated rotational angular velocity ωw_x,y of the wheel 22m by calculating the kinematic model shown by the following equations (23a) and (23b). .

Abmt_x＝r2＊ωw_dot_y＋(↑ωb_dot×↑p2)_x ……(23a)
Abmt_y＝-r2＊ωw_dot_x＋(↑ωb_dot×↑p2)_y ...(23b)

Here, r2 in equations (23a) and (23b) is the same as that shown in equations (21a) and (21b) above, and ωw_dot_x and ωw_dot_y are the first-order differential values of the estimated rotational angular velocity values ωw_x and ωw_y, respectively. (temporal rate of change). In addition, ↑ωb_dot is the first derivative value (temporal rate of change) of the vector ↑ωb (vector with ωb_x, ωb_y, ωb_z as three components) of the estimated angular velocity value ωb, and ↑p2 is the tilting fulcrum C2 of the base body 23. This is the position vector (see FIG. 3) of the sensor position (mounting position of the acceleration sensor 6) relative to the sensor position. Then, (↑ωb_dot×↑p2)_x is the X-axis direction component of the vector product (↑ωb_dot×↑p2), and (↑ωb_dot×↑p2)_y is the Y-axis direction component of the vector product (↑ωb_dot×↑p2). It is an ingredient.

In the present embodiment, as described above, the motion acceleration estimating unit 13 calculates the centripetal acceleration estimated value Abmr_x,y and the translational motion acceleration estimated value Abmt_x,y from the base body 23 due to the motion of the moving body 1B. It is determined as the motion acceleration generated at the sensor position. The processing of the arithmetic processing device 10 other than the matters explained above is the same as in the first embodiment. This embodiment can produce the same effects as the first embodiment.

In addition, in this embodiment, the mobile body 1B equipped with the intermediate wheels 22m has been described, but a mobile body without the intermediate wheels 22m, for example, a four-wheeled mobile body having two front wheels and two rear wheels, or a front wheel The vehicle may also be a three-wheeled moving body having one rear wheel and two rear wheels.

1A, 1B... moving body, 3, 23... base (predetermined part), 6... acceleration sensor, 7... angular velocity sensor, 10... arithmetic processing unit (posture estimating device) 11... angular velocity estimating section, 12... translational acceleration estimating section , 13... Motion acceleration estimation section, 14... Attitude estimation section, 14a... Reference attitude angle estimation section (reference attitude estimation section), 14b... Arithmetic section (error estimation section), 14c... Feedback control section, 14e... Integrator (integrator) processing section).

Claims (1)

A device for estimating the posture of a predetermined part of a moving body,
an angular velocity estimation unit that generates an estimated value of the angular velocity of the predetermined portion from the output of an angular velocity sensor mounted on the predetermined portion;
a translational acceleration estimation unit that generates an estimated value of the translational acceleration at the mounting position of the acceleration sensor from the output of the acceleration sensor mounted at the predetermined portion, the estimated value including a gravitational acceleration component;
The estimated value of the motion acceleration generated at the mounting position of the acceleration sensor due to the motion of the predetermined region is calculated by calculating the motion acceleration that has a correlation with the motion acceleration of the predetermined region without using a target value regarding the motion of the predetermined region. a motion acceleration estimation unit that generates from observed values of body motion state quantities;
a posture estimation unit that generates an estimated value of the posture of the predetermined part from the estimated value of the angular velocity, the estimated value of the translational acceleration, and the estimated value of the motion acceleration;
The motion acceleration estimation unit is configured to generate an estimated value of motion acceleration including translational motion acceleration and centripetal acceleration generated at the mounting position of the acceleration sensor,
The posture estimating unit includes a reference posture estimating unit that estimates a reference posture of the predetermined part based on the difference between the estimated value of the translational acceleration and the estimated value of the motion acceleration; an error estimator that estimates the difference between the latest orientation estimate and the reference orientation estimate as an error in the orientation estimate; and according to the error estimate, the error is reduced to zero at a predetermined period. a feedback control unit that generates a feedback operation amount according to a feedback control law so as to approximate the angular velocity; and a feedback control unit that generates an estimated value of the posture of the predetermined part by integrating an input obtained by correcting the estimated value of the angular velocity using the feedback operation amount. and an integral processing section to
The observed value of the motion state quantity is a motion state quantity other than the translational acceleration detected by the acceleration sensor, and is an observed value of the motion state quantity observed in accordance with the motion of the moving body in the predetermined period. A posture estimation device for a moving body, characterized by the following.

Images