Walking stick navigator for position determination

• US 20050203701A1
• Filed: 01/25/2005
• Published: 09/15/2005
• Est. Priority Date: 12/03/2001
• Status: Active Grant

• Current Assignee: Bruno Scherzinger
• Original Assignee: Bruno Scherzinger
• Inventors: Scherzinger, Bruno
• Granted Patent: US 7,043,364 B2
• Time in Patent Office: Days
• US Class Current: 701/207
• CPC Class Codes: G01C 15/00 Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00

Abstract

A walking stick navigator (WSN) apparatus and process comprises an aided INS (AINS) on a staff assembly similar in operation and appearance to a GPS survey instrument. However, when GPS is available, the AINS is aided by GPS data, and the surveyor manipulates the staff assembly like a standard GPS survey instrument. When GPS is not available due to signal obstruction, the surveyor manipulates the staff assembly as a walking stick. A switch means coupled to the lower end of the staff assembly provides a stationary interval signal when the surveyor plants and holds the WSN on the ground while walking. An input process is coupled to the AINS output signals and to the stationary interval signals and provides at least one aiding input signal to the AINS for each successive stationary interval, thereby allowing the AINS to control its velocity error and position drift during a GPS outage.

Claims

1. A walking stick navigator (WSN) on a staff having a lower end, the WSN being carried by a surveyor and comprising:

an Aided Inertial Navigation System (AINS) coupled to the staff, the AINS having an IMU providing inertial outputs,

a switch coupled to the lower end to provide a contact signal indicating when the lower end of the staff assembly is stationary, the contact signal defining the duration of a stationary interval,

a digital computer running a position measurement program solving a position aiding algorithm responsive to the IMU outputs and the contact signal, the position measurement program providing an aiding signal to the AINS.

2. The WSN of claim 1 wherein the position measurement program further comprises the step of using an algorithm for calculating a position increment measurement vector {overscore (z)}_SNV-PP by the steps of:

calculating a first relative IMU position vector {overscore (p)}₁ⁿ at the beginning of the contact signal and a second relative IMU position vector {overscore (p)}₂ⁿ at the conclusion of the contact signal and

calculating a time synchronized relative IMU displacement vector Δ

{overscore (p)}_1-2ⁿ from the difference between the second relative IMU position vector {overscore (p)}₂ⁿ and the first relative IMU position vector {overscore (p)}₁ⁿ,

obtaining the inertial navigation solution displacement vector Δ

{overscore (r)}_SNV1-2ⁿ for the contact signal interval from the inertial navigation system and

calculating the position increment measurement vector {overscore (z)}_SNV-PP from the difference between the inertial navigation solution displacement vector Δ

{overscore (r)}_SNV1-2ⁿ and the time synchronized relative IMU displacement vector Δ

{overscore (p)}_1-2ⁿ.

3. The WSN of claim 1 wherein the position measurement program further comprises:

a Kalman filter having an iteration rate, and an algorithm for calculating a zero-velocity update (ZUPD) measurement having

an IMU-to-Ground switch Relative Lever Arm (IGRLA) vector input defining the location of an IMU on the shaft assembly with respect to the lower end of the shaft assembly, the algorithm calculating a relative IMU velocity vector {overscore (v)}_GR-IMUⁿ with respect to stationary ground at each Kalman filter cycle time from the beginning of the contact signal until the end of the contact signal by taking the cross product of an angular rate of the IMU with the IGRLA vector while in contact with the ground, and multiplying each respective cross product by the direction cosine matrix for a body to the navigational reference system, the program then constructing the ZUPD measurement by subtracting the relative IMU velocity vector {overscore (v)}_GR-IMUⁿ from an equivalent inertial navigator velocity vector {overscore (v)}_SNVⁿ obtained from the AINS.

4. The WSN of claim 1 further comprising:

a Global Position Satelite (GPS) receiver providing acceptable GPS position aiding signals to the AINS, the surveyor moving along a path to be surveyed, the surveyor carrying the staff assembly with the lower end above the ground during intervals in which acceptable GPS position aiding signals to the AINS are available.

5. The WSN of claim 1 comprising a GPS receiver coupled to the staff assembly provide a GPS aiding signal to the AINS as the surveyor moves along a path to be surveyed, the surveyor carrying the staff assembly without contact with the ground during intervals when an acceptable GPS signal is available, the AINS being aided by GPS data, and during intervals in which an acceptable GPS aiding signal is not available, the surveyor manipulating the staff assembly as a walking stick while the surveyor is walking, the surveyor bringing the lower end of the shaft assembly into contact with the ground, the switch providing the contact signal, the position measurement program being responsive to the contact signal and coupled to the AINS output signals to provide at least one aiding input signal to the AINS for each successive stationary interval.

6. A walking stick navigator (WSN) apparatus comprising:

a staff assembly having a lower end and a top end, the staff assembly being carried by a surveyor moving along a path to be surveyed, the surveyor positioning the lower end of the staff assembly at a stationary point on the ground at a start of a stride, and pivoting the staff assembly around the stationary point in the direction of surveyor movement, the surveyor lifting the staff assembly and repositioning the lower end of the staff assembly to a stationary point in the direction of surveyor movement at the conclusion of the stride,

a sequence being repeated with each successive stride interval,

an Aided Inertial Navigation System (AINS) coupled to and aligned on the staff assembly, the AINS system providing output signals comprising position, velocity and platform angle signals,

a switch means coupled to the lower end of the staff assembly for providing stationary interval signals characterizing each successive stationary interval, and

a digital computer coupled to be responsive to AINS output signals and to the stationary interval signals running a program solving a position aiding algorithm providing at least one aiding input to the AINS for each successive stationary interval.

7. The WSN of claim 6 wherein the switch means further comprises:

a micro-switch switch means coupled to the lower end of the staff assembly and characterized to provide the stationary interval signal during the period that the lower end is in contact with the ground.

8. The WSN of claim 6 wherein the switch means further comprises:

a spring restored plunger switch having a frame coupled to the lower end of the staff assembly, the frame having a cylindrical hole, and a spring restored plunger residing therein, the plunger being transferred into the cylindrical hole by contact with the ground, the motion of the plunger transferring an electrical contact to provide the stationary interval signal.

9. The WSN of claim 6 wherein the program solving a position aiding algorithm further comprises:

a position measurement program responsive to the AINS output signals for providing a position increment measurement vector {overscore (z)}_SNV-PP during a portion of each step for each respective stationary interval, to the AINS for controlling position error drift.

10. The WSN of claim 6 wherein the AINS further comprises a Kalman filter designed be responsive to the position increment measurement vector {overscore (z)}_SNV-PP for each stationary period for estimating and regulating position and velocity vector errors to obtain a low position error drift.

11. The WSN of claim 9 wherein the Kalman filter computes the position increment measurement vector {overscore (z)}_SNV-PP for each stationary interval by the following four steps:

1. the Kalman filter computes

a first relative inertial measurement unit (IMU) position vector {overscore (p)}₁ⁿ and

a second relative inertial measurement unit (IMU) position vector {overscore (p)}₂ⁿ at times t1 and t2 as follows;

{overscore (p)}₁ⁿ=C_bⁿ(t₁){overscore (l)}_IMU-GR^b
{overscore (p)}₂ⁿ=C_bⁿ(t₂){overscore (l)}_IMU-GR^b

in which C_bⁿ is a direction cosine matrix (DCM) from the b-frame or IMU body frame to the n-frame or INS navigation frame,

2. the Kalman filter computes a time synchronized relative IMU displacement vector after time t2 as follows;

Δ

{overscore (p)}_1-2ⁿ={overscore (p)}₂ⁿ={overscore (p)}₁ⁿ

3. the Kalman filter computes an inertial navigation solution displacement vector Δ

{overscore (r)}_SNV1-2ⁿ for the interval from t1 to t2 as follows;

$\mathrm{\Delta <br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{SNV1}<br><br>\text{-}<br><br>2}^n={\int <br><br>}_{t_1}^{t_2}<br><br>{\stackrel{\rightharpoonup <br><br>}{v}}_{\mathrm{SNV}}^n<br><br>\text{\hspace{1em}<br><br>}<br><br>d<br><br>t$

where {overscore (v)}_SNVⁿ is an inertial navigator velocity vector resolved in an INS navigation frame, and

4. the Kalman filter computes the position increment measurement vector {overscore (z)}_SNV-PP by taking the difference between the time synchronized relative IMU displacement vector Δ

{overscore (p)}_1-2ⁿ and the inertial navigation solution displacement vector Δ

{overscore (r)}_SNV1-2ⁿ as;

{overscore (z)}_SNV-PP=Δ

{overscore (r)}_SNV1-2ⁿ−

Δ

{overscore (p)}_1-2ⁿ

whereby the Kalman filter uses the position increment measurement vector {overscore (z)}_SNV-PP to estimate and regulate position and velocity errors to be nearly zero and to obtain a low position error drift rate.

12. The WSN of claim 111 wherein the position aiding algorithm further comprises:

a velocity measurement program responsive to the AINS output signals for providing a relative IMU velocity vector {overscore (v)}_GR-IMU to the AINS during a portion of each step for each respective stationary interval to control the position error drift.

13. The WSN of claim 12 wherein the AINS comprises:

a Kalman filter designed to be responsive to a ZUPD velocity measurement vector {overscore (z)}_SNV-ZV for each stationary interval for estimating and regulating position and velocity vector errors to obtain a low position error drift.

14. The WSN of claim 13 wherein the Kalman filter computes the ZUPD velocity measurement vector {overscore (z)}_SNV-ZV for each stationary interval by the following steps:

1. the Kalman filter computes the relative IMU velocity vector {overscore (v)}_GR-IMUⁿ with respect to the stationary ground reference point at each Kalman filter cycle time between times t1 and t2 via the following equation;

{overscore (v)}_GR-IMUⁿ=C_bⁿ({overscore (ω

)}_IMU^b×

{overscore (l)}_GR-IMU^b)=−

C_bⁿ({overscore (ω

)}_IMU^b×

{overscore (l)}_IMU-GR^b)

in which C_bⁿ is a direction cosine matrix (DCM) from the b-frame or IMU body frame to the n-frame or INS navigation frame, and where {overscore (ω

)}_IMU^b is an angular rate of the IMU corrected for Earth rate, and wherein

{overscore (z)}_SNV-ZV={overscore (v)}_SNVⁿ−

{overscore (v)}_GR-IMUⁿ

whereby the Kalman filter uses the ZUPD velocity measurement vector {overscore (z)}_SNV-ZV to estimate and regulate position and velocity errors to be nearly zero and to obtain a low position error drift.

15. The WSN of claim 14 wherein the Kalman filter is characterized to calculate the ZUPD velocity measurement vector {overscore (z)}_SNV-ZV with an iteration rate of at least 10 iterations per second during the stationary interval between the time t1 when the surveyor plants the WSN and the ground switch closes and the time t2 when the surveyor lifts the WSN and the ground switch opens.

16. A walking stick navigator (WSN) on a shaft assembly, a surveyor carrying the shaft assembly and positioning a shaft lower end to be in contact with the ground in front of the surveyor marking the start of

a stationary interval, the surveyor

pivoting the shaft assembly in the direction of his movement, and

raising the shaft assembly interrupting the shaft lower end being in contact with the ground at the conclusion of each step

marking the end of the stationary interval, the WSN comprising:

an AINS having an IMU providing output signals developed from the outputs of a plurality of inertial sensors,

a digital computer running a position measurement aiding program, the digital computer being responsive to the AINS output signals for providing a position increment measurement vector {overscore (z)}_SNV-PP to the AINS during a portion of each step for each respective stationary interval to control position error drift.

17. The WSN of claim 16 comprising:

a switch means coupled to the shaft assembly for providing a stationary interval signal to the AINS characterizing the interval during which the shaft lower end is in contact with the ground.

18. The WSN of claim 16 wherein the AINS further comprises a Kalman filter designed to be responsive to the position increment measurement vector {overscore (z)}_SNV-PP for each stationary period for estimating and regulating position and velocity vector errors to obtain a low position error drift.

19. The WSN of claim 18 wherein the Kalman filter computes the position increment measurement vector {overscore (z)}_SNV-PP for each stationary interval by the following four steps:

step 1. the Kalman filter computes a first relative IMU position vector {overscore (p)}₁ⁿ and a second relative IMU position vector {overscore (p)}₂ⁿ at times t1 and t2 as follows;

{overscore (p)}₁ⁿ=C_bⁿ(t₁){overscore (l)}_IMU-GR^b
{overscore (p)}₂ⁿ=C_bⁿ(t₂){overscore (l)}_IMU-GR^b

in which C_bⁿ is a direction cosine matrix (DCM) from the b-frame or IMU body frame to the n-frame or INS navigation frame, and step 2. the Kalman filter computes a time synchronized relative IMU displacement vector Δ

{overscore (p)}_1-2ⁿ after time t2 using the following equation;

Δ

{overscore (p)}_1-2ⁿ={overscore (p)}₂ⁿ−

{overscore (p)}₁ⁿ

step 3. the Kalman filter computes an inertial navigation solution displacement vector Δ

{overscore (r)}_SNV1-2ⁿ for the interval from t1 to t2 as follows;

$\mathrm{\Delta <br><br>}<br><br>{\stackrel{\rightharpoonup <br><br>}{r}}_{\mathrm{SNV1}<br><br>\text{-}<br><br>2}^n={\int <br><br>}_{t_1}^{t_2}<br><br>{\stackrel{\rightharpoonup <br><br>}{v}}_{\mathrm{SNV}}^n<br><br>\text{\hspace{1em}<br><br>}<br><br>d<br><br>t$

where {overscore (v)}_SNVⁿ is an inertial navigator velocity vector resolved in an INS navigation frame,

step 4. the Kalman filter computes the position increment measurement vector {overscore (z)}_SNV-PP by taking the difference between the time synchronized relative IMU displacement vector Δ

{overscore (p)}_1-2ⁿ and the inertial navigation solution displacement vector Δ

{overscore (r)}_SNV1-2ⁿ as;

{overscore (z)}_SNV-PP=Δ

{overscore (r)}_SNV1-2ⁿ−

Δ

{overscore (p)}_1-2ⁿ

whereby the Kalman filter uses the position increment measurement vector {overscore (z)}_SNV-PP to estimate and regulate position and velocity errors to be nearly zero and to obtain a low position error drift rate.