JP2023533065A - Absolute Heading Estimation Using Constrained Motion - Google Patents

Abstract

The present invention relates to a method of measuring at least one of declination, orientation and position of an object, comprising the steps of: pivoting a gyroscope attached to said object at a pivot angular velocity about a pivot axis; using the gyroscope to measure an unmeasured pivot about an axis different from the pivot axis; and measuring using the pivot angular velocity; and measuring the declination of the object relative to true north from the component of the rotation of the earth acting on the gyroscope; Position is measured starting from the initial orientation and position of the object using the unmeasured pivot, the pivot angular velocity, and the distance traveled by the object, and the measured rotation of the earth. compensating for the effects of the components of the body on the orientation and position of the object, wherein the object is a vehicle or part of a vehicle, the vehicle comprising a first vehicle portion and a second vehicle portion. wherein the first vehicle portion is pivotable relative to the second vehicle portion about the pivot axis, the gyroscope is mechanically attached to the first vehicle portion; and The method further comprises measuring the pivoting angular velocity of the gyroscope about the pivot axis using a second sensor different from the gyroscope. [Selection diagram] Fig. 1

Description

The present disclosure relates to a method for measuring at least one of declination, orientation and position of an object, the method comprising moving a gyroscope attached to said object at a pivoting angular rate about a pivot axis. pivoting; measuring with the gyroscope any unmeasured pivoting of the object about an axis different from the pivot axis; ) using the unmeasured pivot and the pivot angular rate, and measuring the declination of the object relative to true north from the components of the rotation of the earth acting on the gyroscope. or measuring the orientation and position of the object starting from the initial orientation and position of the object using the unmeasured pivot, the pivot angular velocity, and the distance traveled by the object; and compensating for the effect of the measured component of earth's rotation on the orientation and position of the object.

North-finding systems are known in the prior art to measure the declination of an object from true north. In these systems for finding north, preferably a gyroscope capable of sensing the rate of rotation of the earth is often used as it can provide a more independent measurement. From the declination of the object relative to true north, the orientation of the object can be determined.

Furthermore, it is known from the prior art to measure the orientation and position of an object starting from an initial orientation and position of the object, which takes into account the distance traveled from the initial position of the object, and , by considering the unmeasured pivot about the axis of the object. For this application, it is recommended to compensate for any impact on the measurement of object pivoting caused by the earth's rotational speed.

The earth rotates at only about 15 degrees per hour, and accurate and direct measurement of this speed requires large and costly sensing equipment, such as navigation grade inertial navigation systems. Sensor rotation techniques have been proposed over the decades to reduce the accuracy requirements of such gyros. In these techniques, gyroscope accuracy is enhanced by rotating the gyroscope about a pivot axis at a known angular velocity during measurement. This technique is also known as carouseling (if the rotation is continuous) or indexing (if the rotation is incremental).

Modern automobiles employ satellite-based positioning systems for navigation. Currently, automotive navigation systems are primarily used to guide drivers on-road and off-road. However, as autonomous driving becomes possible, vehicle navigation systems will play an increasingly important role. Reliability of navigation, positioning and bearing estimation becomes more important as humans have less influence in controlling vehicles.

H. Seraji, "Configuration control of redundant manipulators: theory and implementation," (IEEE Transactions on Robotics and Automation), Vol. 88062 D. Titterton, J. Weston, Strapdown Inertial Navigation Technology, Second Edition (Progress in Astronautics & Aeronautics), ISBN-13:9781563476938, January 2005, 283, paragraph 10.3.2, "Ground alignment methods ” D. Titterton, J. Weston, "Strapdown Inertial Navigation Technology", Second Edition (Progress in Astronautics & Aeronautics), ISBN-13:9781563476938, January 2005, "Ground alignment methods", page 31, "Local geographic navigation frame mechanization." ”

However, due to landscape constraints around the vehicle, 100% coverage of satellite signals cannot be guaranteed under all circumstances. For example, availability and accuracy are degraded in canyons between urban buildings, on the ground such as tunnels or mines, or on roads surrounded by obstructions such as tall trees or rocks.

