ARRANGEMENT FOR AND METHOD OF TWO DIMENSIONAL AND THREE DIMENSIONAL PRECISION LOCATION AND ORIENTATION DETERMINATION

• US 20100039318A1
• Filed: 11/06/2006
• Published: 02/18/2010
• Est. Priority Date: 11/06/2006
• Status: Active Grant

• Current Assignee: Tomtom Global Content B.V (TomTom NV)
• Original Assignee: Tomtom Global Content B.V (TomTom NV)
• Inventors: Kmiecik, Marcin Michal, Miksa, Krzysztof
• Granted Patent: US 8,326,532 B2
• Time in Patent Office: Days
• US Class Current: 342/357.590
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G01C 21/28 with correlation of data from several navigational instruments
• CPC Class Codes: G01S 19/23 Testing, monitoring, correcting or calibrating of receiver elements
• CPC Class Codes: G01S 19/47 the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Abstract

In one embodiment of the present invention, a method of and apparatus for determining inaccurate GPS samples in a set of GPS samples is disclosed, according to the following actions: a) obtaining GPS samples as taken by a global positioning system on board a vehicle when traveling along a trajectory; b) obtaining a first estimation of the trajectory based on the GPS samples; c) obtaining a second estimation of the trajectory at least based on measurements made by an inertial measurement unit on board vehicle when traveling along the trajectory; d) comparing the first and second estimations; e) establishing locations where the first estimation shows a variation compared with the second estimation above a predetermined threshold; f) if no such locations can be established continue with action j), otherwise continue with action g); g) removing GPS samples associated with the locations of high variation as being inaccurate GPS samples, thus forming a set of remaining GPS samples; h) calculating the first estimation anew of the trajectory based on the remaining GPS samples and calculating the second estimation anew; i) repeating actions d) to h); j) ending the actions.

Claims

1. Computer arrangement for determining inaccurate global positioning system samples in a set of global positioning system samples, comprising a processor arranged to communicate with a memory storing a computer program comprising instructions and data that can be run by the processor, said processor being arranged to perform at least the following actions when it runs said computer program:

a) obtaining global positioning system samples as taken by a global positioning system on board a land based vehicle when traveling along a trajectory;

b) obtaining a first estimation of said trajectory based on said global positioning system samples;

c) obtaining a second estimation of said trajectory at least based on measurements made by an inertial measurement unit on board said land based vehicle when traveling along said trajectory;

d) comparing said first and second estimations;

e) establishing locations where said first estimation shows a variation compared with said second estimation above a predetermined threshold;

f) if no such locations can be established continue with action j), otherwise continue with action g);

g) removing global positioning system samples associated with said locations of high variation as being inaccurate global positioning system samples, thus forming a set of remaining global positioning system samples;

h) calculating said first estimation anew of said trajectory based on said remaining global positioning system samples and calculating said second estimation anew;

i) repeating actions d) to h); and

j) ending said actions.

2. Computer arrangement according to claim 1, wherein said computer arrangement is arranged to test in action j) before ending said actions whether a predetermined criterion is met, and if said predetermined criterion is not met to repeat actions d) to h), said predetermined criterion including at least one of:

a minimum distance traveled by said vehicle and a minimum time period of travel of the vehicle.

3. Computer arrangement according to claim 2, wherein processor is arranged to check said predetermined criterion of minimum distance traveled by said vehicle based on distance measurements made by a distance measurement instrument on board said vehicle.

4. Computer arrangement according to claim 2, wherein said minimum distance is 100 m.

5. Computer arrangement according to claim 1, wherein said second estimation of said trajectory is also based on measurements made by a distance measurement instrument on board said land based vehicle when traveling along said trajectory.

6. Computer arrangement according to claim 1, wherein said second estimation of said trajectory is also based on measurements as to direction of travel by said vehicle as measured by said global positioning system on board said land based vehicle when traveling along said trajectory.

7. Computer arrangement according to claim 1, wherein said second estimation of said trajectory is also based on measurements as to speed of travel by said vehicle as measured by said global positioning system on board said land based vehicle when traveling along said trajectory.

