Patent Application
20100332126
1. Field of the Invention
The present invention relates to navigation system, more specifically, relates to system for correction position in a navigation system.
2. Description of Prior Art
An inertial navigation system is a navigation system that includes a calculator and motion sensors to continuously calculate a position, heading angle, velocity, and other information of a moving object without the need for external references. Other terms used to refer to inertial navigation system or closely related devices include inertial guidance system, inertial reference platform, and many other variations.
An inertial navigation system is initially provided with initial navigation information, e.g., a position and an orientation, of a moving object from another source, e.g., a human operator, a GPS satellite receiver, etc., and thereafter calculates the updated navigation information of the moving object by integrating motion information of the moving object, e.g., linear velocity and angular velocity, received from the motion sensors on the initial navigation information. However, accuracy errors and measurement errors of the motion sensors, e.g., a gyroscope and an accelerometer, may accumulate during the integration process. After a relatively long time period, accumulate errors may cause relatively large offsets, which may not be ignored any more, between a real moving line of the moving object and a trajectory line calculated by the inertial navigation system.
A calculator of the conventional inertial navigation system can calculate a trajectory line of the moving object according to the measured information including linear velocity and angular velocity of the moving object. Because of the accuracy errors and the measurement errors of the motion sensors, the linear velocity and the angular velocity measured by the motion sensors may be different from the real values. As consequence, when the moving object goes straight from the east to the west and then turns to the north at a crossroad, the trajectory line 102 may shift west relative to the real moving line 104 if the measured linear velocity is greater than the real linear velocity of the moving object. Furthermore, a direction of the trajectory line 102 may rotate θ degree anticlockwise relative to a direction of the real moving line 104 if the measured angular velocity is less than the real angular velocity of the moving object.
As result, the trajectory line 102 calculated by the conventional inertial navigation system may be different to the moving line 104. The error between the trajectory line 102 and the moving line 104 may affect the system performance.
The present invention provides an inertial navigation system with error correction based on a navigation map. The inertial navigation system includes a trajectory calculator for calculating a position and an orientation of a moving object based on moving information of the moving object, an error correcting unit coupled to the trajectory calculator for correcting the position and the orientation of the moving object based on reference road information, and a storage unit coupled to the error correcting unit for storing geographical information of a road network and providing the reference road information according to the geographical information of the road network.
In another embodiment, the present invention provides a method for generating navigation information of a moving object and correcting said navigation information. The method includes calculating a position and an orientation of a moving object by a trajectory calculator based on moving information of the moving object, correcting the position and the orientation by an error correcting unit based on reference road information, and providing the reference road information by a storage unit according to geographical information of a road network stored in the storage unit.
Features and advantages of embodiments of the claimed subject matter will become apparent as the following detailed description proceeds, and upon reference to the drawings, wherein like numerals depict like parts, and in which:
Reference will now be made in detail to the embodiments of the present invention. While the invention will be described in conjunction with these embodiments, it will be understood that they are not intended to limit the invention to these embodiments. On the contrary, the invention is intended to cover alternatives, modifications, and equivalents, which may be included within the spirit and scope of the invention as defined by the appended claims.
Furthermore, in the following detailed description of the present invention, numerous specific details are set forth in order to provide a thorough understanding of the present invention. However, it will be recognized by one of ordinary skill in the art that the present invention may be practiced without these specific details. In other instances, well known methods, procedures, components, and circuits have not been described in detail as not to unnecessarily obscure aspects of the present invention.
The present invention improves accuracy of a navigation system by reducing the accumulated error of an inertial navigation system according to reference road information indicated in a navigation map.
As shown in
The inertial navigation system 200 further includes a processor 210 for calculating trajectory line of the vehicle and correcting the trajectory line according to reference road information provided by a map storage unit 212. In one embodiment, the processor 210 can get the reference road information based on a navigation map stored in the storage unit 212. The navigation map indicates geographic data of a road network. In another embodiment, the roads in the navigation map can be regarded as single lines with a starting point and an end point. The navigation map can indicate position information and orientation information of the single road lines. In the processor 210, a trajectory calculator 208 can be used to calculate the trajectory line of the vehicle based on the measured linear velocity and angular velocity received from the milemeter 204 and the gyroscope 202 respectively.
When the system starts to operate, the trajectory calculator 208 can periodically update the position and the orientation of the vehicle by integrating the received angular velocity and linear velocity of the vehicle upon the previous position and orientation.
In one embodiment, the inertial navigation system 200 adopts a World Geodetic System (WGS). The World Geodetic System is a standard used in navigation. The World Geodetic System defines a standard coordinate frame for the Earth. In the standard coordinate frame for the Earth, the position of the vehicle consists of a longitude component and a latitude component. The position can be updated by updating the longitude component and the latitude component respectively. The longitude component and the latitude component can be updated periodically based on the current linear velocity of the vehicle and the previous longitude and latitude components according to equation (1).
newLon represents the longitude component of the current position. newLat represents the latitude component of the current position. oldLon represents the longitude component of the previous position. oldLat represents the latitude component of the previous position. VE represents an east component of the linear velocity of the vehicle. VN represents a north component of the linear velocity of the vehicle. T represents a unit interval of the linear velocity, e.g., 1 s. R represents a distance from the previous position to an origin of the standard coordinate frame for the Earth.
The orientation of the vehicle can be updated according to equation (2).
newOri=oldOri+VA*T (2)
newOri represents the current orientation. oldOri represents the previous orientation. VA represents the angular velocity measured by the gyroscope 202 currently. T represents a unit interval of the angular velocity, e.g., 1 s.
Additionally, when the system starts up, the initial information including an initial position and an initial orientation of the vehicle can be provided from another source, e.g., a human operator, a GPS satellite receiver and etc., to the trajectory calculator 208. The trajectory calculator 208 can calculate the first position and orientation of the vehicle by integrating the received angular velocity and linear velocity on the initial information.
As result, the trajectory calculator 208 can determine the trajectory line of the vehicle by periodically updating the position and the orientation of the vehicle and sending the trajectory line to a display screen 214. The display screen 214 can also get the navigation map from the map storage unit 212, match the trajectory line to the navigation map and display the trajectory line of the vehicle upon the navigation map.
The processor 210 further includes an error correcting unit 206 for correcting the position and the orientation of the vehicle calculated by the trajectory calculator 208 based on the navigation map stored in the map storage unit 212. In one embodiment, the error correcting unit 206 can correct the position and the orientation of the vehicle when the vehicle turns around a corner or goes straight on a road. In order to monitor moving states of the vehicle, which include turning around a corner and going straight on a road, the error correcting unit 206 periodically calculates a turning angle of the vehicle according to the angular velocity of the vehicle received from the gyroscope 202. The turning angle represents an orientation difference between a current orientation and a previous orientation measured before a predetermined time interval, e.g., 5 seconds.
If the turning angle is within a predetermined range, e.g., from 60 degree to 120 degree, the error correcting unit 206 may conclude that the vehicle is turning around a corner. If the turning angle is less than a predetermined value, e.g., 20 degree, the error correcting unit 206 may conclude that the vehicle is going straight on a road.
If the vehicle turns to road R1 at a corner, the error correcting unit 206 can record a calculated position of the vehicle at the corner, and get a reference position of the corner based on the navigation map from the map storage unit 212. Then if the vehicle keeps going straight on road R1, the error correcting unit 206 can compare the calculated position of the vehicle with the reference position. If a difference between the calculated position and the reference position is greater than a predetermined threshold PTHR, the error correcting unit 206 can instruct the trajectory calculator 208 to correct the current position of the vehicle according to the navigation map.
If the vehicle goes straight on road R2, the error correcting unit 206 can get the calculated orientation of the vehicle from the trajectory calculator 208, and get a reference orientation of road R2 based on the navigation map from the map storage unit 212. The error correcting unit 206 can compare the current orientation of the vehicle with the reference orientation of road R2. If a difference between the current orientation and the reference orientation is greater than a predetermined threshold OTHR, the error correcting unit 206 can instruct the trajectory calculator 208 to correct the current orientation of the vehicle according to the navigation map.
Additionally, if the vehicle goes straight on road R3 and an orientation difference between the current orientation of the vehicle and the reference orientation of road R3 is less than the predetermined threshold OTHR, the error correcting unit 206 can periodically calculate a vertical distance from the current position of the vehicle to the target road line. As described before, each road indicated in the navigation map can be regarded as a single line. If the vertical distance is greater than a predetermined threshold DTHR, the error correcting unit 206 can instruct the trajectory calculator 208 to correct the current position of the vehicle to a corresponding position on the target road line.
As result, the trajectory calculator 210 can calculate the next position and orientation of the vehicle based on the corrected position and orientation.
In the error correcting unit 206, a turning angle calculator 302 is used for periodically calculating a turning angle of a moving object, e.g., a vehicle, based on the angular velocity from the gyroscope 202. A controller 304 is used for monitoring moving states of the vehicle according to the turning angle of the vehicle, and controlling the trajectory calculator 208 to correct the calculated position and orientation of the vehicle according to the corresponding moving states of the vehicle. The moving states of the vehicle can include turning around a corner and going straight on a road.
During operation, the turning angle calculator 302 can receive the angular velocity from the gyroscope 202 periodically and calculate the turning angle of the vehicle according to the angular velocity. More specifically, the gyroscope 202 can generate the current angular velocity of the vehicle to the turning angle calculator 302 periodically. The turning angle calculator 302 can store a predetermined number of the angular velocity values, which are received from the gyroscope 202 sequentially, in a storage unit (not shown). The turning angle calculator 302 can calculate a current turning angle of the vehicle according to equation (3).
θ=VA1*T1+VA2T2+VA3*T3+ . . . +VAn*Tn (3)
θ represents the turning angle of the vehicle. VA1, VA2, . . . , and VAn represent the angular velocity values stored in the storage unit. T1, T2, . . . , and Tn respectively represent time intervals of measuring the angular velocity values VA1, VA2, . . . , and VAn. The angular velocity values VA1, VA2, . . . , and VAn are measured in sequence.
For example, the gyroscope 202 measures the angular velocity of the vehicle every one second. The turning angle calculator 302 stores five angular velocity values VA1, VA2, VA3, VA4 and VA5 received from the gyroscope 202 sequentially. Then the turning angle of the vehicle can be calculated according to equation (4).
θ=VA1+VA2VA3+VA4+VA5 (4)
After calculating the current turning angle of the vehicle, the turning angle calculator 302 can continue calculating the next turning angle according to a new series of the angular velocity values. In order to get the new series of the angular velocity values, the turning angle calculator 302 can delete the angular velocity value measured firstly, e.g., VA1 illustrated in equation (3), from the storage unit and then store a new angular velocity value VA(n+1) measured currently in the storage unit. Therefore, the next turning angle can be calculated based on the new series of the angular velocity values VA2, VA3, VA4, . . . , VA(n+1) according to equation (3).
In one embodiment, the turning angle calculator 302 periodically generates the turning angle of the vehicle to the controller 304. The controller 304 can determine whether the vehicle turns around a corner or goes straight on a road during a certain period according to the turning angle. If the turning angle is within a predetermined range, e.g., from 60 degree to 120 degree, the controller 304 can determine that the vehicle turns around a corner. If the turning angle is less than a predetermined value, e.g., 20 degree, the controller 304 can determine that the vehicle goes straight on a road.
According to the corresponding moving states of the vehicle, the controller 304 can get the calculated position and/or orientation of the vehicle from the trajectory calculator 208. The controller 304 can also get the reference position and/or orientation of corresponding roads based on the navigation map from the map storage unit 212. Thereafter, the controller 304 can compare the calculated position and orientation with the reference position and orientation respectively, and instruct the trajectory calculator 208 to correct the calculated position and orientation of the vehicle based on the navigation map according to the comparison result.
As shown in
The vehicle runs on road R1 from the east to the west and then turns north to road R2 at corner C1. Thereafter the vehicle runs straight on road R2. After turning around corner C1, the trajectory line 402 calculated by the trajectory calculator 208 may be shifted west relative to the road line 404, which is the real trajectory line of the vehicle, if the linear velocity measured by the milemeter 204 is greater than the real linear velocity of the moving object.
In order to reduce a position offset of the trajectory line 402, the error correcting unit 206 can monitor the moving state of the vehicle and correct the calculated position of the vehicle based on the navigation map stored in the map storage unit 212 if the vehicle turns to road R2 at corner C1 and then goes straight on road R2.
In one embodiment, the controller 302 monitors the moving state of the vehicle according to the turning angles of the vehicle received from the turning angle calculator 304 periodically. If the vehicle turns to road R2 at corner C1, a turning angle output from the turning angle calculator 302 can be within a predetermined range, e.g., from 60 degree to 120 degree. Accordingly, the controller 304 can get a calculated position OV of the vehicle from the trajectory calculator 208 and store it in a storage unit (not shown). The controller 304 can also get a reference position OREF of the start point of the road R2, which is also the position of corner C1, from the map storage unit 212 and store it in the storage unit
Thereafter, the controller 304 can continuously monitor the moving state of the vehicle during a predetermined period after turning around the corner C1. If the latter turning angles, which are received from the turning angle calculator 302 during the predetermined period are less than a predetermined value, e.g., 20 degree, the controller 304 can determine that the vehicle is going straight on road R2 after turning around corner C1. Accordingly, the controller 304 can compare the calculated position OV with the reference position OREF. Both the calculated position OV and the reference position OREF were stored previously in the storage unit. After the predetermined period, the vehicle arrives at position P1.
If a difference between the calculated position OV and the reference position OREF is greater than a predetermined threshold, the trajectory calculator 208 corrects current position P1 to position P2 on the road line 404. In one embodiment, the trajectory calculator 208 can calculate a distance between positions P1 and OV. Position P2 can be determined as a position on the road line 404 while a distance between positions P2 and OREF can be the same as the distance between positions P1 and OV. The trajectory calculator 208 can update the trajectory line 406 of the vehicle based on corrected position P2. As shown, the trajectory line of the vehicle can be corrected based on the navigation map even an accumulated error in the milemeter 204 exists.
If any turning angle received from the turning angle calculator 302 is greater than the predetermined value during the predetermined period, the controller 304 can conclude that the vehicle is not going on a straight line on the road R2 during the predetermined period after turning around corner C1. Hence, the controller 304 removes the calculated position OV and the reference position OREF from the storage unit. The controller 304 can continue monitoring the moving state of the vehicle. When the vehicle turns around another corner and then goes straight for a predetermined period, the position of the vehicle calculated by the trajectory calculator 208 can be corrected accordingly.
As shown in
In order to reduce an orientation offset of the trajectory line 502, the error correcting unit 206 can correct the calculated orientation of the vehicle based on the navigation map stored in the map storage unit 212 when the vehicle goes straight on road R1.
In one embodiment, the controller 304 monitors the moving state of the vehicle according to the turning angles of the vehicle received from the turning angle calculator 304 periodically. The controller 304 starts to monitor the moving state of the vehicle when the vehicle is at position OV, in one embodiment. If the turning angles calculated by the turning angle calculator 302 during a predetermined period are less then a predetermined value, the controller 304 can conclude that the vehicle goes straight on road R1. Accordingly, the controller 304 gets the calculated orientation of the vehicle from the trajectory calculator 208 and gets the reference orientation of the road line 504 from the map storage unit 212. Thereafter, the controller 304 compares the calculated orientation with the reference orientation. After the predetermined period, the vehicle reaches position P1.
If an orientation difference e between the calculated orientation and the reference orientation is greater than a predetermined threshold OTHR, the trajectory calculator 208 corrects the current orientation of the vehicle to the reference orientation. The trajectory calculator 208 can update the trajectory line 506 of the vehicle based on the corrected orientation from position P1. As shown, the trajectory line of the vehicle can be corrected based on the navigation map even an accumulated error in the gyroscope 202 exists.
As shown in
In order to reduce the position offset of the trajectory line 602, the error correcting unit 206 can correct the calculated position of the vehicle based on the navigation map stored in the map storage unit 212 when the vehicle goes straight on road R1.
The controller 304 monitors the moving state of the vehicle according to the turning angles of the vehicle received from the turning angle calculator 304 periodically. In one embodiment, the controller 304 starts to monitor the turning angle of the vehicle when the vehicle is at position OV. During a predetermined period, if the turning angles calculated by the turning angle calculator 302 are less then a predetermined value, the controller 304 can determine that the vehicle goes straight on road R1.
Thereafter, the controller 304 periodically gets a calculated position of the vehicle from the trajectory calculator 208 and calculates a vertical distance from the calculated position to the road line 604. If a vertical distance D1 is greater than a predetermined threshold DTHR when the vehicle is at position P1, the trajectory calculator 208 corrects the current position of the vehicle to position P2 on the road line 604. A line between position P1 and position P2 can be vertical to the road line 604.
The trajectory calculator 208 can update the trajectory line 606 of the vehicle based on corrected position P2. The controller 304 can continue calculating the vertical distances from the calculated positions to the road line 604. If a vertical distance from position P3 on the trajectory line 606 to the road line 604 is greater than the predetermined threshold DTHR, the trajectory calculator 208 corrects the current position of the vehicle to position P4 on the road line 604. A line between position P3 and position P4 can be vertical to the road line 604. As shown, the trajectory line of the vehicle can be corrected based on the navigation map even an orientation difference between the orientation of the trajectory line and the road line is less than the predetermined threshold OTHR.
In block 708, the error correcting unit 206 can correct the position and the orientation of the moving object calculated by the trajectory calculator 208 based on the reference road information. In one embodiment, the error correcting unit 206 can compare the calculated position and/or orientation of the moving object with a reference position and/or orientation indicated in the reference road information respectively according to different moving states of the moving object. The moving states can include turning around a corner and going straight on a road. The error correcting unit 206 can correct the calculated position and orientation of the moving object according to the navigation map based on the comparison result.
In block 710, the position and the orientation of the moving object can be displayed on the display screen 214.
In block 802, the turning angle calculator 302 calculates a turning angle θ of the moving object based on the moving information during a predetermined period. In one embodiment, the turning angle θ can be calculated according to a predetermined number of the angular velocity values received from the gyroscope 202 sequentially. The turning angle θ can be given by equation (5).
θ=VA1*T1+VA2*T2+VA3*T3+ . . . +VAn*Tn (5)
wherein VA1, VA2, . . . , and VAn represent a predetermined number of the angular velocity values. T1, T2, . . . , and Tn respectively represent time intervals for measuring the angular velocity values VA1, VA2, . . . , and VAn. The angular velocity values VA1, VA2, . . . , and VAn are measured in sequence.
In block 804, if the turning angle θ is within a predetermined range (θ1, θ2), e.g., 60 degree<θ<120 degree, which indicates the moving object turns to a road R1 at a corner, the flowchart goes to block 806. In block 806, the controller 304 can get the current position P1 of the moving object from the trajectory calculator 208 and store it in a storage unit. The controller 304 can also get a reference position PREF of the corresponding corner based on the navigation map from the map storage unit 212, and store it in the storage unit. Then the controller 304 keeps monitoring the moving state of the moving object.
In block 808, the turning angle calculator 302 calculates a next turning angle θ′ of the moving object during a next predetermined period. In block 810, if the next turning angle θ′ is less than a predetermined threshold θTHR, which indicates the moving object is going straight on the road R1 after turns around the corner, the controller 304 can compare the stored position P1 with the reference position PREF in block 812.
In block 810, if the next turning angle θ′ is not less than the predetermined threshold θTHR, which indicates the moving object is not going on a straight line on the road R1 after turning around the corner, the flowchart 800 goes back to block 802. The controller 304 can keep monitoring the moving state of the moving object by judging the turning angles of the moving object calculated by the turning angle calculator 302 periodically.
In block 814, if a difference between the stored position P1 and the reference position PREF is greater than a predetermined threshold PTHR, the controller 304 instructs the trajectory calculator 208 to correct current position of the moving object to a corresponding position on a reference road line of the road R1 in block 816. As result, the position offset of the trajectory calculator 208 can be reduced accordingly. If the difference between the stored position P1 and the reference position PREF is not greater than the predetermined threshold PTHR in block 814, the flowchart 800 goes back to block 802.
In block 804, if the turning angle e is not within the predetermined range (θ1, θ2), the turning angle θ can be judged in block 818. In block 818, if the turning angle θ is less than a predetermined threshold θ3, e.g., 20 degree, which indicates the moving object is going straight on a road R2, the orientation calculated by the trajectory calculator 208 can be corrected based on a reference orientation indicated in the reference road information in block 820. In one embodiment, the controller 304 can compare the current orientation calculated by the trajectory calculator 208 with a reference orientation of the road R2. If a difference between the current orientation and the reference orientation is greater than a predetermined threshold, the controller 304 can instruct the trajectory calculator 208 to correct the orientation of the moving object to the reference orientation.
In block 822, the position calculated by the trajectory calculator 208 can be corrected based on a corresponding road line of the road R2 indicated in the reference road information, when the moving object goes straight on the road R2. In one embodiment, the controller 304 can get a current position of the moving object calculated by the trajectory calculator 208 and calculate a vertical distance from the current position of the moving object to the reference road line of the road R2. If the vertical distance is greater than a predetermined threshold, the controller 304 can instruct the trajectory calculator 208 to correct the current position of the moving object to a corresponding position on the reference road line of the road R1.
In block 818, if the turning angle θ is not less than the predetermined threshold θ3, the flowchart 800 goes to block 802. Then the controller 304 can keep monitoring the moving state of the moving object by judging the turning angles of the moving object calculated by the turning angle calculator 302 periodically.
Accordingly, embodiments in accordance with the present invention provide an inertial navigation system with error correction based on a navigation map. The inertial navigation system can include a plurality of motion sensors coupled to a moving object for measuring moving information of the moving object, a trajectory calculator coupled to the motion sensors for calculating a position and an orientation of the moving object based on the moving information of the moving object, an error correcting unit coupled to the trajectory calculator for correcting the position and the orientation of the moving object based on reference road information, and a storage unit coupled to the error correcting unit for storing geographical information of a road network and providing the reference road information according to the geographical information of the road network.
In order to reduce the accumulated error, the error correcting unit can compare the calculated position of the moving object with a reference position of a corner indicated in the reference road information when the moving object turns around the corner, and instruct the trajectory calculator to correct the position of the moving object based on the reference road information according to the comparison result. Furthermore, the error correcting unit can compare the calculated orientation of the moving object with a reference orientation of a road indicated in the reference road information when the moving object goes straight on the road, and instruct the trajectory calculator to correct the orientation of the moving object to the reference orientation according to the comparison result. Additionally, the error correcting unit can calculate a distance from the calculated position of the moving object to a reference road line of a road indicated in the reference road information when the moving object goes straight on the road, and instruct the trajectory calculator to correct the position of the moving object to a corresponding position on the reference road line according to the calculation result.
While the foregoing description and drawings represent embodiments of the present invention, it will be understood that various additions, modifications and substitutions may be made therein without departing from the spirit and scope of the principles of the present invention as defined in the accompanying claims. One skilled in the art will appreciate that the invention may be used with many modifications of form, structure, arrangement, proportions, materials, elements, and components and otherwise, used in the practice of the invention, which are particularly adapted to specific environments and operative requirements without departing from the principles of the present invention. The presently disclosed embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims and their legal equivalents, and not limited to the foregoing description