To overcome situations in which satellite signals are not available or are of poor quality, gyroscopes can be incorporated into vehicles to allow measurement of the component of earth rotation acting on the gyroscope. would be desirable.

This object according to the present disclosure is solved by a method for measuring at least one of declination, orientation and position of an object. The method includes pivoting a gyroscope attached to the object at a pivot angular velocity about a pivot axis; measuring using the gyroscope; measuring a component of the rotation of the earth acting on the gyroscope using the unmeasured pivot and the pivot angular rate; from the components of the earth's rotation acting on the gyroscope, or determining the orientation and position of the object starting from the initial orientation and position of the object and the unmeasured pivoting , using the pivoting angular velocity and the distance traveled by the object, and compensating for the effect of the measured component of the rotation of the earth on the orientation and position of the object. The object is a vehicle or part of a vehicle, the vehicle comprising a first vehicle portion and a second vehicle portion, the first vehicle portion being oriented relative to the second vehicle portion about the pivot axis. Pivotable, the gyroscope is mechanically attached to the first vehicle portion. The method further includes measuring the pivoting angular velocity of the gyroscope about the pivot axis using a second sensor different from the gyroscope.

The basic concept of the present disclosure is a gyroscope mounted on a vehicle, the ability to measure at least the declination or heading of the vehicle or part of the vehicle taking into account the component of the rotation of the earth acting on said gyroscope. to provide a gyroscope with In order to be able to measure the rotational component of the earth acting on said gyroscope, the gyroscope is pivoted about a pivot axis at a pivot angular rate, and to measure this pivoting, a separate sensor is used.

The present disclosure utilizes a vehicle that includes two vehicle portions that are pivotable relative to each other about a pivot axis. The gyroscope is mechanically attached to one of the two vehicle parts, the first vehicle part. Pivoting the two vehicle parts relative to each other then defines a carouseling or indexing of the gyroscope.

By mounting the gyroscope on a first vehicle part that is pivoted relative to the second vehicle part, either carouseling or indexing of the gyroscope is available at considerably lower cost and space. is. This allows for efficient bias estimation of the gyroscope.

In order to take advantage of the pivoting movement of the gyroscope caused by the relative pivoting of the first vehicle part and the second vehicle part about the pivot axis, the second vehicle part A second sensor measures the pivoting angular velocity of the first vehicle part relative to the pivot about said pivot axis. This second sensor is separate from the gyroscope. By measuring the relative pivoting angular velocity of the first vehicle portion with respect to the second vehicle portion, the pivoting angular velocity of the gyroscope about the pivot axis is also measured. The separate second sensor provides a pivoting angular velocity that is unaffected by earth velocity.

A method according to the present disclosure may be used to determine at least one of the orientation or position of an object. Orientation in the sense of this disclosure is part of the description of how said object is positioned within the space it occupies. Orientation refers to the virtual rotation required to move the object from a reference orientation to its current orientation. In contrast, the position of the object refers to the virtual transformation required to move the object from its reference configuration to its current configuration. Together, orientation and position completely describe how the object is placed in space. According to the present disclosure, the orientation of the object is relative to true north.

Gyroscope carouseling according to the present disclosure is used to measure the component of earth rotation acting on the gyroscope. The measured component of earth rotation acting on the gyroscope according to the present disclosure is used in two different ways.

According to a first aspect, the component of earth rotation acting on the gyroscope is used to measure the declination of the object relative to true north. Declination according to the present disclosure is defined as the angle between a predetermined axis of an object and true north in a plane that is horizontal at the location of said object. In order for true north to coincide with the predetermined axis of the object, the defined axis of the object must be pivoted by this angle. True north is the direction along the planet's surface toward the point where the planet's axis of rotation intersects the planet's surface. The due south can be similarly defined.

In one embodiment, the declination relative to true north is then used to determine the orientation of the object. Azimuth, according to the present disclosure, includes information about the declination relative to true north and at least one further rotation about a further pivot axis (to align the object from a predetermined reference position to its current position). required).

