Absolute heading estimation using constrained motion

Abstract

The present invention relates to a method for measuring at least one of a declination angle, an orientation, and a position of an object, comprising the steps of: pivoting a gyroscope attached to the object at a pivot angular velocity about a pivot axis; measuring an unmeasured pivot of the object about an axis different from the pivot axis using the gyroscope; measuring a component of the Earth's rotation acting on the gyroscope using the unmeasured pivot and the pivot angular velocity; and measuring the declination angle of the object with respect to true north from the component of the Earth's rotation acting on the gyroscope; or measuring the orientation and position of the object by measuring the unmeasured pivot from an initial orientation and initial position of the object. , the pivot angular velocity, and the distance traveled by the object, and compensating for the effect of the measured component of the Earth's rotation on the orientation and position of the object, wherein the object is a vehicle or part of a vehicle, the vehicle including a first vehicle portion and a second vehicle portion, the first vehicle portion being pivotable relative to the second vehicle portion about the pivot axis, the gyroscope being mechanically attached to the first vehicle portion, and the method further comprising measuring the pivot angular velocity of the gyroscope about the pivot axis using a second sensor distinct from the gyroscope.

Description

[Technical Field]

[0001] The present disclosure relates to a method for measuring at least one of an object's declination angle, orientation, and position, the method comprising: pivoting a gyroscope attached to the object at a pivot angular velocity about a pivot axis; measuring an unmeasured pivot of the object about an axis different from the pivot axis using the gyroscope; measuring a component of the Earth's rotation acting on the gyroscope using the unmeasured pivot motion and the pivot angular velocity; and measuring the object's declination angle with respect to true north from the component of the Earth's rotation acting on the gyroscope; or measuring the object's orientation and position using the unmeasured pivot motion, the pivot angular velocity, and a distance traveled by the object starting from an initial orientation and position of the object, and compensating for the effect of the measured component of the Earth's rotation on the object's orientation and position. [Background technology]

[0002] In the prior art, north-finding systems are known for measuring the declination angle of an object from true north. In these north-finding systems, preferably, a gyroscope capable of sensing the rotation rate of the Earth is often used, as this provides a more independent measurement. From the declination angle of the object relative to true north, the orientation of the object can be determined.

[0003] 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 by taking into account the distance traveled by the object from its initial position and by taking into account any unmeasured pivoting of the object about its axis. For this application, it is recommended to compensate for any effect on the measurement of pivoting of the object caused by the rotational speed of the Earth.

[0004] The Earth rotates at a rate of only about 15 degrees per hour, and accurately and directly measuring this rate requires large, costly sensing devices, such as navigation-grade inertial navigation systems. To reduce the accuracy requirements of such gyros, sensor rotation techniques have been proposed over the decades. In these techniques, the accuracy of a gyroscope is increased by rotating the gyroscope at a known angular velocity about its pivot point during measurement. This technique is also known as carouseling (when the rotation is continuous) or indexing (when the rotation is incremental).

[0005] Modern automobiles employ satellite-based positioning systems for navigation. Currently, automobile navigation systems are primarily used to guide drivers on and off-road. However, as autonomous driving becomes possible, vehicle navigation systems will play an increasingly important role. As human influence over vehicle control decreases, the reliability of navigation, positioning, and orientation estimation becomes more important. [Prior art documents] [Non-patent literature]

[0006] [Non-Patent Document 1] H. Seraji, “Configuration control of redundant manipulators: theory and implementation,” (IEEE Transactions on Robotics and Automation), Vol. 5, No. 4, pp. 472-490, August 1989, Code: 10.1109 / 70.88062 [Non-patent document 2] D. Titterton, J. Weston, “Strapdown Inertial Navigation Technology” 2nd edition (Progress in Astronautics & Aeronautics), ISBN-13:9781563476938, January 2005, p. 283, Section 10.3.2, “Ground alignment methods” [Non-patent document 3] D. Titterton, J. Weston, “Strapdown Inertial Navigation Technology” 2nd edition (Progress in Astronautics & Aeronautics), ISBN-13:9781563476938, January 2005, “Ground alignment methods”, p. 31 “Local geographic navigation frame mechanization” Summary of the Invention [Problem to be solved by the invention]