8. Computer arrangement according to claim 1, wherein said processor (is arranged to perform at least the following actions any time before it performs action d) when it runs said computer program:

obtaining a first parameter as a function of time associated with a portion of said trajectory traveled by said vehicle, said first parameter being one of a group of parameters comprising;

x value, y value, z value, orientation in xy plane, roll and pitch, as defined in an xyz system, said first parameter being obtained from a first measurement system on board said vehicle while traveling along said portion of said trajectory;

obtaining a second parameter as a function of time associated with said portion of said trajectory traveled by said vehicle, said second parameter being an equivalent of said first parameter but obtained from said measurements made by said inertial measurement unit;

comparing said first and second parameters as a function of time and deducing from said comparing a measurement error in said second parameter in said portion of said trajectory; and

compensating an error in said second estimation of said trajectory due to said measurement error in said second parameter.

9. Computer arrangement according to claim 8, wherein said first measurement system is said global positioning system.

10. Computer arrangement according to claim 8, wherein said measurement error relates to at least one of drift and shifts in said measurements made by said inertial measurement unit.

11. Computer arrangement according to claim 1, wherein said processor is arranged to, in said action of obtaining said second estimation of said trajectory, calculate successive locations of said trajectory and interpolating between said locations while using a clothoide shaped approximation between said locations.

12. Computer arrangement according to claim 1, wherein the processor is arranged to calculate an orientation of said vehicle in an xy plane when traveling along said trajectory as function of time, said xy plane being defined locally by an x-axis and an y-axis, said x-axis being along said trajectory and said y-axis being perpendicular to said x-axis and to a local gravity vector, while performing at least the following actions:

estimating a first orientation signal as a function of time based on said remaining global positioning system samples;

estimating a second orientation signal as a function of time based on orientation measurements made by said inertial measurement unit;

comparing the first orientation signal and the second orientation signal as a function of time;

deriving an error in said second orientation signal as a function of time based on said comparing action;

compensating said error in said second orientation signal thus rendering an error compensated second orientation signal; and

calculating said orientation of said vehicle as a function of time along the traveled trajectory based on the first orientation signal and the error compensated second orientation signal.

13. Computer arrangement according to claim 12, wherein said processor is arranged to calculate a first estimation of at least one of a pitch and roll of said vehicle while traveling along said trajectory as a function of time, in accordance with at least the following actions:

obtaining measurement data from said inertial measurement unit as to movement accelerations;

obtaining speed measurement data in said x-direction from a distance measurement unit on board said vehicle;

deriving movement accelerations of the vehicle in said x-direction; and

removing the movement acceleration from the measurements made by the IMU system, rendering the acceleration.

14. Computer arrangement according to claim 13, wherein said processor is arranged to calculate an approximation of at least one of pitch and roll of said vehicle while traveling along said trajectory as a function of time, while performing at least the following actions:

calculating;

{right arrow over (a)}_x=d{right arrow over (v)}_x/dt 

 

(9)

{right arrow over (a)}_y={right arrow over (ω

)}_z×

{right arrow over (v)}_x 

 

(10)

where;

{right arrow over (a)}_x=acceleration in the x-direction;

{right arrow over (v)}_x=velocity in the x-direction as derived from the distance measurement unit;

{right arrow over (a)}_y=acceleration as measured in the y-direction;

{right arrow over (ω

)}_z=speed of heading change;

calculating;

{right arrow over (a)}_z={right arrow over (ω

)}_y×

{right arrow over (v)}_x 

 

(11)

where;

{right arrow over (a)}_z equals the acceleration in a direction opposite to the local gravity vector;

{right arrow over (ω

)}_y equals the pitch of the vehicle relative to the local gravity vector.

calculating;

{right arrow over (a)}_{ST x}={right arrow over (a)}_{IMU x}−

{right arrow over (a)}_x 

 

(12)

{right arrow over (a)}_{ST y}={right arrow over (a)}_{IMU y}−

{right arrow over (a)}_y 

 

(13)

{right arrow over (a)}_{ST z}={right arrow over (a)}_{IMU z}−

{right arrow over (a)}_z 

 

(14)

where;