Alternatively, the measured components of the rotation of the earth acting on the gyroscope are the orientation and position of the object at the target orientation and after the object has moved from the known initial orientation and known initial position to the target orientation and target location. Used to increase the accuracy of measurements with target locations. In this aspect, the unmeasured pivoting of the object and the distance traveled by the object from its initial position to the target position (path length) are used to determine the orientation and position of the object at the target position. .

If the rotation of the earth is not taken into account, there will be deviations in the pivot angle. Minimizing this deviation would provide more accurate information about orientation.

In this aspect, the unmeasured pivoting of the object and the distance traveled by the object from the initial position to the target position, i.e. the path length, are used to determine the orientation and position of the object at the target position. be done. The distance traveled by the object is determined by multiplying the pivot angle of the pivot and the known dimensions of the part of the vehicle. A method for measuring distance traveled is described in US Pat.

Measurements of the object's orientation and position at the target location are affected by the rotation of the Earth superimposed on both the pivot axis and the unmeasured pivot axis. By redundantly measuring the orbital pivot angle using a sensor separate from the gyro sensor, the effects of the earth's rotational component can be compensated for in both axes. Said sensors apart from gyro sensors are for example accelerometers, rotary encoders, magnetometers, visual sensors or light detection ranging systems. Such a sensor should be able to provide a pivot angle or pivot angular rate that is unaffected by earth velocity.

In one embodiment of the disclosure, the vehicle is a wheeled vehicle or a tracked vehicle, the first vehicle portion is a wheel, and the second vehicle portion is a wheeled vehicle or tracked vehicle. A rail vehicle frame or body. In order to be able to infer the distance traveled from the pivot using the wheels of the vehicle, it is necessary to know the radius of the wheels. If the vehicle is a tracked vehicle, it is clear that the wheels in one embodiment are wheels that guide a continuous track. In an alternative embodiment, said wheel is the steering wheel of said vehicle.

In a further embodiment of the present disclosure, the vehicle comprises a manipulator arm, the first vehicle portion is a first arm section of the manipulator arm, and the second vehicle portion is a first arm section of an excavator arm. A two-arm section, or the frame or body of the vehicle.

Surprisingly, using a pivoting motion of the manipulator arm yields similar results to using a rotation of the wheel (although the manipulator provides less than 360 degrees of pure pivoting, not a full rotation). . Further, using a manipulator arm to provide pivoting motion of the gyroscope allows measurement of the manipulator arm, or a portion thereof, as an object within the meaning of this disclosure.

The above disclosure merely describes carouseling and indexing based on the pivotal motion of a manipulator providing a defined pivotal motion of the gyroscope in combination with the measurement of the rotation component of the earth. However, using the pivoting motion of a vehicle manipulator can be considered an invention in itself, even without measuring the component of earth rotation acting on the gyroscope.

Accordingly, the present disclosure further relates to a method of determining at least one of the orientation or position of an object. The method includes pivoting a gyroscope attached to the object at a pivot angular velocity about a pivot axis; , using a gyroscope to measure the orientation and position of the object, starting from the initial orientation and position of the object, the unmeasured pivot, the angular velocity of the pivot, and the displacement of the object; and measuring with the distance. The object is a vehicle or part of a vehicle, the vehicle comprising a first vehicle portion and a second vehicle portion, the first vehicle portion being oriented relative to the second vehicle portion about the pivot axis. Pivotable, the gyroscope is mechanically attached to the first vehicle portion. The method further includes measuring the pivoting angular velocity of the gyroscope about the pivot axis using a second sensor different from the gyroscope. The vehicle comprises a manipulator arm, the first vehicle portion being a first arm section of the manipulator arm and the second vehicle portion being a second arm section of the manipulator arm or a frame or body of the vehicle. be.

In one embodiment of the present disclosure, said vehicle with manipulator arm is an excavator and said manipulator arm is an arm of an excavator.

In one embodiment of the present disclosure, said second sensor is an acceleration sensor, said acceleration sensor comprising a known distance from said pivot axis.