[0007] However, 100% satellite signal coverage cannot be guaranteed in all environments due to limitations of the landscape around the vehicle. For example, availability and accuracy are reduced in urban canyons, on the ground such as in tunnels or mines, or on roads surrounded by obstructions such as tall trees or rocks.

[0008] To overcome situations where satellite signals are unavailable or of poor quality, it would be desirable to incorporate a gyroscope into the vehicle to enable measurement of the components of the Earth's rotation acting on the gyroscope. [Means for solving the problem]

[0009] This object according to the present disclosure is solved by a method for measuring at least one of an object's declination angle, orientation, and position, comprising: pivoting a gyroscope attached to the object at a pivot angular velocity about a pivot axis, measuring an unmeasured pivot of the object about an axis different from the pivot axis using the gyroscope, measuring a component of the Earth's rotation acting on the gyroscope using the unmeasured pivot motion and the pivot angular velocity, and measuring the object's declination angle relative to true north from the component of the Earth's rotation acting on the gyroscope, or measuring the object's orientation and position using the unmeasured pivot motion, the pivot angular velocity, and a distance traveled by the object starting from an initial orientation and position of the object, and compensating for the effect of the measured component of the Earth's rotation on the object's orientation and position. The object is a vehicle or part of a vehicle, the vehicle including a first vehicle portion and a second vehicle portion, the first vehicle portion pivotable relative to the second vehicle portion about the pivot axis, and the gyroscope is mechanically attached to the first vehicle portion. The method further includes measuring the pivot angular velocity of the gyroscope about the pivot axis using a second sensor distinct from the gyroscope.

[0010] The basic idea of ​​the present disclosure is to provide a gyroscope mounted on a vehicle, capable of measuring at least the declination or orientation of a vehicle or part of a vehicle, taking into account the component of the Earth's rotation acting on said gyroscope. To be able to measure the component of the Earth's rotation acting on said gyroscope, the gyroscope is pivoted about a pivot axis at a pivot angular velocity, and a separate sensor is used to measure this pivot.

[0011] The present disclosure utilizes a vehicle including two vehicle sections pivotable relative to one another about a pivot axis, the gyroscope being mechanically attached to one of the two vehicle sections, i.e., the first vehicle section, and pivoting the two vehicle sections relative to one another defines carouseling or indexing of the gyroscope.

[0012] By mounting the gyroscope on a first vehicle part that is pivoted relative to a second vehicle part, carouseling or indexing of the gyroscope can be utilized at a significantly lower cost and space-saving manner, thereby enabling efficient bias estimation of the gyroscope.

[0013] To utilize the pivotal motion of the gyroscope caused by the relative pivotal movement of the first and second vehicle portions about the pivot axis, a second sensor measures the pivotal angular velocity of the first vehicle portion about the pivot axis relative to the second vehicle portion. This second sensor is separate from the gyroscope. By measuring the relative pivotal angular velocity of the first vehicle portion about the second vehicle portion, the pivotal angular velocity of the gyroscope about the pivot axis is also measured. The separate second sensor provides a pivotal angular velocity that is not affected by Earth velocity.

[0014] The method according to the present disclosure can be used to measure at least one of the orientation or position of an object. Orientation in the sense of the present disclosure is part of the description of how the object is located in the space it occupies. Orientation refers to the virtual rotation required to move the object from a reference configuration to its current configuration. In contrast, the position of the object refers to the virtual translation required to move the object from a reference configuration to its current configuration. Orientation and position together completely describe how the object is located in space. According to the present disclosure, the orientation of the object is relative to true north.