{right arrow over (a)}_{IMU x}=acceleration measured by the inertial measurement unit in x axis

{right arrow over (a)}_{ST x}=stationary acceleration in x axis

{right arrow over (a)}_{IMU y}=acceleration measured by the inertial measurement unit in y axis

{right arrow over (a)}_{ST y}=stationary acceleration in y axis

{right arrow over (a)}_{IMU z}=acceleration measured by the inertial measurement unit in z axis

{right arrow over (a)}_{ST z}=stationary acceleration in z axis

calculating at least one of;

$\begin{array}{cc}\sin <br><br>\left({\mathrm{pitch}}_{\mathrm{app}}\right)=\frac{a_{\mathrm{STx}}}{\sqrt{a_{\mathrm{STx}}^2+a_{\mathrm{STy}}^2+a_{\mathrm{STz}}^2}}& \left(15\right)\\ \sin <br><br>\left({\mathrm{roll}}_{\mathrm{app}}\right)=\frac{a_{\mathrm{STy}}}{\sqrt{a_{\mathrm{STx}}^2+a_{\mathrm{STy}}^2+a_{\mathrm{STz}}^2}}& \left(16\right)\end{array}$

where;

pitch_app=approximation of pitch

roll_app=approximation of roll.

15. Computer arrangement according to claim 14, wherein said processor is arranged to calculate an estimation of said at least one of pitch and roll from said measurements made by said inertial measurement unit, to compare said approximation of said at least one of pitch and roll with said estimation of said at least one of said pitch and roll and to compensate errors in said estimation of said at least one of pitch and roll based on said comparison, in order to render at least one of a true pitch, defined as an error compensated pitch, and a true roll, defined as an error compensated roll.

16. Computer arrangement according to claim 14, wherein the processor is arranged to calculate a slope angle slope_angle of said vehicle while travelling along said trajectory relative to a local gravity vector ({right arrow over (g)}_loc) in accordance with:

slope_angle=arcsin(υ

_z/υ

_x) 

 

(17)

where;

υ

_z=velocity in a direction opposite to said local gravity vector calculated from;

{right arrow over (v)}_z=∫

{right arrow over (a)}_STz·

dt 

 

(18)

17. Computer arrangement according to claim 15, wherein the processor is arranged to calculate a slope angle slope_angle of said vehicle while travelling along said trajectory relative to a local gravity vector (g_loc) in accordance with:

slope_angle=arcsin(υ

_z/υ

_x) 

 

(17)

υ

_z=υ

_x·

sin(pitch_true−

k·

a_x) 

 

(19)

where;

υ

_z=velocity in a direction opposite to said local gravity vector;

pitch_true=true pitch;

a_x=acceleration in x-direction as derived from the distance measurement unit;

k=a constant.

18. Computer arrangement according to claim 16, wherein the processor is arranged to:

calculate;

υ

_z=υ

_x·

sin(pitch_true−

k·

a_x) 

 

(19)

where;

pitch_true=true pitch;

a_x=acceleration in x-direction as derived from the distance measurement unit;

k=a constant;

use equation (19) to remove errors from υ

_z as obtained by equation (18) rendering an error compensated vertical speed value υ

<sub>z,err—

comp</sub>;

substitute υ

_z in equation (17) by the error compensated vertical speed υ

<sub>z,err—

comp</sub>.

19. Computer arrangement according to claim 16, wherein the processor is arranged to calculate a z-level z(t) in a direction opposite to said local gravity vector ({right arrow over (g)}_loc) from:

z(t)=∫

v_x(t)·

sin(slope_angle(t)) 

 

(20)

20. Method of determining inaccurate global positioning system samples in a set of global positioning system samples, said method comprising:

a) obtaining global positioning system samples as taken by a global positioning system on board a vehicle when traveling along a trajectory;

b) obtaining a first estimation of said trajectory based on said global positioning system samples;

c) obtaining a second estimation of said trajectory at least based on measurements made by an inertial measurement unit on board said vehicle when traveling along said trajectory;

d) comparing said first and second estimations;

e) establishing locations where said first estimation shows a variation compared with said second estimation above a predetermined threshold;

f) if no such locations can be established continue with action j), otherwise continue with action g);