In a further embodiment of the present disclosure, said pivoting is a rotation of 360 degrees or more. For example, it is clear that the wheels of a moving wheeled or tracked vehicle of said vehicle rotate through a full rotation of 360 degrees or more at a given but varying speed.

In further embodiments of the present disclosure, the gyroscope is a microelectromechanical system (MEMS). So far, MEMS-based gyroscopes have not been able to measure the component of earth rotation acting on the gyroscope. However, with effective denoising and bias compensation (particularly done by carouseling/indexing according to the present disclosure), commercially available inexpensive MEMS-based gyroscopes are available, which allow the component of earth rotation can be measured.

In one embodiment of the present disclosure, the gyroscope and accelerometer form a six degree of freedom measurement system. In so-called 6-DOF measurement systems, accelerometers and gyroscopes observe measurements from three non-parallel measurement axes (a so-called 6-DOF system).

Further advantages, features and applications of the present disclosure will become apparent from the following description of embodiments and the corresponding accompanying drawings. The foregoing, as well as the following detailed description of the embodiments, will be better understood when read in conjunction with the accompanying drawings. It should be understood that the illustrated embodiments are not limited to the precise arrangements and instrumentation shown. In the drawings, similar elements are indicated by the same reference numerals.

1 is a schematic side view of one embodiment of an excavator; FIG. It is a figure which shows the time change of a pivot angle. The time derivative of the pivot angle is the pivot angular velocity. FIG. 4 is a schematic diagram of different pivot angles and corresponding declination angles; 2 is a schematic top view of the excavator of FIG. 1 during operation; FIG.

Figure 1 is a schematic side view of an excavator 1 as an example of a vehicle within the meaning of the present disclosure.

Reference numerals 101a, 101b, 101c indicate MEMS-based gyroscopes placed at different locations on the excavator. A gyroscope is used to perform the method according to the present disclosure. Reference numerals 102a, 102b, 102c indicate different parts of the excavator 1 that are considered objects of the present disclosure. Using the methods described in this disclosure, at least the orientation or position of each portion 102a, 102b, 102c of excavator 1 is determined.

In summary, the schematically illustrated excavator allows the following setup of gyroscopes 101a, 101b, 101c and objects 102a, 102b, 102c.

The gyroscope 101a is attached to wheels 2 that guide the continuous track 3 of the excavator 1 . Wheels 2 are rotating around pivot shafts 4 . The gyroscope 101a is used to measure the orientation and position of the excavator 1 chassis 102a. An accelerometer 103a as a second sensor is attached to the wheel 2 to measure the pivoting angular velocity ω _carousel of the gyroscope 101a.

Gyroscope 101b is mounted on manipulator arm 5 on first arm section 102c. The manipulator arm 5 includes a first arm section 102c and a shovel 8 as a second arm section). The first manipulator arm section 102c is pivotable about the pivot axis 7 relative to the body 102b of the excavator 1 . The gyroscope 101b is used to measure the orientation and position of the excavator 1 body 102b. A second sensor, an accelerometer 103b, is attached to the first manipulator arm section 102c to measure the pivoting angular velocity ω _carousel of the gyroscope 101b.

Furthermore, the shovel 8 is pivotable about the pivot axis 9 with respect to the first manipulator arm section 102c. A gyroscope 101 c is attached to the excavator 8 . During operation, the excavator 1 performs a pivoting movement about the pivot axis 9 with respect to the first manipulator arm section 102c. An accelerometer 103c as a second sensor is attached to the excavator 8 to measure the pivoting angular velocity ω _carousel of the gyroscope 101c.

As an example, the measurement of the deviation (deviation) of the main body 102b of the excavator 1 from true north will be described below.

The processing system uses the measured signal ω from the gyroscope 101, the measured signal a from the accelerometer 103b, and the initial orientation C ₀ as inputs and provides the position p and orientation C as outputs f. i.e.