[0015] Carouseling of a gyroscope according to the present disclosure is used to measure the component of the Earth's rotation acting on said gyroscope. The measured component of the Earth's rotation acting on a gyroscope according to the present disclosure is used in two different ways.

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

[0017] In one embodiment, the declination angle relative to true north is then used to determine the orientation of the object, which orientation according to the present disclosure is considered to include the declination angle relative to true north and information about at least one additional rotation about an additional pivot axis (necessary to align the object from a predetermined reference orientation to its current orientation).

[0018] Alternatively, the measured component of the Earth's rotation acting on the gyroscope is used to refine measurements of the object's orientation and position at the target orientation and the target position after the object has moved from a known initial orientation and a known initial position to the target orientation and target position. In this embodiment, the unmeasured pivotal motion of the object and the distance the object travels from the initial position to the target position (path length) are used to determine the object's orientation and position at the target position.

[0019] If the rotation of the Earth is not taken into account, deviations in the pivot angle will occur, and minimizing these deviations will provide more accurate information about heading.

[0020] In this embodiment, the unmeasured pivotal motion of the object and the distance the object has traveled 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. The distance traveled by the object is determined by multiplying the pivot angle of the turning axis by a known dimension of a portion of the vehicle. A method for measuring the distance traveled is described in U.S. Pat. No. 5,649,499.

[0021] Measurement of the object's orientation and position at the target location is affected by the Earth's rotation, which is superimposed on both the turning axis and the unmeasured turning axis. By redundantly measuring the turning pivot angle using a sensor separate from the gyro sensor, the effect of the Earth's rotation component can be compensated for on both axes. The sensor separate from the gyro sensor can be, for example, an accelerometer, a rotary encoder, a magnetometer, a visual sensor, or a light detection and ranging system. Such a sensor should be able to provide a pivot angle or pivot angular velocity that is not affected by the Earth's velocity.

[0022] In one embodiment of the present disclosure, the vehicle is a wheeled or tracked vehicle, and the first vehicle part is a wheel and the second vehicle part is the frame or body of the wheeled or tracked vehicle. In order to be able to infer the distance traveled using the wheel of the vehicle from the pivot, it is necessary to know the radius of the wheel. If the vehicle is a tracked vehicle, it is clear that the wheel in one embodiment is a wheel that guides a continuous track. In an alternative embodiment, the wheel is the steering wheel of the vehicle.

[0023] In a further embodiment of the present disclosure, the vehicle comprises a manipulator arm, the first vehicle part being a first arm section of the manipulator arm, and the second vehicle part being a second arm section of an excavator arm, or a frame or body of the vehicle.

[0024] Surprisingly, the use of a pivoting movement of a manipulator arm achieves similar results to the use of a rotating wheel (although the manipulator provides a pure pivoting movement of less than 360 degrees, not a full rotation). Furthermore, the use of a manipulator arm to provide the pivoting movement of said gyroscope allows the measurement of the manipulator arm or parts thereof as an object within the meaning of the present disclosure.

[0025] The above disclosure only describes carouseling and indexing based on pivotal movements of a manipulator providing a defined pivotal movement of the gyroscope in combination with measuring the components of the Earth's rotation. However, utilizing the pivotal movements of a vehicle's manipulator can be considered an invention in itself, even without measuring the components of the Earth's rotation acting on the gyroscope.

[0026] Accordingly, the present disclosure further relates to a method for measuring at least one of an orientation or a position of an object, the method comprising: pivoting a gyroscope attached to the object at a pivot angular velocity about a pivot axis; measuring an unmeasured pivot of the object about an axis different from the pivot axis using the gyroscope; and measuring the orientation and position of the object using the unmeasured pivot, the pivot angular velocity, and a distance traveled by the object starting from an initial orientation and initial position of the object. The object is a vehicle or part of a vehicle, the vehicle including a first vehicle portion and a second vehicle portion, the first vehicle portion pivotable relative to the second vehicle portion about the pivot axis, and the gyroscope mechanically attached to the first vehicle portion. The method further comprises measuring the pivot 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 part being a first arm section of the manipulator arm, and the second vehicle part being a second arm section of the manipulator arm, or a frame or body of the vehicle.

