Walking stick navigator for position determination

• US 7,043,364 B2
• Filed: 01/25/2005
• Issued: 05/09/2006
• Est. Priority Date: 12/03/2001
• Status: Active Grant

• Current Assignee: Applanix Corporation, Inc.
• Original Assignee: Applanix Corporation, Inc.
• Inventors: Scherzinger, Bruno
• Primary Examiner(s): Zanelli, Michael J.
• Application Number:
  US11/043,872
• Publication Number:
  US 20050203701A1
• Time in Patent Office: 469 Days
• Field of Search: 701/207, 701/213, 701/216, 701/220, 342/357.06, 342/357.14, 702/5, 702/160
• US Class Current: 701/213
• 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, and a top 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 GPS receiver and antenna attached to the staff, the GPS receiver being coupled to provide a first aiding signal to the AINS,

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 a second 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 {right arrow over (z)}_SNV-PP by the steps of:

calculating a first relative IMU position vector {right arrow over (ρ

)}₁ⁿ at the beginning of the contact signal and a second relative IMU position vector {right arrow over (ρ

)}₂ⁿ at the conclusion of the contact signal and

calculating a time synchronized relative IMU displacement vector Δ

{right arrow over (ρ

)}_1-2ⁿ from the difference between the second relative IMU position vector {right arrow over (ρ

)}₂ⁿ and the first relative IMU position vector {right arrow over (ρ

)}₁ⁿ,

obtaining the inertial navigation solution displacement vector Δ

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

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

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

{right arrow over (ρ

)}_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 {right arrow over (ν

)}_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 {right arrow over (ν

)}_GR-IMUⁿ from an equivalent inertial navigator velocity vector {right arrow over (ν

)}_SNVⁿ obtained from the AINS.

4. The WSN of claim 1 wherein the

Global Position Satellite (GPS) receiver and antenna are mounted on the top end of the staff to provide 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 wherein the GPS receiver attached to the staff assembly provideds 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 first GPS receiver positioned to be close to the top end of the staff, the first GPS receiver being coupled to provide a first aiding signal to the AINS, and

a second GPS receiver positioned to be close to the bottom end of the staff, the second GPS receiver being coupled to provide a second aiding signal to the AINS,

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 AINS further comprises a Kalman filter designed be responsive to the position increment measurement vector {right arrow over (z)}_SNV-PP for each stationary period for estimating and regulating position and velocity vector errors to obtain a low position error drift.

10. 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 {right arrow over (z)}_SNV-PP during a portion of each step for each respective stationary interval, to the AINS for controlling position error drift.

11. The WSN of claim 9 wherein the Kalman filter computes the position increment measurement vector {right arrow over (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 {right arrow over (ρ

)}₁ⁿ and

a second relative inertial measurement unit (IMU) position vector at {right arrow over (ρ

)}₂ⁿ at times t1 and t2 as follows;

{right arrow over (ρ

)}₁ⁿ−

C_bⁿ(t₁){right arrow over (l)}_IMU-GR^b

{right arrow over (ρ

)}₂ⁿ=C_bⁿ(t₂){right arrow over (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;

Δ

{right arrow over (ρ

)}_1-2ⁿ={right arrow over (ρ

)}₂ⁿ−

{right arrow over (ρ

)}₁ⁿ

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

{right arrow over (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><br><br>d<br><br>t$

where {right arrow over (ν

)}_SNVⁿ is an inertial navigator velocity vector resolved in an INS navigation frame, and

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

{right arrow over (ρ

)}_1-2ⁿ and the inertial navigation solution displacement vector Δ

{right arrow over (r)}_SNV1-2ⁿ as;

{right arrow over (z)}_SNV-PP=Δ

{right arrow over (r)}_SNV1-2ⁿ−

Δ

{right arrow over (ρ

)}_1-2ⁿ

whereby the Kalman filter uses the position increment measurement vector {right arrow over (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 11 wherein the position aiding algorithm further comprises:

a velocity measurement program responsive to the AINS output signals for providing a relative IMU velocity vector {right arrow over (ν

)}_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 {right arrow over (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 {right arrow over (z)}_SNV-ZV for each stationary interval by the following steps:

1. the Kalman filter computes the relative IMU velocity vector {right arrow over (ν

)}_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;

${\stackrel{\_}{\mathrm{\nu <br><br>}}}_{\mathrm{GR}-\mathrm{IMU}}^n=C_b^n<br><br>\left({\stackrel{\_}{\mathrm{\omega <br><br>}}}_{\mathrm{IMU}}^b⨯<br><br>{\stackrel{\_}{l}}_{\mathrm{GR}-\mathrm{IMU}}^b\right)=-C_b^n<br><br>\left({\stackrel{\_}{\mathrm{\omega <br><br>}}}_{\mathrm{IMU}}^b⨯<br><br>{\stackrel{\_}{l}}_{\mathrm{IMU}-\mathrm{GR}}^b\right)$

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 {right arrow over (ω

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

{right arrow over (z)}_SNV-ZV={right arrow over (ν

)}_SNVⁿ−

{right arrow over (ν

)}_GR-IMUⁿ

whereby the Kalman filter uses the ZUPD velocity measurement vector {right arrow over (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 {right arrow over (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 GPS receiver coupled to a top end of the shaft, the GPS receiver being coupled to provide a first aiding signal to the AINS,

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 signal 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 position increment measurement signal is a position increment measurement vector {right arrow over (z)}_SNV-PP, and wherein,

the AINS further comprises a Kalman filter designed to be responsive to the position increment measurement vector {right arrow over (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 {right arrow over (z)}_SNV-PP for each stationary interval by the following four steps:

step 1. the Kalman filter computes a first relative IMU position vector {right arrow over (ρ

)}₁ⁿ and a second relative IMU position vector {right arrow over (ρ

)}₂ⁿ at times t1 and t2 as follows;

{right arrow over (ρ

)}₁ⁿ=C_bⁿ(t₁){right arrow over (l)}_IMU-GR^b

{right arrow over (ρ

)}₂ⁿ=C_bⁿ(t₂){right arrow over (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 Δ

{right arrow over (ρ

)}_1-2ⁿ after time t2 using the following equation;

Δ

{right arrow over (ρ

)}_1-2ⁿ={right arrow over (ρ

)}₂ⁿ−

{right arrow over (ρ

)}₁ⁿ

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

{right arrow over (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><br><br>d<br><br>t$

where {right arrow over (ν

)}_SNVⁿ is an inertial navigator velocity vector resolved in an INS navigation frame,

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

{right arrow over (ρ

)}_1-2ⁿ and the inertial navigation solution displacement vector Δ

{right arrow over (r)}_SNV1-2ⁿ as;

{right arrow over (z)}_SNV-PP=Δ

{right arrow over (r)}_SNV1-2−

Δ

{right arrow over (ρ

)}_1-2ⁿ

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