式中、[ωｘ]は、ジャイロスコープデータの非対称行列である。これは地球速度の影響を受けており、Ｃ又はＣの一部の明確な計測値を用いて、地球速度の影響を推定できる。 The processing system allows the estimation of the position of the chassis 102a through the constraints due to the motion of the wheels 2, and iteratively finds the optimal C. If C is a direction cosine matrix, its differential value is

where [ωx] is the asymmetric matrix of gyroscope data. It is affected by the Earth's velocity, and using well-defined measurements of C or part of C, the influence of the Earth's velocity can be estimated.

If the effect of earth velocity is minimized, C already contains azimuth information that can be absolutely referenced. To clarify why perfect heading estimation is possible, both gravity and earth velocity are observable in the body frame of reference (i.e., the frame of reference fixed to rotating gyroscope 101a and accelerometer 103a). , is also known in the Earth frame, ie, a reference frame fixed to the Earth/Moon/Mars. Knowing two non-parallel vectors in this way means that complete orientation information is available, as is known in the art. This is described, for example, in US Pat. An earth velocity signal affected by the rotation of the earth slows the rotation of the vehicle. According to this disclosure, the direction of the earth velocity signal is estimated by finding the direction in which the earth velocity produces the least error in the navigation output. When the raw gyroscope signal is processed by an inertial navigation algorithm, any constant signal component therein will be sinusoidally modulated in the calculation of acceleration. Examples of pseudo-constant signal components are sensor bias due to MEMS fabrication or the earth's rotation rate. From the joint angular dynamics and the dimensions of the vehicle wheels, it can be seen that this sinusoidal signal is not a real signal, but a composite signal due to the combined effects of the forced rotation of the gyroscope 101a and the quasi-stationary component. It is clear that inertial navigation algorithms are very sensitive to constant offsets. For example, looking at accelerations derived from inertial navigation algorithms and accelerations derived from measurements of joint angular dynamics (also obtained by inertial sensors, but using angles and vehicle dimensions) captures the component caused by earth rotation. It is possible to The minimization process finds the orientation that gives the best match between angular derived acceleration and inertial navigation derived acceleration. This is similar to bias estimation for MEMS-based devices known from the prior art. Acceleration differences based on inertial and dimension-based calculations are minimized by trying different absolute headings. This minimization can be performed in the position domain or the velocity domain (whichever is more optimal for computational convenience). Since the Earth velocity signal is very weak, Earth velocity measurements are much more susceptible to noise than bias estimates. Observation over a longer period of time, however, will reveal distinct differences in acceleration, velocity, and/or position derived from incorrect absolute headings.

In the following example, we assume that both the accelerometer and the gyroscope are observing measurements from three non-parallel measurement axes (a so-called six degree of freedom system).