[0027] In one embodiment of the present disclosure, the vehicle with a manipulator arm is an excavator, and the manipulator arm is an excavator arm.

[0028] In one embodiment of the present disclosure, the second sensor is an acceleration sensor, the acceleration sensor including a known distance from the pivot axis.

[0029] In a further embodiment of the present disclosure, the pivoting is a rotation of 360 degrees or more. For example, it will be appreciated that the wheels of a wheeled or tracked vehicle will rotate through 360 degrees or more of a full rotation at a given but varying speed while the vehicle is in motion.

[0030] In a further embodiment of the present disclosure, the gyroscope is a microelectromechanical system (MEMS). Until now, MEMS-based gyroscopes have not been able to measure the components of the Earth's rotation acting on the gyroscope. However, with effective noise rejection and bias compensation (particularly provided by carouseling / indexing in accordance with the present disclosure), commercially available, inexpensive MEMS-based gyroscopes are available that can measure the components of the Earth's rotation.

[0031] In one embodiment of the present disclosure, the gyroscope and accelerometer form a six degrees of freedom measurement system, in which the accelerometer and gyroscope observe measurements from three non-parallel measurement axes (a so-called six degrees of freedom system).

[0032] Further advantages, features, and applications of the present disclosure will become apparent from the following description of the 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 is to be understood that the embodiments shown are not limited to the precise arrangements and apparatus shown. In the drawings, similar elements are designated by the same reference numerals. [Brief explanation of the drawings]

[0033] [Figure 1] FIG. 1 is a schematic side view of an embodiment of an excavator. [Figure 2] 1 is a diagram showing a change in a pivot angle over time, and the time derivative of the pivot angle is the pivot angular velocity. [Figure 3] 1 is a schematic diagram of different pivot angles and the corresponding deflection angles. [Figure 4] FIG. 2 is a schematic top view of the excavator of FIG. 1 in operation. DETAILED DESCRIPTION OF THE INVENTION

[0034] FIG. 1 is a schematic side view of an excavator 1 as an example of a vehicle in the sense of the present disclosure.

[0035] Reference numerals 101a, 101b, and 101c indicate MEMS-based gyroscopes located at different locations on the excavator. The gyroscopes are used to perform the method according to the present disclosure. Reference numerals 102a, 102b, and 102c indicate different parts of the excavator 1 that are considered objects in the present disclosure. Using the method described in the present disclosure, at least the orientation or position of each part 102a, 102b, and 102c of the excavator 1 is measured.

[0036] In summary, the excavator shown diagrammatically allows the following set-up of the gyroscopes 101a, 101b, 101c and the bodies 102a, 102b, 102c:

[0037] The gyroscope 101a is attached to a wheel 2 that guides a continuous track 3 of the excavator 1. The wheel 2 rotates about a pivot 4. The gyroscope 101a is used to measure the orientation and position of the chassis 102a of the excavator 1. The pivot angular velocity ω of the gyroscope 101a is carousel In order to measure the acceleration, an accelerometer 103a is attached to the wheel 2 as a second sensor.

[0038] The gyroscope 101b is mounted on a first arm section 102c of the manipulator arm 5. The manipulator arm 5 includes the first arm section 102c and a shovel 8 as a second arm section. The first manipulator arm section 102c is pivotable about a 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 body 102b of the excavator 1. The pivot angular velocity ω of the gyroscope 101b carousel A second sensor, an accelerometer 103b, is attached to the first manipulator arm section 102c to measure .

[0039] Furthermore, the shovel 8 is pivotable about a pivot axis 9 relative to the first manipulator arm section 102c. A gyroscope 101c is attached to the shovel 8. During operation, the excavator 1 undergoes a pivoting movement about the pivot axis 9 relative to the first manipulator arm section 102c. The pivoting angular velocity ω of the gyroscope 101c is carousel In order to measure the acceleration, an accelerometer 103c is attached to the shovel 8 as a second sensor.