g) removing global positioning system samples associated with said locations of high variation as being inaccurate global positioning system samples, thus forming a set of remaining global positioning system samples;

h) calculating said first estimation anew of said trajectory based on said remaining global positioning system samples and calculating said second estimation anew;

i) repeating actions d) to h); and

j) ending said actions.

21. A computer program product comprising instructions and data that can be loaded by a computer arrangement to allow said computer arrangement to perform the method of claim 20.

22. Data carrier comprising a computer program product according to claim 21.

23. Computer arrangement for calculating a slope angle slope_angle of a vehicle while travelling along a trajectory relative to a local gravity vector ({right arrow over (g)}_loc), comprising a processor arranged to communicate with a memory storing a computer program comprising instructions and data that can be run by the processor, the vehicle comprising a distance measurement unit and an inertial measurement unit, an xy plane being defined locally by an x-axis and an y-axis, said x-axis being along said trajectory and said y-axis being perpendicular to said x-axis and to said local gravity vector, the processor being arranged to calculate said slope angle from measurements made by both said distance measurement unit and said inertial measurement unit.

24. Computer arrangement according to claim 23, wherein said processor is arranged to calculate said slope angle slope_angle in accordance with:

slope_angle=arcsin(υ

_z/υ

_x) 

 

(17)

where;

υ

_z=velocity in a direction opposite to said local gravity vector;

{right arrow over (v)}_x=velocity in the x-direction as derived from the distance measurement unit.

25. Computer arrangement according to claim 24, wherein said processor is arranged to calculate υ

_z from;

{right arrow over (v)}_z=∫

{right arrow over (a)}_STz·

dt 

 

(18)

where;

{right arrow over (a)}_STz={right arrow over (a)}_IMUz−

{right arrow over (a)}_z 

 

(14)

with;

{right arrow over (a)}_IMUz=acceleration z-direction measured by the inertial measurement unit;

{right arrow over (a)}_z=acceleration in z-direction opposite to the local gravity vector calculated by;

{right arrow over (a)}_z={right arrow over (ω

)}_y×

{right arrow over (v)}_x 

 

(11)

where;

{right arrow over (ω

)}_y equals the pitch of the vehicle relative to the local gravity vector.

26. Computer arrangement according to claim 24, wherein said processor is arranged to calculate v_z from:

υ

_z=υ

_x·

sin(pitch_true−

k·

a_x) 

 

(19)

where;

pitch_true=true pitch of the vehicle relative to the local gravity vector;

a_x—

acceleration in x-direction as derived from the distance measurement unit;

k=a constant.

27. Computer arrangement according to claim 25, wherein the processor is arranged to:

calculate;

υ

_z=υ

_z·

sin(pitch_true−

k·

a_x) 

 

(19)

where;

pitch_true=true pitch of the vehicle relative to the local gravity vector;

a_x=acceleration in x-direction as derived from the distance measurement unit;

k=a constant;

use equation (19) to remove errors from υ

_z as obtained by equation (18) rendering an error compensated vertical speed value υ

<sub>z,err—

comp</sub>;

substitute v. in equation (17) by the error compensated vertical speed υ

<sub>z,err—

comp</sub>.

28. Computer arrangement according to claim 24, wherein the processor (is arranged to calculate a z-level z(t) in a direction opposite to said local gravity vector ({right arrow over (g)}_loc) from:

z(t)=∫

v_x(t)·

sin(slope_angle(t)) 

 

(20)

29. Method of calculating a slope angle slope angle of a vehicle while travelling along a trajectory relative to a local gravity vector ({right arrow over (g)}_loc),the vehicle comprising a distance measurement unit and an inertial measurement unit, an xy plane being defined locally by an x-axis and an y-axis, said x-axis being along said trajectory and said y-axis being perpendicular to said x-axis and to said local gravity vector, the method comprising calculating said slope angle from measurements made by both said distance measurement unit and said inertial measurement unit.

30. A computer program product comprising instructions and data that can be loaded by a computer arrangement to allow said computer arrangement to perform the method of claim 29.

31. Data carrier comprising a computer program product according to claim 30.