FIG. 2 shows the pivot angle of the pivot axis 7 calculated using inertial navigation formulas known in the prior art (eg US Pat. The asterisks indicate the result of giving the inertial navigation mechanization algorithm an erroneous declination. The circles show the results when the algorithm was given a better estimate of the argument. The deviation between these two is due to the earth's velocity affecting the measurements. The diamonds indicate the pivot angle, which is obtained by calculating the accelerometer-calculated position (using known dimensions) as external feedback to the inertial navigation mechanization system. FIG. 3 shows the difference between the wrong declination and the feedback system results (asterisks) and the difference between the better declination and the feedback system (circles). It will be appreciated that this difference will be smaller for better declination, so algorithms aimed at minimizing this difference will resolve the declination. Also shown in FIG. 3 is the corresponding declination in a top view of the excavator declination, with the solid arrows indicating the true and corresponding declination of FIG. Methods to minimize the difference can be brute force or, preferably, gradient search algorithms or extended Kalman filters or modern machine learning methods. The feedback system can be an extended Kalman filter with position update, as known in the art. Position updates derived from pivot angles and vehicle dimensions are then considered external measurements of the system. Preferably, such a filter may be an extended Kalman filter containing state estimates for gyroscope bias and augmented by a neural network.

FIG. 4 is a top view of the excavator 1 of FIG. Distance traveled 401 can be derived from the known wheel radius and pivot angle. Another travel distance 401 can be calculated from the known dimensions of the manipulator arm 5 and the known pivot angle using vector summation (eg shovel position 402 relative to vehicle center). The declination is the angle 403 between true north N and the principal axis 404 of the object.

Note that features described in relation to one embodiment may be used in other embodiments, as will be readily appreciated by those skilled in the art. Although the present invention has been described in detail with reference to the drawings, this description is merely illustrative and should not be taken as limiting the scope of protection defined by the claims. Known distances or radii may indicate such measurement information in mm, cm, meter level, for example.

In the claims, the term "comprising" does not exclude other elements or steps, and the undefined article (a) does not exclude a plurality. The mere fact that certain features are recited in different claims does not exclude their combination. Reference numerals in the claims should not be construed as limiting the scope of protection.

REFERENCE SIGNS LIST 1 excavator 2 wheels 3 continuous track 4, 7, 9 pivot 5 manipulator arm 6 first arm section 8 shovel 101a, 101b, 101c gyroscope 102a chassis 102b body 102c first arm section 103a, 103b, 103c accelerometer 401 distance traveled 402 shovel position 403 angle between due north and object principal axis 404 object principal axis ω _carousel angular velocity N due north

Claims (8)

A method of measuring at least one of declination, orientation and position of an object (102a, 102b, 102c), comprising:
pivoting gyroscopes (101a, 101b, 101c) attached to said objects (102a, 102b, 102c) about pivot axes (4, 7, 9) at a pivoting angular velocity (ω _carousel );
Measuring unmeasured pivotal movements of said objects (102a, 102b, 102c) about axes different from said pivot axes (4, 7, 9) using said gyroscopes (101a, 101b, 101c) and
the components of the rotation of the earth acting on said gyroscopes (101a, 101b, 101c),
the unmeasured pivotal motion; and
measuring using the pivot angular velocity (ω _carousel );
measuring the declination of said object (102a, 102b, 102c) relative to true north from the component of earth rotation acting on said gyroscope (101a, 101b, 101c); or
determining the orientation and position of the object (102a, 102b, 102c) starting from an initial orientation and position of the object (102a, 102b, 102c),
said unmeasured pivot;
measured using the pivot angular velocity (ω _carousel ) and the distance traveled by the objects (102a, 102b, 102c), and
compensating for the effect of the measured component of earth rotation on the orientation and position of the object (102a, 102b, 102c);
said object is a vehicle (1) or a part (102a, 102b, 102c) of a vehicle, said vehicle (1) comprising a first vehicle portion and a second vehicle portion;
said first vehicle portion being pivotable relative to said second vehicle portion about said pivot axis (4, 7, 9);
said gyroscope (101a, 101b, 101c) is mechanically attached to said first vehicle portion; and
The method further comprises:
The pivoting angular velocity (ω _carousel ) of the gyroscopes (101a, 101b, 101c) about the pivot axes (4, 7, 9) is set to a different A method, characterized in that it comprises the step of measuring with two sensors (103a, 103b, 103c). The vehicle is a wheeled or tracked vehicle, the first vehicle portion is a wheel, the wheel includes a known wheel radius, and the second vehicle portion is the frame or body of the wheeled vehicle. 2. The method of claim 1, wherein The vehicle comprises a manipulator arm, the first vehicle portion is a first arm section of the manipulator arm and the second vehicle portion is a second arm section of the manipulator arm, or the second vehicle 2. The method of claim 1, wherein the part is a vehicle frame or body. 4. A method according to any preceding claim, wherein said second sensor is an acceleration sensor, said acceleration sensor comprising a known distance from said pivot axis. 5. A method according to any preceding claim, wherein said pivoting is a rotation of 360 degrees or more. 6. The method of any of claims 1-5, wherein the gyroscope is a microelectromechanical system. 7. The method of any of claims 1-6, wherein the gyroscope and accelerometer form a six degree of freedom measurement system. 8. A method according to any preceding claim, further comprising determining the orientation of the object from the declination of the object relative to true north.

Images