[0040] 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.

[0041] The processing system uses the measurement signal ω from the gyroscope 101, the measurement signal a from the acceleration sensor 103b, and the initial orientation C0 as inputs, and provides the position p and orientation C as outputs f. That is,

number

[0042] The processing system allows estimation of the position of the chassis 102a via constraints imposed by the motion of the wheels 2, and can iteratively find the optimum C. If C is a direction cosine matrix, its derivative is given by

number

[0043] Once the effect of Earth velocity has been minimized, C already contains absolutely referenced orientation information. To clarify why a perfect orientation estimate is possible, it should be understood that both gravity and Earth velocity are observable in the body reference frame (i.e., a reference frame fixed to the rotating gyroscope 101a and accelerometer 103a) and known in the Earth frame, i.e., a reference frame fixed to the Earth / Moon / Mars. Knowing these two non-parallel vectors in this way means that perfect orientation information is obtained, as is known in the art. This is described, for example, in U.S. Patent No. 6,244,999. Earth velocity signals, affected by the Earth's rotation, slow the vehicle's rotation. According to the present disclosure, the direction of the Earth velocity signal is estimated by finding the direction in which Earth velocity introduces the least error into 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 acceleration calculation. Examples of pseudo-constant signal components are sensor biases due to MEMS processing or the Earth's rotational velocity. The joint angle dynamics and the dimensions of the vehicle wheels reveal 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 a quasi-stationary component. It is clear that the inertial navigation algorithm is very sensitive to certain offsets. For example, by looking at the acceleration derived from the inertial navigation algorithm and the acceleration derived from the joint angle dynamics measurements (also obtained by the inertial sensors, but using angles and vehicle dimensions), it is possible to identify the component caused by the Earth rotation. A minimization process finds the orientation that gives the best match between the angle-derived acceleration and the inertial navigation-derived acceleration. This is similar to the bias estimation for MEMS-based devices known from the prior art. The difference in acceleration based on the inertial calculation and the dimension-based calculation is minimized by trying different absolute orientations. This minimization can be performed in the position domain or the velocity domain (whichever is most suitable for computational convenience). Because the Earth velocity signal is very weak, the Earth velocity measurement is much more susceptible to noise than the bias estimation.However, observations over a longer period of time will reveal clear differences in acceleration, velocity, and / or position derived from an incorrect absolute orientation.

[0044] In the following examples, 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).

[0045] FIG. 2 shows the pivot angle of the swivel axis 7 calculated using inertial navigation formulas known in the prior art (e.g., U.S. Pat. No. 5,629,399). The asterisks indicate the results when an incorrect declination angle is fed into the inertial navigation mechanization algorithm. The circles indicate the results when a better declination angle estimate is fed into the algorithm. The deviation between these two is due to the influence of Earth velocity on the measurement. The diamonds indicate the pivot angle, which is calculated using the accelerometer-calculated position (using known dimensions) as external feedback to the inertial navigation mechanization system. FIG. 3 shows the difference between the incorrect declination angle and the feedback system result (asterisk) and the difference between the better declination angle and the feedback system result (circle). It can be seen that this difference is smaller for better declination angles, and therefore an algorithm aimed at minimizing this difference will resolve the declination angle. Also in FIG. 3, the corresponding declination angle is shown in a top view of the excavator's declination, with solid arrows indicating the true declination angle of FIG. 2 and the corresponding declination angle. The method for minimizing the difference can be brute force, or preferably a gradient search algorithm or an extended Kalman filter or modern machine learning methods. The feedback system can be an extended Kalman filter with position updates, as known in the art, where the position updates derived from the pivot angle and vehicle dimensions are considered external measurements of the system. Preferably, such a filter includes state estimation for gyroscope bias and can be an extended Kalman filter augmented with a neural network.

[0046] Figure 4 is a top view of the excavator 1 of Figure 1, showing the accumulated position history from the trajectory, e.g., deviation angle and distance traveled 401. Distance traveled 401 can be derived from known wheel radii and pivot angles. Another distance traveled 401 can be calculated from known dimensions of the manipulator arm 5 and known pivot angles using vector addition (e.g., shovel position relative to the center of the vehicle 402). Deviation angle is the angle 403 between true north N and the object's major axis 404.

[0047] It should be noted that, as can be easily understood by those skilled in the art, features described in connection with one embodiment may also be used in other embodiments. Although the present invention has been described in detail and with reference to the drawings, this description is merely exemplary and should not be considered as limiting the scope of protection defined by the claims. The known distance or radius may indicate such measurement information, for example, in mm, cm, or meter levels.

[0048] In the claims, the term "comprising" does not exclude other elements or steps, nor does the undefined article (a) exclude a plurality. The mere fact that several features are recited in different claims does not exclude their combination. Reference numerals in the claims should not be considered as limiting the scope of protection. [Explanation of symbols]

[0049] 1 Excavator 2 wheels 3 consecutive tracks 4, 7, 9 pivot axes 5 Manipulator Arm 6. First Arm Section 8. Shovel 101a, 101b, 101c Gyroscope 102a chassis 102b main body 102c First Arm Section 103a,103b,103c Accelerometer 401 Distance traveled 402 Shovel Position 403 The angle between true north and the major axis of an object 404 Principal Axis of an Object ω carousel angular velocity N True North

Claims

1. A method for measuring at least one of a declination angle, a heading, and a position of a vehicle (102a, 102b, 102c), comprising: the vehicle (102a, 102b, 102c) comprises a manipulator arm (5), a first arm portion (6) of the manipulator arm (5) being pivotable about a pivot axis (4, 7, 9) relative to a second arm portion of the manipulator arm (5) or relative to a frame or body of the vehicle (102a, 102b, 102c); The gyroscopes (101a, 101b, 101c) mechanically attached to the first arm portion (6) are rotated about the pivot axes (4, 7, 9) at a pivot angular velocity (ω ｃａｒｏｕｓｅｌ ) pivoting the The component of the Earth's rotation acting on the gyroscopes (101a, 101b, 101c) is defined as the pivotal angular velocity (ω ｃａｒｏｕｓｅｌ ) measuring the measuring the declination angle of the vehicle (102a, 102b, 102c) relative to true north from the component of the Earth's rotation acting on the gyroscope (101a, 101b, 101c); or The orientation and position of the vehicle (102a, 102b, 102c) are calculated by calculating the pivot angular velocity (ω) starting from an initial orientation and initial position of the vehicle (102a, 102b, 102c). ｃａｒｏｕｓｅｌ ), and the distance traveled by the vehicle (102a, 102b, 102c), and compensating for the effect of the measured components of the Earth's rotation on the orientation and position of the vehicle (102a, 102b, 102c); The pivotal angular velocity (ω) of the gyroscope (101a, 101b, 101c) about the pivot axis (4, 7, 9) ｃａｒｏｕｓｅｌ ) using a second sensor (103a, 103b, 103c) different from the gyroscope (101a, 101b, 101c).

2. 2. The method of claim 1, wherein the vehicle is a wheeled or tracked vehicle, the first arm portion (6) is a wheel, the wheel comprising a known wheel radius, and the second arm portion is a frame or body of the wheeled vehicle.

3. 3. The method of claim 1, wherein the second sensor is an acceleration sensor, the acceleration sensor being positioned a known distance from the pivot axis.

4. The method of any one of claims 1 to 3, wherein the pivoting is a rotation of 360 degrees or more.

5. The method of any one of claims 1 to 4, wherein the gyroscope is a microelectromechanical system.

6. The method of claim 3 , wherein the gyroscope and the acceleration sensor form a six-degree-of-freedom measurement system.

Images