WO2020059383A1 - 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム - Google Patents

航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム Download PDF

Info

Publication number
WO2020059383A1
WO2020059383A1 PCT/JP2019/032415 JP2019032415W WO2020059383A1 WO 2020059383 A1 WO2020059383 A1 WO 2020059383A1 JP 2019032415 W JP2019032415 W JP 2019032415W WO 2020059383 A1 WO2020059383 A1 WO 2020059383A1
Authority
WO
WIPO (PCT)
Prior art keywords
gnss
vslam
error
scale
attitude angle
Prior art date
Application number
PCT/JP2019/032415
Other languages
English (en)
French (fr)
Inventor
戸田　裕行
中村　拓
達也 園部
奈緒美 藤澤
Original Assignee
古野電気株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 古野電気株式会社 filed Critical 古野電気株式会社
Priority to JP2020548153A priority Critical patent/JP7190500B2/ja
Priority to CN201980058853.1A priority patent/CN112840235A/zh
Priority to EP19861916.5A priority patent/EP3855216A4/en
Publication of WO2020059383A1 publication Critical patent/WO2020059383A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude
    • G01S19/54—Determining attitude using carrier phase measurements; using long or short baseline interferometry
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/52—Determining velocity

Definitions

  • the present invention relates to a navigation device using the technology of Visual Simultaneous Localization and Mapping (VSLAM).
  • VSLAM Visual Simultaneous Localization and Mapping
  • VSLAM Visual Simultaneous Localization and Mapping
  • VSLAM estimates spatial information such as feature points around its own device using a moving image acquired by a monocular camera, and performs, for example, mapping. VSLAM calculates the relative position and the like of its own device using the spatial information and the mapping result.
  • VSLAM the relative positional relationship between the own device and other feature points can be calculated, but the absolute positional relationship has indefiniteness.
  • the navigation device of the present invention includes a GNSS operation unit, a VSLAM operation unit, and a correction information estimation unit.
  • the GNSS calculation unit calculates the GNSS speed and the GNSS attitude angle using the amount of change in the phase of the GNSS signal transmitted from the positioning satellite or the amount of change in the frequency of the GNSS signal.
  • the VSLAM calculation unit performs a VSLAM calculation using a moving image to estimate a VSLAM position at a plurality of times and a VSLAM attitude angle at a plurality of times.
  • the correction information estimating unit uses the GNSS speed, the GNSS attitude angle, the VSLAM position at a plurality of times, and the VSLAM attitude angle to determine the scale or scale error of the VSLAM position with respect to the position of the absolute coordinate system, and the error of the GNSS speed. Or, an error of the GNSS position obtained from the GNSS speed is estimated.
  • the absolute positional relationship between the own device and other feature points can be calculated with high accuracy while using VSLAM.
  • FIG. 2 is a functional block diagram illustrating a configuration of a calculation unit in the navigation device according to the first embodiment of the present invention. It is a functional block diagram showing the composition of the navigation system concerning a 1st embodiment of the present invention.
  • FIG. 3 is a functional block diagram illustrating a configuration of a correction information estimation unit according to the first embodiment of the present invention.
  • FIG. 3 is a diagram for explaining the concept of spatial information and GNSS position correction according to the first embodiment of the present invention. It is a graph which shows transition of the error of GNSS speed and GNSS position. It is a flowchart which shows the processing flow of the generation method of navigation assistance information. It is a functional block diagram showing the composition of the navigation system concerning a 2nd embodiment of the present invention.
  • FIG. 3 is a functional block diagram illustrating a configuration of a calculation unit in the navigation device according to the first embodiment of the present invention. It is a functional block diagram showing the composition of the navigation system concerning a 1st embodiment of the present invention.
  • FIG. 9 is a functional block diagram illustrating a configuration of a calculation unit according to a second embodiment of the present invention. It is a functional block diagram showing the composition of the navigation system concerning a 3rd embodiment of the present invention. It is a functional block diagram showing the composition of the operation part concerning a 3rd embodiment of the present invention.
  • (A), (B) is a figure which shows the example of the navigation assistance image displayed by the navigation apparatus which concerns on 3rd Embodiment. It is a flowchart which shows the processing flow of the generation method of navigation assistance information.
  • a navigation device, a navigation support method, and a navigation support program according to the first embodiment of the present invention will be described with reference to the drawings.
  • a ship is used as a moving object
  • the configuration of the present invention can be applied to other water, underwater moving objects, land moving objects, and air moving objects.
  • FIG. 1 is a functional block diagram showing a configuration of a calculation unit in the navigation device according to the first embodiment of the present invention.
  • FIG. 2 is a functional block diagram showing a configuration of the navigation device according to the first embodiment of the present invention.
  • FIG. 3 is a functional block diagram illustrating a configuration of the correction information estimating unit according to the first embodiment of the present invention.
  • the navigation device 10 includes a GNSS receiver 111, a GNSS receiver 112, a GNSS receiver 113, an arithmetic unit 12, a GNSS antenna 21, a GNSS antenna 22, a GNSS antenna 23, and a camera 30. Note that the navigation device 10 only needs to include at least the calculation unit 12.
  • the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23 are installed on a ship (not shown) to which the navigation device 10 is mounted.
  • the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23 are installed in a so-called open sky environment.
  • the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23 are arranged so that they are not all aligned at the same time. That is, for example, the GNSS antenna 23 is not arranged on a straight line connecting the GNSS antenna 21 and the GNSS antenna 22.
  • the GNSS antenna 21 receives a GNSS signal from a GNSS satellite (not shown) and outputs the GNSS signal to the GNSS receiving unit 111.
  • the GNSS antenna 22 receives a GNSS signal from a GNSS satellite (not shown) and outputs the GNSS signal to the GNSS receiving unit 112.
  • the GNSS antenna 23 receives a GNSS signal from a GNSS satellite (not shown) and outputs the GNSS signal to the GNSS receiving unit 113.
  • the GNSS signal is a signal in which a PRN (pseudo-random noise) code and navigation message data are superimposed on a carrier signal having a predetermined frequency.
  • the PRN code is a code for identifying the source positioning satellite.
  • the navigation message data is data including orbit information of a positioning satellite, correction information, and the like.
  • Each of the GNSS receiving unit 111, the GNSS receiving unit 112, and the GNSS receiving unit 113 includes a memory that stores a program for capturing and tracking a GNSS signal, and an arithmetic element such as an IC that executes the program. Have been.
  • the GNSS receiver 111, the GNSS receiver 112, and the GNSS receiver 113 capture and track the acquired GNSS signal by a known method. At this time, the GNSS receiver 111, the GNSS receiver 112, and the GNSS receiver 113 capture and track a code and a carrier signal (carrier signal).
  • the GNSS receiving unit 111, the GNSS receiving unit 112, and the GNSS receiving unit 113 acquire observation data by capturing and tracking, and output the observation data to the calculation unit 12.
  • the observation data includes at least a carrier phase integrated value (ADR).
  • the GNSS receiver 111, the GNSS receiver 112, and the GNSS receiver 113 sequentially execute the acquisition of the integrated value of the carrier phase at predetermined time intervals in synchronization with the time of the GNSS, for example, at each epoch. I do.
  • the GNSS receiver 111, the GNSS receiver 112, and the GNSS receiver 113 transmit the Doppler frequency of the carrier signal, the code phase, the pseudo distance calculated by the code phase, the positioning position, and the like to the arithmetic unit 12 as observation data. It is also possible to output.
  • the camera 30 is, for example, a monocular camera.
  • the camera 30 captures an image around a ship on which the navigation device 10 is mounted, and generates a moving image.
  • the camera 30 outputs the moving image to the calculation unit 12.
  • the calculation unit 12 uses the observation data from the GNSS reception unit 111, the GNSS reception unit 112, and the GNSS reception unit 113, and the moving image from the camera 30 to scale error of the VSLAM position with respect to the position of the absolute coordinate system ( Scale error) and a GNSS speed error or a GNSS position error obtained from the GNSS speed. In addition, the calculation unit 12 corrects the spatial information obtained from the moving image using the scale corrected by the scale error. The calculation unit 12 corrects the position of the navigation device 10 (own device position) using the error of the GNSS speed or the error of the GNSS position.
  • the calculation unit 12 stores a program for processing such as VSLAM calculation, calculation of a GNSS calculation value, estimation of a scale error, estimation of an error of the own device position, correction of spatial information, correction of the own device position, which will be described later, It is composed of an arithmetic element such as a CPU that executes the program.
  • the calculation unit 12 includes a VSLAM calculation unit 121, a GNSS calculation unit 122, a correction information estimation unit 123, a spatial information correction unit 124, and a GNSS position correction unit 125.
  • the arithmetic unit 12 only needs to include at least the VSLAM arithmetic unit 121, the GNSS arithmetic unit 122, and the correction information estimating unit 123.
  • the VSLAM calculation unit 121 calculates the position (VSLAM position) of the own device with respect to the reference point (see 90 (coordinate Po) in FIG. 4) and the spatial information of each feature point by a known method using the acquired moving image. presume. At this time, the VSLAM calculation unit 121 estimates the position of the own device with respect to the reference point (VSLAM position) and the spatial information of each feature point in synchronization with the GNSS time.
  • the VSLAM calculation unit 121 synchronizes with the acquisition time of the integrated value of the carrier phase, and at each acquisition time of the moving image, the VSLAM position Pv that is the position of the own device with respect to the reference point, the VSLAM attitude angle AAv of the own device, Further, spatial information of each feature point is estimated. For example, the VSLAM calculation unit 121 estimates the VSLAM position Pv, the VSLAM attitude angle AAv, and the spatial information of each feature point in synchronization with a GNSS speed calculation time described later. Note that the spatial information is represented by a spatial vector of a feature point starting from a reference point of the VSLAM operation.
  • the VSLAM calculation unit 121 outputs the VSLAM position Pv and the VSLAM attitude angle AAv to the correction information estimation unit 123 and the space information correction unit 124. Further, the VSLAM operation unit 121 outputs the spatial information to the spatial information correction unit 124. At this time, the VSLAM calculation unit 121 outputs the VSLAM position Pv, the VSLAM attitude angle AAv, and the spatial information in association with the estimated time.
  • the GNSS calculation unit 122 includes a GNSS speed calculation unit 1221 and a GNSS attitude angle calculation unit 1222.
  • the GNSS speed calculation unit 1221 calculates the GNSS speed Vg using the change amount of the integrated value of the carrier phase acquired by the GNSS reception unit 111, the GNSS reception unit 112, or the GNSS reception unit 113. At this time, the GNSS speed calculation unit 1221 calculates the GNSS speed Vg at predetermined time intervals synchronized with the GNSS time. By using the amount of change in the integrated value of the carrier phase, the GNSS speed calculation unit 1221 can calculate the GNSS speed Vg with high speed and high accuracy. That is, the GNSS speed calculation unit 1221 can calculate the GNSS speed Vg at high speed and with high accuracy without performing processing such as determination of an integer value bias or acquiring correction information from the outside.
  • the GNSS speed calculation unit 1221 outputs the GNSS speed Vg to the correction information estimation unit 123.
  • the GNSS speed calculation unit 1221 outputs the GNSS speed Vg in association with the calculation time.
  • the GNSS attitude angle calculation unit 1222 uses a carrier phase integrated value acquired by each of the GNSS reception unit 111, the GNSS reception unit 112, and the GNSS reception unit 113, and uses a known method (for example, a baseline vector between GNSS antennas).
  • GNSS attitude angle AAg is calculated from the above method.
  • the integrated value of the carrier phase the GNSS attitude angle AAg is calculated with high accuracy.
  • GNSS posture angle calculation unit 1222 outputs GNSS posture angle AAg to correction information estimation unit 123. At this time, the GNSS attitude angle calculation unit 1222 outputs the GNSS attitude angle AAg in association with the calculation time.
  • the correction information estimation unit 123 calculates the scale S.
  • the correction information estimating unit 123 estimates the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg using the VSLAM position Pv, the VSLAM attitude angle AAv, the GNSS speed Vg, and the GNSS speed angle AAg. Note that the correction information estimating unit 123 may estimate the error ⁇ Pg of the GNSS position Pg instead of the error ⁇ Vg of the GNSS speed Vg.
  • the GNSS position Pg is obtained, for example, by integrating the GNSS speed Vg with the coordinates of the reference point as the origin.
  • the correction information estimation unit 123 corrects the scale S using the scale error ⁇ S, and outputs the corrected scale S to the spatial information correction unit 124. Further, the correction information estimation unit 123 outputs the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg to the GNSS position correction unit 125.
  • the mode in which the scale error ⁇ S is estimated has been described, but the scale S may be estimated.
  • the correction information estimation unit 123 includes a rotation matrix calculation unit 1231 and an estimation calculation unit 1232.
  • the rotation matrix calculation unit 1231 calculates a rotation matrix C for performing conversion between the VSLAM coordinate system and the GNSS coordinate system using the VSLAM attitude angle AAv and the GNSS attitude angle AAg.
  • the rotation matrix calculation unit 1231 outputs the rotation matrix C to the estimation calculation unit 1232.
  • the estimation calculation unit 1232 calculates the scale S, estimates the scale error ⁇ S, and the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg.
  • the estimation calculation unit 1232 calculates the GNSS distance Dgij by integrating the GNSS speed Vg.
  • the correction information estimation unit 123 calculates the VSLAM distance Dvij using the amount of change of the VSLAM position Pv at the same time as the calculation target time of the GNSS distance Dgij.
  • the estimation calculation unit 1232 sets a system equation using the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg as state variables.
  • the estimation calculation unit 1232 sets an observation equation including the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg as state variables, the VSLAM position Pv and the GNSS speed Vg as observation values, and including the rotation matrix C in the transformation matrix. I do.
  • the estimation operation unit 1232 performs Kalman filter processing using these system equations and observation equations. Thereby, the estimation calculation unit 1232 estimates the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg.
  • the estimation calculation unit 1232 can estimate the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg with high accuracy.
  • the estimation calculation unit 1232 sets a system equation using the scale error ⁇ S and the error ⁇ Pg of the GNSS position Pg as state variables. In addition, the estimation calculation unit 1232 sets an observation equation including the scale error ⁇ S and the error ⁇ Pg of the GNSS position Pg as state variables, the VSLAM position Pv and the GNSS speed Vg as observation values, and including the rotation matrix C in the transformation matrix. I do.
  • the estimation operation unit 1232 performs Kalman filter processing using these system equations and observation equations. Thereby, the estimation calculation unit 1232 estimates the scale error ⁇ S and the error ⁇ Pg of the GNSS position Pg.
  • the estimation calculation unit 1232 can estimate the scale error ⁇ S and the error ⁇ Pg of the GNSS position Pg with high accuracy.
  • the mode in which the scale error ⁇ S is estimated has been described, but the scale S may be estimated.
  • a system equation and an observation equation may be set using the scale S as a state variable instead of the scale error ⁇ S.
  • the spatial information correction unit 124 corrects the spatial information using the scale S.
  • the scale S is corrected with high accuracy by estimating the scale error ⁇ S with high accuracy. Therefore, the spatial information correction unit 124 can correct spatial information with high accuracy.
  • the GNSS position correction unit 125 calculates the GNSS position Pg by integrating the GNSS speed Vg.
  • the GNSS position correction unit 125 calculates and outputs the GNSS position Pg using the speed Vgc obtained by correcting the GNSS speed Vg with the error ⁇ Vg.
  • the GNSS position correction unit 125 outputs the GNSS position Pgc obtained by correcting the GNSS position Pg calculated using the GNSS speed Vg with the error ⁇ Pg.
  • the GNSS position correction unit 125 can correct the GNSS position Pg with high accuracy.
  • FIG. 4 is a graph showing a time transition between the accumulated error of the GNSS speed and the error of the GNSS position after correction. As shown in FIG. 4, even if the integrated error of the GNSS speed Vg increases with the elapsed time, the error of the corrected GNSS position Pgc is suppressed to a small value.
  • the navigation device 10 can calculate the positional relationship between the position of the navigation device itself and each feature point of the spatial information with high accuracy.
  • FIG. 5 is a diagram for explaining the concept of spatial information and GNSS position correction according to the first embodiment of the present invention.
  • the GNSS antenna 21, the GNSS antenna 22, the GNSS antenna 23, and the camera 30 are arranged at substantially the same position (observation position) 91 in the coordinate system of the ship 900. More specifically, it is assumed that the geometric center of the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23 and the camera 30 are arranged at least at the same position on the horizontal plane of the hull coordinate system.
  • the VSLAM position and the GNSS position at the first time ti are described as being coincident with each other for easy understanding.
  • the present invention is not limited to this, and the following processing can be realized.
  • the spatial information for the feature point 911v of the quay 910v by the VSLAM calculation is a space vector Ve911v starting from the reference point 90 (coordinate Po) and ending at the feature point 911v.
  • the space vector Ve911v includes indefiniteness inherent to the camera 30 as described in the above-described problem.
  • the scale S is a value for correcting the error of the VSLAM position with respect to the reference point 90 (coordinate Po) with high accuracy using the GNSS speed with high accuracy as described above.
  • the scale error ⁇ S is estimated with high accuracy by a filtering process. Therefore, the scale S corrected by the scale error ⁇ S has higher accuracy.
  • the space vector Ve911c is obtained by correcting the space vector Ve911v obtained by the VSLAM operation to a highly accurate value according to the absolute coordinate system. Accordingly, the navigation device 10 can calculate the space vector Ve911v of the feature point 911 of the quay 910 in the absolute coordinate system with high accuracy.
  • the spatial information for the feature point 921v of the building 920v by the VSLAM operation is a space vector Ve921v starting from the reference point 90 (coordinate Po) and ending at the feature point 921v.
  • the space vector Ve921v includes indefiniteness inherent to the camera 30 as described in the above-described problem.
  • the scale S is a value for correcting the error of the VSLAM position with respect to the reference point 90 (coordinate Po) with high accuracy using the GNSS speed with high accuracy as described above.
  • the scale error ⁇ S is estimated with high accuracy by a filtering process. Therefore, the scale S corrected by the scale error ⁇ S has higher accuracy.
  • the space vector Ve921c is obtained by correcting the space vector Ve921v obtained by the VSLAM operation to a highly accurate value according to the absolute coordinate system, and the space vector Ve921v of the feature point 921 of the building 920 in the absolute coordinate system is highly accurate. Can be calculated.
  • the GNSS position correction unit 125 uses the error ⁇ Pg of the GNSS position Pg to determine the second time of its own device (more specifically, for example, the geometric center of the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23).
  • the GNSS position Pgj at tj is corrected. Since the error ⁇ Pg of the GNSS position Pg is estimated with high accuracy by the filter processing, the corrected GNSS position Pgjc has a high accuracy value.
  • the positions of the feature points 911 and 912 are calculated with high accuracy, and the position of the own device 91 is calculated with high accuracy.
  • the positions of the point 911 and the feature point 912 can be grasped with high accuracy.
  • the navigation device 10 can correct the error of the spatial information using the VSLAM operation with high accuracy, and can convert the spatial information around the ship 900 with a highly accurate real scale value. Can be obtained. Further, the navigation device 10 can calculate the position of the ship 900 with high accuracy. Thereby, the navigation device 10 can map the positional relationship between the ship 900 and the feature points around the ship 900, that is, the navigation support information with high accuracy.
  • the mode using the Kalman filter is shown.
  • the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg are estimated using other equations of the discrete time stochastic system. You may.
  • the correction information estimation unit 123 uses the GNSS attitude angle AAg and the VSLAM attitude angle AAv to add an error of an angle difference (VSLAM attitude angle offset angle) between the VSLAM attitude angle and the GNSS attitude angle AAg to the state variable. It is also possible to estimate the error of the angle difference.
  • FIG. 6 is a flowchart illustrating a processing flow of a method for generating navigation support information. The specific contents of each process have been described above, and description thereof will be omitted.
  • the arithmetic processing device calculates a GNSS operation value using the carrier phase in the GNSS signal (S11).
  • the GNSS calculation value includes the GNSS speed Vg and the GNSS attitude angle AAg.
  • the arithmetic processing unit performs a VSLAM operation from the moving image input from the camera, and estimates a VSLAM position, a VSLAM attitude angle, and spatial information (S12).
  • the arithmetic processing device executes the calculation of the GNSS operation value and the estimation of the VSLAM position and space information in a time-synchronized manner.
  • the time synchronization is not limited to the simultaneous execution, and the calculation of the GNSS calculation value and the estimation of the VSLAM position and space information are executed by attaching the time information respectively, and the synchronization at the same time is performed later. May be combined.
  • the arithmetic processing unit estimates the scale error ⁇ S and the observation value error by filter processing such as a Kalman filter (S13).
  • the observed value error is, for example, an error ⁇ Vg of the GNSS speed Vg and an error ⁇ Pg of the GNSS position Pg.
  • the arithmetic processing device corrects the GNSS observation value using the observation value error (S14), and corrects the spatial information using the scale S corrected by the scale error ⁇ S (S15).
  • FIG. 6 shows an aspect in which the correction based on the scale error ⁇ and the observation value error is sequentially performed ⁇ , but the correction may be performed at a preset time interval.
  • the scale S may be estimated.
  • the GNSS position Pg corrected by the error ⁇ Pg of the GNSS position Pg may be fed back to the VSLAM calculation unit 121.
  • the VSLAM calculation unit 121 estimates the VSLAM position Pv and the VSLAM attitude angle AAv by applying the corrected GNSS position Pg, that is, the high-precision GNSS position Pg to the constraint condition. Thereby, the VSLAM position Pv and the VSLAM attitude angle AAv can be estimated with higher accuracy.
  • FIG. 7 is a functional block diagram showing a configuration of the navigation device according to the second embodiment of the present invention.
  • FIG. 8 is a functional block diagram illustrating a configuration of the calculation unit according to the second embodiment of the present invention.
  • the navigation device 10A according to the second embodiment differs from the navigation device 10 according to the first embodiment in that a GNSS-IMU combined sensor 24 is used, and an operation associated therewith.
  • the configuration of the unit 12A is different.
  • the other configuration of the navigation device 10A is the same as that of the navigation device 10, and the description of the same parts is omitted.
  • the navigation device 10A includes a calculation unit 12A, a GNSS-IMU combined sensor 24, and a camera 30.
  • the GNSS-IMU combined sensor 24 and the camera 30 are connected to the calculation unit 12A.
  • the GNSS-IMU combined sensor 24 includes, for example, a set of, for example, two GNSS antennas and a GNSS receiving unit, and an inertial sensor that realizes an acceleration sensor and an angular velocity sensor.
  • the GNSS receiving section outputs GNSS observation data including at least the integrated value of the carrier phase.
  • the inertial sensor outputs IMU observation data including acceleration (IMU acceleration) and angular velocity (IMU angular velocity).
  • the calculation unit 12A includes a VSLAM calculation unit 121, a GNSS calculation unit 122, a correction information estimation unit 123A, a spatial information correction unit 124, a GNSS position correction unit 125, and an IMU calculation unit 126.
  • the VSLAM calculation unit 121 estimates the VSLAM position, the VSLAM attitude angle, and the spatial information as described above.
  • the VSLAM calculation unit 121 outputs the VSLAM position and the VSLAM attitude angle to the correction information estimation unit 123A as VSLAM estimation data.
  • the GNSS calculation unit 122 calculates the GNSS position and the GNSS speed as described above, and outputs the GNSS position and the GNSS speed to the correction information estimation unit 123A as GNSS observation data.
  • the IMU calculation unit 126 calculates the IMU attitude angle using the angular velocity included in the IMU observation data.
  • the IMU calculation unit 126 calculates the IMU speed and the IMU position using the acceleration included in the IMU observation data.
  • the IMU calculation unit 126 outputs the IMU attitude angle, the IMU speed, and the IMU position to the correction information estimation unit 123A as IMU observation data (IMU calculation value).
  • the correction information estimation unit 123A sets a system equation.
  • the state variables of this system equation include the above-described scale error ⁇ S, the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg, the calculation error of the angle difference between the VSLAM attitude angle and the GNSS attitude angle, and the calculation error of the GNSS observation data. , And the calculation error of the IMU observation data.
  • the correction information estimation unit 123A sets an observation equation using these as state variables and VSLAM estimation data, GNSS observation data, and IMU observation data as observation values.
  • the correction information estimation unit 123A performs Kalman filter processing using these system equations and observation equations. Accordingly, the correction information estimation unit 123A calculates the scale error ⁇ S, the error ⁇ Vg of the GNSS speed Vg, the calculation error of the angle difference between the VSLAM attitude angle and the GNSS attitude angle, the calculation error of the GNSS observation data, and the calculation of the IMU observation data. Estimate the error.
  • the correction information estimation unit 123A corrects the scale S using the scale error ⁇ S, and outputs the corrected scale S to the spatial information correction unit 124.
  • the spatial information correction unit 124 corrects the spatial information using the corrected scale S. In this description, the mode in which the scale error ⁇ S is estimated has been described, but the scale S may be estimated.
  • the correction information estimation unit 123A outputs the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg to the GNSS position correction unit 125.
  • the GNSS position correction unit 125 corrects the GNSS position Pg using the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg.
  • the correction information estimation unit 123A feeds back the scale error ⁇ S and the calculation error of the angle difference between the VSLAM attitude angle and the GNSS attitude angle to the VSLAM calculation unit 121.
  • the VSLAM calculation unit 121 reflects the scale error ⁇ S and the calculation error of the angle difference between the VSLAM attitude angle and the GNSS attitude angle, that is, the estimation error of the VSLAM estimation data, in the estimation of the VSLAM estimation data. As a result, the VSLAM estimation data converges to a highly accurate value.
  • the correction information estimation unit 123A feeds back the calculation error of the GNSS observation data to the GNSS calculation unit 122.
  • the GNSS calculation unit 122 reflects the calculation error of the GNSS observation data in the calculation of the GNSS observation data. As a result, the GNSS observation data converges to a highly accurate value.
  • the correction information estimation unit 123A feeds back the calculation error of the IMU observation data to the IMU calculation unit 126.
  • the IMU calculation unit 126 reflects the calculation error of the IMU observation data in the calculation of the IMU observation data. Thereby, the IMU observation data converges to a highly accurate value.
  • the accuracy of the VSLAM estimation data, the GNSS observation data, and the IMU observation data is improved. Therefore, the scale error ⁇ S and the error ⁇ Vg of the GNSS speed Vg or the error ⁇ Pg of the GNSS position Pg can be estimated with high accuracy.
  • the navigation device 10A can correct the spatial information with higher accuracy, and can correct its own position with higher accuracy. Therefore, the navigation device 10A can calculate the positional relationship between the position of the navigation device itself and each feature point of the spatial information, that is, the navigation support information with higher accuracy.
  • correction information estimation unit 123 uses the IMU attitude angle and the VSLAM attitude angle AAv to add an error of the angle difference (offset angle) between the VSLAM attitude angle AAv and the IMU attitude angle to the state variable, and calculates the angle difference.
  • the error can also be estimated.
  • FIG. 9 is a functional block diagram showing a configuration of the navigation device according to the third embodiment of the present invention.
  • FIG. 10 is a functional block diagram illustrating a configuration of a calculation unit according to the third embodiment of the present invention.
  • the navigation device 10B according to the third embodiment is different from the navigation device 10 according to the first embodiment in that the configuration and processing of an arithmetic unit 12B and a display 13 are provided. different.
  • the other configuration of the navigation device 10B is the same as that of the navigation device 10, and the description of the same parts is omitted.
  • the display 13 is composed of, for example, a liquid crystal display or the like, and is connected to the calculation unit 12B.
  • the calculation unit 12B includes a VSLAM calculation unit 121, a GNSS calculation unit 122, a correction information estimation unit 123, a spatial information correction unit 124, a GNSS position correction unit 125, a navigation support information generation unit 127, and a display.
  • An image generation unit 128 is provided.
  • the VSLAM calculation unit 121, the GNSS calculation unit 122, the correction information estimation unit 123, the spatial information correction unit 124, and the GNSS position correction unit 125 in the calculation unit 12B are the same as those of the calculation unit 12, and the description is omitted.
  • the navigation support information generation unit 127 generates navigation support information using the spatial information corrected by the scale S, the GNSS position, the GNSS speed, and the GNSS attitude angle corrected by the error ⁇ Vg or the error ⁇ Pg.
  • the navigation support information includes a horizontal grid, a predicted track, and the like.
  • the navigation support information generation unit 127 sets a horizontal plane using the VSLAM attitude angle and the GNSS attitude angle. More specifically, the navigation support information generation unit 127 calculates an angle difference (offset angle) between the VSLAM attitude angle and the GNSS attitude angle. The navigation support information generation unit 127 sets a correction angle so as to cancel this angle difference. The navigation support information generation unit 127 sets the horizontal plane (horizontal plane on the captured image) of the VSLAM coordinate system by rotating the horizontal plane set in the GNSS coordinate system by the correction angle. That is, the navigation support information generation unit 127 sets the horizontal plane of the VSLAM coordinate system by correcting the attitude angle of the horizontal plane with the correction angle.
  • the navigation support information generation unit 127 further sets a horizontal grid with reference to the horizontal plane.
  • the horizontal grid is represented by line segments parallel to the horizontal plane.
  • the navigation support information generation unit 127 can set the horizontal grid at intervals according to the actual dimensions of the space captured in the captured image by using the above-described scale S. Then, since the scale S is corrected with high precision as described above, the navigation support information generation unit 127 can generate a horizontal grid that exactly corresponds to the actual dimensions of the space captured in the captured image.
  • the navigation support information generation unit 127 calculates a predicted wake from a known method, using the corrected GNSS position, GNSS speed, and GNSS attitude angle.
  • the navigation support information generation unit 127 performs coordinate conversion to the VSLAM coordinate system by rotating the predicted wake at the above-described correction angle. Thereby, the navigation support information generation unit 127 can arrange the predicted wake with high accuracy on the horizontal plane. Further, by using this configuration and processing, even if the ship 900 is shaken, the predicted wake can be arranged with high accuracy on the horizontal plane.
  • the navigation support information generation unit 127 outputs the horizontal grid, the predicted wake, and the coordinates of the feature points included in the spatial information to the display image generation unit 128 as navigation support information.
  • the display image generation unit 128 generates a navigation support image as shown in FIG. 11A or 11B using the navigation support information and the captured image, and outputs the generated navigation support image to the display unit 13.
  • the display 13 displays the navigation support image.
  • FIGS. 11A and 11B are diagrams illustrating examples of navigation support images displayed by the navigation device according to the third embodiment.
  • a solid line indicates a horizontal grid
  • a dashed line indicates a predicted wake
  • ⁇ indicates a feature point.
  • FIG. 11A is a navigation support image in which a horizontal grid, a predicted wake, and a feature point are superimposed on a captured image.
  • the horizontal plane is set accurately
  • the horizontal grid is set with high accuracy to the scale of the captured image
  • the angle difference (offset angle) between the predicted wake and the horizontal plane is corrected. I have. Therefore, in the case of the navigation support image as shown in FIG. 11A, the horizontal grid 31, the predicted wake 32, and the feature point 33 are accurately superimposed on the captured image 301.
  • FIG. 11B is a navigation support image using an ortho image based on a horizontal plane based on a captured image.
  • the horizontal plane is set accurately, the horizontal grid is set with high accuracy to the scale of the captured image, and the angle difference (offset angle) between the predicted wake and the horizontal plane is corrected. I have. Therefore, in the case of the navigation support image as shown in FIG. 11B, the horizontal plane serving as the reference of the ortho image is accurately set, and the horizontal grid 31, the predicted wake 32, and the feature point 33 are accurately displayed on the ortho image 302. Superimposed.
  • the navigation device 10B can provide accurate navigation support information to the operator and the operator of the ship 900.
  • a window may be set in a part of the navigation support image in FIG. 11A, and the ortho image shown in FIG. 11B may be fitted in the window.
  • the navigation device 10B can provide various types of accurate navigation support information on a single screen to the operator and the operator of the ship 900.
  • the mode in which the estimated horizontal plane is corrected using the angle difference (offset angle) between the VSLAM attitude angle AAv and the GNSS attitude angle AAg has been described.
  • this angle difference it is also possible to omit this angle difference.
  • the predicted wake can be arranged with high accuracy on the horizontal plane.
  • FIG. 12 is a flowchart illustrating a processing flow of a method for generating navigation support information. The specific contents of each process have been described above, and description thereof will be omitted.
  • the arithmetic processing unit calculates the angle difference (offset angle) between the GNSS attitude angle or the IMU attitude angle calculated by the above-described method and the VSLAM attitude angle (S21).
  • the arithmetic processing unit sets a correction angle for canceling the angle difference (S22).
  • the arithmetic processing device sets a horizontal plane (horizontal plane in the VSLAM coordinate system) on the captured image using the correction angle (S23).
  • the arithmetic processing unit sets a horizontal grid using the scale S calculated and corrected as described above (S24).
  • the arithmetic processing unit calculates a predicted wake in the GNSS coordinate system (S25). Specifically, the predicted wake is calculated from a known method using the GNSS speed and the GNSS attitude angle.
  • the arithmetic processing unit performs coordinate conversion of the predicted wake using the correction angle (S26).
  • the arithmetic processing unit generates navigation support information including a horizontal grid, a predicted wake, and a feature point, and generates a display image using the navigation support information and the captured image.
  • the head-up display is performed with the direction extending straight from the bow as the center direction, but it is also possible to display the navigation coordinate system based on east, west, north and south.
  • the coordinate transformation of the horizontal grid, the predicted wake, and the feature point may be performed using GNSS observation values such as the GNSS attitude angle.
  • a horizontal plane is detected using a homography transformation using the height of the camera 30 and the GNSS attitude angle. You may. In this case, after the initialization of the VSLAM operation, the horizontal plane is detected using the spatial information as described above.
  • the horizontal plane may be set using, for example, feature points included in the spatial information and known information regarding the horizontal plane associated with the feature points, and may include a stereo camera, a radar, a laser scanner, a motion sensor, and the like. May be set using the output data of the sensor.
  • the configuration of the navigation device 10 according to the first embodiment is used as a part of the navigation device 10B according to the third embodiment.
  • the configuration of the navigation device 10A according to the second embodiment may be used as a part of the navigation device 10B according to the third embodiment.
  • the predicted wake can be arranged with higher accuracy with respect to the horizontal plane, and the positional relationship between the horizontal grid and the predicted wake can be higher.
  • All of the processes described herein can be embodied and fully automated by software code modules executed by a computing system including one or more computers or processors.
  • the code modules may be stored on any type of non-transitory computer readable media or other computer storage. Some or all of the methods may be embodied in dedicated computer hardware.
  • any particular operation, event, or function of the algorithms described herein can be performed in a different sequence, added, merged, or omitted altogether. (Eg, not all actions or events described are required to execute the algorithm). Further, in certain embodiments, the operations or events are performed in parallel rather than serially, for example, through multi-threading, interrupt processing, or over multiple processors or processor cores or on other parallel architectures. Can be. Further, different tasks or processes may be performed by different machines and / or computing systems that can function together.
  • the various illustrative logic blocks and modules described in connection with the embodiments disclosed herein may be implemented or performed by a machine such as a processor.
  • the processor may be a microprocessor, but, alternatively, the processor may be a controller, a microcontroller, or a state machine, or a combination thereof.
  • the processor can include an electrical circuit configured to process the computer-executable instructions.
  • the processor includes an application specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other programmable device that performs logical operations without processing computer-executable instructions.
  • ASIC application specific integrated circuit
  • FPGA field programmable gate array
  • a processor may also be a combination of computing devices, such as a combination of a digital signal processor (digital signal processor) and a microprocessor, multiple microprocessors, one or more microprocessors in combination with a DSP core, or any other such device. Such a configuration can be implemented. Although described herein primarily in terms of digital technology, a processor may include primarily analog components. For example, some or all of the signal processing algorithms described herein can be implemented with analog circuits or mixed analog and digital circuits.
  • a computing environment includes any type of computer system including, but not limited to, a microprocessor, a mainframe computer, a digital signal processor, a portable computing device, a device controller, or a computer system based on a computing engine in the apparatus. be able to.
  • conditional languages such as “could”, “could”, “will” or “possibly” refer to certain embodiments including certain features, elements and / or steps
  • Embodiments are understood in the context generally used to convey that they do not. Accordingly, such a conditional language is generally either a way in which the features, elements and / or steps are required in one or more embodiments, or one or more embodiments , Elements and / or steps are not necessarily meant to necessarily include the logic to determine whether they are included or performed in any particular embodiment.
  • a disjunctive language such as the phrase "at least one of X, Y, and Z" is used when the item, term, or the like is X, Y, Z, or any combination thereof, unless otherwise specified. Is understood in the context that is commonly used to indicate that it can be (eg, X, Y, Z). Thus, such disjunctive languages generally require each of at least one of X, at least one of Y, or at least one of Z, where a particular embodiment is present, respectively. It does not mean.
  • ⁇ Unless otherwise indicated, numbers such as “one” should generally be construed as including one or more of the stated items. Thus, phrases such as "a single device configured to" are intended to include one or more of the listed devices. Such one or more listed devices may also be collectively configured to perform the recited citations. For example, ⁇ a processor configured to execute A, B and C below '' includes a first processor configured to execute A and a second processor configured to execute B and C. Processor. In addition, even though a specific number of enumerations of an introduced example is explicitly recited, one of ordinary skill in the art would typically recognize at least the enumeration of such enumerations (e.g., other modifiers). The mere enumeration of "with two enumerations" without (usually) should normally be taken to mean at least two enumerations, or more than one enumeration).
  • the term “floor” can be interchanged with the terms “ground” or “water surface”.
  • the term “vertical / vertical” refers to the direction perpendicular / vertical to the defined horizontal line. Terms such as “upper”, “lower”, “lower”, “upper”, “side”, “higher”, “lower”, “upper”, “beyond” and “below” are defined relative to the horizontal plane. ing.
  • connection As used herein, the terms “attach,” “connect,” “pair,” and other related terms, unless otherwise noted, are removable, movable, fixed, adjustable, And / or should be construed to include a removable connection or connection. Connections / connections include direct connections and / or connections having an intermediate structure between the two components described.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

【課題】ＶＳＬＡＭを用いながら、自装置と他の特徴点等との間の絶対的な位置関係を高精度に算出する。 【解決手段】航法装置は、ＧＮＳＳ演算部、ＶＳＬＡＭ演算部、および、補正情報推定部を備える。ＧＮＳＳ演算部は、測位衛星から送信されるＧＮＳＳ信号の位相の変化量またはＧＮＳＳ信号の周波数の変化量を用いてＧＮＳＳ速度およびＧＮＳＳ姿勢角を算出する。ＶＳＬＡＭ演算部は、動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定する。補正情報推定部は、ＧＮＳＳ速度、ＧＮＳＳ姿勢角、複数の時刻での前記ＶＳＬＡＭ位置、および、ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対するＶＳＬＡＭ位置のスケールまたはスケール誤差と、ＧＮＳＳ速度の誤差またはＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差と、を推定する。

Description

航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム
　本発明は、ＶＳＬＡＭ（Ｖｉｓｕａｌ　Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）の技術を利用した航法装置に関する。
　現在、ＶＳＬＡＭ（Ｖｉｓｕａｌ　Ｓｉｍｕｌｔａｎｅｏｕｓ　Ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　Ｍａｐｐｉｎｇ）の技術が多方面で利用されている。
　ＶＳＬＡＭは、単眼カメラで取得した動画像を用いて、自装置の周囲にある特徴点等の空間情報を推定し、例えば、マッピングを行っている。また、ＶＳＬＡＭは、この空間情報およびマッピング結果を用いて、自装置の相対位置等を算出している。
Ｇｅｏｒｇ　Ｋｌｅｉｎ、Ｄａｖｉｄ　Ｍｕｒｒａｙ、"Ｐａｒａｌｌｅｌ　Ｔｒａｃｋｉｎｇ　ａｎｄ　Ｍａｐｐｉｎｇ　ｆｏｒ　Ｓｍａｌｌ　ＡＲ　Ｗｏｒｋｓｐａｃｅ"、Ａｃｔｉｖｅ　Ｖｉｓｉｏｎ　Ｌａｂｏｒａｔｏｒｙ　Ｄｅｐａｒｔｍｅｎｔ　ｏｆ　Ｅｎｇｉｎｅｅｒｉｎｇ　Ｓｃｉｅｎｃｅ　Ｕｎｉｖｅｒｓｉｔｙ　ｏｆ　Ｏｘｆｏｒｄ Ｔａｋａｆｕｍｉ　Ｔａｋｅｔｏｍｉ、Ｈｉｄｅａｋｉ　Ｕｃｈｉｙａｍａ　ａｎｄ　Ｓｅｉ　Ｉｋｅｄａ、"Ｖｉｓｕａｌ－ＳＬＡＭ　Ａｌｇｏｒｉｔｈｍｓ：ａ　Ｓｕｒｖｅｙ　ｆｒｏｍ　２０１０　ｔｏ　２０１６"
　しかしながら、ＶＳＬＡＭでは、自装置と他の特徴点との間の相対的な位置関係は、算出できるが、絶対的な位置関係は、不定性を有する。
　したがって、本発明の目的は、ＶＳＬＡＭを用いながら、自装置と他の特徴点等との間の絶対的な位置関係を高精度に算出する技術を提供することにある。
　この発明の航法装置は、ＧＮＳＳ演算部、ＶＳＬＡＭ演算部、および、補正情報推定部を備える。ＧＮＳＳ演算部は、測位衛星から送信されるＧＮＳＳ信号の位相の変化量またはＧＮＳＳ信号の周波数の変化量を用いてＧＮＳＳ速度およびＧＮＳＳ姿勢角を算出する。ＶＳＬＡＭ演算部は、動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置および複数の時刻でのＶＳＬＡＭ姿勢角を推定する。補正情報推定部は、ＧＮＳＳ速度、ＧＮＳＳ姿勢角、複数の時刻での前記ＶＳＬＡＭ位置、および、ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対するＶＳＬＡＭ位置のスケールまたはスケール誤差と、ＧＮＳＳ速度の誤差またはＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差と、を推定する。
　この構成では、ＶＳＬＡＭ位置のスケールまたはスケール誤差と、自装置位置の誤差とが高精度に補正される。
　この発明によれば、ＶＳＬＡＭを用いながら、自装置と他の特徴点等との間の絶対的な位置関係を高精度に算出できる。
本発明の第１の実施形態に係る航法装置における演算部の構成を示す機能ブロック図である。 本発明の第１の実施形態に係る航法装置の構成を示す機能ブロック図である。 本発明の第１の実施形態に係る補正情報推定部の構成を示す機能ブロック図である。 本発明の第１の実施形態に係る空間情報およびＧＮＳＳ位置の補正の概念を説明するための図である。 ＧＮＳＳ速度およびＧＮＳＳ位置の誤差の推移を示すグラフである。 航法支援情報の生成方法の処理フローを示すフローチャートである。 本発明の第２の実施形態に係る航法装置の構成を示す機能ブロック図である。 本発明の第２の実施形態に係る演算部の構成を示す機能ブロック図である。 本発明の第３の実施形態に係る航法装置の構成を示す機能ブロック図である。 本発明の第３の実施形態に係る演算部の構成を示す機能ブロック図である。 （Ａ）、（Ｂ）は、第３の実施形態に係る航法装置で表示する航法支援画像の例を示す図である。 航法支援情報の生成方法の処理フローを示すフローチャートである。
　本発明の第１の実施形態に係る航法装置、航法支援方法、および、航法支援プログラムについて、図を参照して説明する。なお、以下では、移動体として船舶を用いる態様を示すが、他の水上、水中移動体、陸上移動体、または、空中移動体にも、本願発明の構成は適用できる。
　図１は、本発明の第１の実施形態に係る航法装置における演算部の構成を示す機能ブロック図である。図２は、本発明の第１の実施形態に係る航法装置の構成を示す機能ブロック図である。図３は、本発明の第１の実施形態に係る補正情報推定部の構成を示す機能ブロック図である。
　（航法装置１０の概要）
　図２に示すように、航法装置１０は、ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、ＧＮＳＳ受信部１１３、演算部１２、ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２、ＧＮＳＳアンテナ２３、および、カメラ３０を備える。なお、航法装置１０は、少なくとも演算部１２を備えていればよい。
　ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２、および、ＧＮＳＳアンテナ２３は、航法装置１０が装着される船舶（図示せず。）に設置されている。ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２、および、ＧＮＳＳアンテナ２３いわゆるオープンスカイの環境に設置されている。ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２、および、ＧＮＳＳアンテナ２３は、全てが同時に一直線上に並ばないように配置されている。すなわち、例えば、ＧＮＳＳアンテナ２３は、ＧＮＳＳアンテナ２１とＧＮＳＳアンテナ２２とを結ぶ直線上に配置されていない。
　ＧＮＳＳアンテナ２１は、図示しないＧＮＳＳ衛星からのＧＮＳＳ信号を受信して、ＧＮＳＳ受信部１１１に出力する。ＧＮＳＳアンテナ２２は、図示しないＧＮＳＳ衛星からのＧＮＳＳ信号を受信して、ＧＮＳＳ受信部１１２に出力する。ＧＮＳＳアンテナ２３は、図示しないＧＮＳＳ衛星からのＧＮＳＳ信号を受信して、ＧＮＳＳ受信部１１３に出力する。
　ＧＮＳＳ信号は、所定の周波数からなる搬送波信号に、ＰＲＮ（擬似ランダムノイズ）コード、および、航法メッセージデータが重畳された信号である。ＰＲＮコードは、送信元の測位衛星を識別するコードである。航法メッセージデータは、測位衛星の軌道情報、補正情報等を含むデータである。
　ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３は、それぞれに、ＧＮＳＳ信号の捕捉、追尾処理等のプログラムを記憶したメモリと、該プログラムを実行するＩＣ等の演算素子とによって構成されている。ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３は、取得したＧＮＳＳ信号を、既知の方法によって捕捉、追尾する。この際、ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３は、コードおよび搬送波信号（キャリア信号）の捕捉、追尾を行う。
　ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３は、捕捉、追尾によって、観測データを取得し、演算部１２に出力する。観測データは、少なくとも、搬送波位相の積算値（ＡＤＲ）を含んでいる。ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３は、この搬送波位相の積算値の取得を、例えば、エポック毎等、ＧＮＳＳの時刻に同期して所定の時間間隔で逐次的に実行する。なお、ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３は、搬送波信号のドップラ周波数、コード位相、コード位相によって算出される擬似距離、測位位置等を、観測データとして演算部１２に出力することも可能である。
　カメラ３０は、例えば、単眼のカメラである。カメラ３０は、航法装置１０が装着される船舶の周囲を撮像し、動画像を生成する。カメラ３０は、動画像を演算部１２に出力する。
　演算部１２は、ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３からの観測データ、および、カメラ３０からの動画像を用いて、絶対座標系の位置に対するＶＳＬＡＭ位置のスケール誤差（スケール誤差）と、ＧＮＳＳ速度の誤差またはＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差と、を推定する。また、演算部１２は、動画像から得られる空間情報を、スケール誤差によって補正したスケールを用いて補正する。また、演算部１２は、ＧＮＳＳ速度の誤差またはＧＮＳＳ位置の誤差を用いて、航法装置１０の位置（自装置位置）を補正する。
　（演算部１２の具体的な構成および処理）
　演算部１２は、後述するＶＳＬＡＭ演算、ＧＮＳＳ演算値の算出、スケール誤差の推定、自装置位置の誤差の推定、空間情報の補正、自装置位置の補正等の処理のプログラムを記憶するメモリと、当該プログラムを実行するＣＰＵ等の演算素子とによって構成されている。
　機能ブロックで示すと、図１に示すように、演算部１２は、ＶＳＬＡＭ演算部１２１、ＧＮＳＳ演算部１２２、補正情報推定部１２３、空間情報補正部１２４、および、ＧＮＳＳ位置補正部１２５を備える。なお、演算部１２は、ＶＳＬＡＭ演算部１２１、ＧＮＳＳ演算部１２２、および、補正情報推定部１２３を少なくとも備えていればよい。
　ＶＳＬＡＭ演算部１２１は、取得した動画像を用いて既知の方法によって、基準点（図４の９０（座標Ｐｏ）参照）に対する自装置の位置（ＶＳＬＡＭ位置）、および、各特徴点の空間情報を推定する。この際、ＶＳＬＡＭ演算部１２１は、ＧＮＳＳ時刻に同期して、基準点に対する自装置の位置（ＶＳＬＡＭ位置）、および、各特徴点の空間情報を推定する。すなわち、ＶＳＬＡＭ演算部１２１は、搬送波位相の積算値の取得時刻に同期しながら、動画像の取得時刻毎に、基準点に対する自装置の位置であるＶＳＬＡＭ位置Ｐｖ、自装置のＶＳＬＡＭ姿勢角ＡＡｖ、および、各特徴点の空間情報を推定する。例えば、ＶＳＬＡＭ演算部１２１は、後述のＧＮＳＳ速度の算出時刻に同期して、ＶＳＬＡＭ位置Ｐｖ、ＶＳＬＡＭ姿勢角ＡＡｖ、および、各特徴点の空間情報を推定する。なお、空間情報は、ＶＳＬＡＭ演算の基準点を基点とする特徴点の空間ベクトルによって表される。
　ＶＳＬＡＭ演算部１２１は、ＶＳＬＡＭ位置Ｐｖ、および、ＶＳＬＡＭ姿勢角ＡＡｖを、補正情報推定部１２３および空間情報補正部１２４に出力する。また、ＶＳＬＡＭ演算部１２１は、空間情報を、空間情報補正部１２４に出力する。この際、ＶＳＬＡＭ演算部１２１は、ＶＳＬＡＭ位置Ｐｖ、ＶＳＬＡＭ姿勢角ＡＡｖ、および、空間情報に対して推定時刻を関連づけして出力する。
　ＧＮＳＳ演算部１２２は、ＧＮＳＳ速度算出部１２２１、および、ＧＮＳＳ姿勢角算出部１２２２を備える。
　ＧＮＳＳ速度算出部１２２１は、ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、または、ＧＮＳＳ受信部１１３で取得した搬送波位相の積算値の変化量を用いて、ＧＮＳＳ速度Ｖｇを算出する。この際、ＧＮＳＳ速度算出部１２２１は、ＧＮＳＳ時刻に同期した所定の時間間隔で、ＧＮＳＳ速度Ｖｇを算出する。搬送波位相の積算値の変化量を用いることによって、ＧＮＳＳ速度算出部１２２１は、ＧＮＳＳ速度Ｖｇを、高速かつ高精度に算出できる。すなわち、ＧＮＳＳ速度算出部１２２１は、整数値バイアスの決定等の処理、外部からの補正情報の取得を行うことなく、ＧＮＳＳ速度Ｖｇを、高速かつ高精度に算出できる。
　ＧＮＳＳ速度算出部１２２１は、ＧＮＳＳ速度Ｖｇを、補正情報推定部１２３に出力する。ＧＮＳＳ速度算出部１２２１は、ＧＮＳＳ速度Ｖｇに算出時刻を関連づけして出力する。
　ＧＮＳＳ姿勢角算出部１２２２は、ＧＮＳＳ受信部１１１、ＧＮＳＳ受信部１１２、および、ＧＮＳＳ受信部１１３のそれぞれで取得した搬送波位相の積算値を用いて、既知の方法（例えば、ＧＮＳＳアンテナ間の基線ベクトルを用いた方法等）から、ＧＮＳＳ姿勢角ＡＡｇを算出する。搬送波位相の積算値を用いることによって、ＧＮＳＳ姿勢角ＡＡｇは、高精度に算出される。
　ＧＮＳＳ姿勢角算出部１２２２は、ＧＮＳＳ姿勢角ＡＡｇを、補正情報推定部１２３に出力する。この際、ＧＮＳＳ姿勢角算出部１２２２は、ＧＮＳＳ姿勢角ＡＡｇに対して、算出時刻を関連付けして出力する。
　補正情報推定部１２３は、スケールＳを算出する。補正情報推定部１２３は、ＶＳＬＡＭ位置Ｐｖ、ＶＳＬＡＭ姿勢角ＡＡｖ、ＧＮＳＳ速度Ｖｇ、および、ＧＮＳＳ姿勢角ＡＡｇを用いて、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇを推定する。なお、補正情報推定部１２３は、ＧＮＳＳ速度Ｖｇの誤差δＶｇに代えて、ＧＮＳＳ位置Ｐｇの誤差δＰｇを推定してもよい。ＧＮＳＳ位置Ｐｇは、例えば、基準点の座標を原点として、ＧＮＳＳ速度Ｖｇを積分することによって得られる。
　補正情報推定部１２３は、スケール誤差δＳを用いて、スケールＳを補正し、補正後のスケールＳを、空間情報補正部１２４に出力する。また、補正情報推定部１２３は、ＧＮＳＳ速度Ｖｇの誤差δＶｇ、または、ＧＮＳＳ位置Ｐｇの誤差δＰｇを、ＧＮＳＳ位置補正部１２５に出力する。なお、本説明では、スケール誤差δＳを推定する態様を示したが、スケールＳを推定してもよい。
　（補正情報推定部１２３の具体的な構成および処理）
　図３に示すように、補正情報推定部１２３は、回転行列算出部１２３１、および、推定演算部１２３２を備える。
　回転行列算出部１２３１は、ＶＳＬＡＭ姿勢角ＡＡｖとＧＮＳＳ姿勢角ＡＡｇとを用いて、ＶＳＬＡＭの座標系とＧＮＳＳの座標系との変換を行う回転行列Ｃを算出する。回転行列算出部１２３１は、回転行列Ｃを推定演算部１２３２に出力する。
　推定演算部１２３２は、スケールＳの算出と、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇを推定する。
　（スケールＳの算出）
　推定演算部１２３２は、ＧＮＳＳ速度Ｖｇを積算してＧＮＳＳ距離Ｄｇｉｊを算出する。補正情報推定部１２３は、ＧＮＳＳ距離Ｄｇｉｊの算出対象時間と同じ時間におけるＶＳＬＡＭ位置Ｐｖの変化量を用いて、ＶＳＬＡＭ距離Ｄｖｉｊを算出する。
　推定演算部１２３２は、ＧＮＳＳ距離Ｄｇｉｊの絶対値ＡＢＳ（Ｄｇｉｊ）を、ＶＳＬＡＭ距離Ｄｖｉｊの絶対値ＡＢＳ（Ｄｖｉｊ）で除算することによって、スケールＳを算出する。すなわち、推定演算部１２３２は、Ｓ＝ＡＢＳ（Ｄｇｉｊ）／ＡＢＳ（Ｄｖｉｊ）の演算を行うことによって、スケールＳを算出する。
　（スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇの推定）
　推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇを状態変数とするシステム方程式を設定する。また、推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇを状態変数とし、ＶＳＬＡＭ位置ＰｖおよびＧＮＳＳ速度Ｖｇを観測値として、回転行列Ｃを変換行列に含む、観測方程式を設定する。
　推定演算部１２３２は、これらシステム方程式と観測方程式とを用いて、カルマンフィルタ処理を実行する。これにより、推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇを推定する。
　このようなフィルタ処理を適用することによって、推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇを高精度に推定できる。
　（スケール誤差δＳ、および、ＧＮＳＳ位置Ｐｇの誤差δＰｇの推定）
　推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ位置Ｐｇの誤差δＰｇを状態変数とするシステム方程式を設定する。また、推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ位置Ｐｇの誤差δＰｇを状態変数とし、ＶＳＬＡＭ位置ＰｖおよびＧＮＳＳ速度Ｖｇを観測値として、回転行列Ｃを変換行列に含む、観測方程式を設定する。
　推定演算部１２３２は、これらシステム方程式と観測方程式とを用いて、カルマンフィルタ処理を実行する。これにより、推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ位置Ｐｇの誤差δＰｇを推定する。
　このようなフィルタ処理を適用することによって、推定演算部１２３２は、スケール誤差δＳ、および、ＧＮＳＳ位置Ｐｇの誤差δＰｇを高精度に推定できる。
　なお、本説明では、スケール誤差δＳを推定する態様を示したが、スケールＳを推定してもよい。この場合、スケール誤差δＳに代えて、スケールＳを状態変数として、システム方程式および観測方程式を設定すればよい。
　（空間情報補正部１２４およびＧＮＳＳ位置補正部１２５の処理）
　空間情報補正部１２４は、スケールＳを用いて、空間情報を補正する。ここで、スケール誤差δＳが高精度に推定されることによって、スケールＳは高精度に補正される。したがって、空間情報補正部１２４は、空間情報を高精度に補正できる。
　ＧＮＳＳ位置補正部１２５は、ＧＮＳＳ速度Ｖｇを積分することによって、ＧＮＳＳ位置Ｐｇを算出する。ここで、ＧＮＳＳ速度Ｖｇの誤差δＶｇを用いる場合、ＧＮＳＳ位置補正部１２５は、ＧＮＳＳ速度Ｖｇを誤差δＶｇで補正した速度Ｖｇｃを用いて、ＧＮＳＳ位置Ｐｇを算出して出力する。また、ＧＮＳＳ位置Ｐｇの誤差δＰｇを用いる場合、ＧＮＳＳ位置補正部１２５は、ＧＮＳＳ速度Ｖｇを用いて算出したＧＮＳＳ位置Ｐｇを誤差δＰｇで補正したＧＮＳＳ位置Ｐｇｃを出力する。ここで、ＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇが高精度に推定されることによって、ＧＮＳＳ位置補正部１２５は、ＧＮＳＳ位置Ｐｇを高精度に補正できる。
　図４は、ＧＮＳＳ速度の積算誤差と補正後のＧＮＳＳ位置の誤差との時間推移を示すグラフである。図４に示すように、ＧＮＳＳ速度Ｖｇの積算誤差が経過時間とともに大きくなっても、補正後のＧＮＳＳ位置Ｐｇｃの誤差は、小さく抑制される。
　そして、空間情報が高精度になり、補正後のＧＮＳＳ位置Ｐｇｃが高精度になることによって、自装置の位置と空間情報の各特徴点との位置関係は、高精度になる。すなわち、航法装置１０は、自装置の位置と空間情報の各特徴点との位置関係を高精度に算出できる。
　図５は、本発明の第１の実施形態に係る空間情報およびＧＮＳＳ位置の補正の概念を説明するための図である。
　なお、以下では、ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２、ＧＮＳＳアンテナ２３およびカメラ３０は、船舶９００の座標系において、略同じ位置（観測位置）９１に配置されているものとして説明する。より具体的には、ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２、ＧＮＳＳアンテナ２３の幾何学的中心とカメラ３０とは、少なくとも、船体座標系の水平面上で、同じ位置に配置されているものとする。
　また、図５では、説明を分かりやすくするために、第１時刻ｔｉでのＶＳＬＡＭ位置とＧＮＳＳ位置とが一致するように記載しているが、これに限ることなく、以下の処理は実現できる。
　例えば、図５の例であれば、ＶＳＬＡＭ演算による岸壁９１０ｖの特徴点９１１ｖに対する空間情報は、基準点９０（座標Ｐｏ）を起点として特徴点９１１ｖを終点とする空間ベクトルＶｅ９１１ｖである。しかしながら、この空間ベクトルＶｅ９１１ｖには、上述の課題に示したようなカメラ３０に固有の不定性が含まれている。
　空間情報補正部１２４は、このＶＳＬＡＭ演算による空間ベクトルＶｅ９１１ｖに対して、スケールＳを乗算することによって、絶対座標系の空間ベクトルＶｅ９１１ｃを算出する。すなわち、空間情報補正部１２４は、Ｖｅ９１１ｃ＝Ｓ・Ｖｅ９１１ｖの演算を実施する。ここで、スケールＳは、上述のように、高精度なＧＮＳＳ速度を用いて、基準点９０（座標Ｐｏ）に対するＶＳＬＡＭ位置の誤差を高精度に補正する値である。さらに、スケール誤差δＳは、フィルタ処理によって高精度に推定されている。したがって、このスケール誤差δＳによって補正されたスケールＳは、さらに高精度になる。したがって、空間ベクトルＶｅ９１１ｃは、ＶＳＬＡＭ演算による空間ベクトルＶｅ９１１ｖを、絶対座標系に準じた高精度な値に補正したものとなる。これにより、航法装置１０は、絶対座標系における岸壁９１０の特徴点９１１の空間ベクトルＶｅ９１１ｖを高精度に算出できる。
　また、図５の例であれば、ＶＳＬＡＭ演算による建築物９２０ｖの特徴点９２１ｖに対する空間情報は、基準点９０（座標Ｐｏ）を起点として特徴点９２１ｖを終点とする空間ベクトルＶｅ９２１ｖである。しかしながら、この空間ベクトルＶｅ９２１ｖには、上述の課題に示したようなカメラ３０に固有の不定性が含まれている。
　空間情報補正部１２４は、スケールＳを用いて、Ｖｅ９２１ｃ＝Ｓ・Ｖｅ９２１ｖの演算を実施する。ここで、スケールＳは、上述のように、高精度なＧＮＳＳ速度を用いて、基準点９０（座標Ｐｏ）に対するＶＳＬＡＭ位置の誤差を高精度に補正する値である。さらに、スケール誤差δＳは、フィルタ処理によって高精度に推定されている。したがって、このスケール誤差δＳによって補正されたスケールＳは、さらに高精度になる。これにより、空間ベクトルＶｅ９２１ｃは、ＶＳＬＡＭ演算による空間ベクトルＶｅ９２１ｖを、絶対座標系に準じた高精度な値に補正したものとなり、絶対座標系における建築物９２０の特徴点９２１の空間ベクトルＶｅ９２１ｖを高精度に算出できる。
　さらに、ＧＮＳＳ位置補正部１２５は、ＧＮＳＳ位置Ｐｇの誤差δＰｇによって、自装置（より具体的には、例えば、ＧＮＳＳアンテナ２１、ＧＮＳＳアンテナ２２およびＧＮＳＳアンテナ２３の幾何学的中心）９１の第２時刻ｔｊにおけるＧＮＳＳ位置Ｐｇｊを補正する。ＧＮＳＳ位置Ｐｇの誤差δＰｇは、フィルタ処理によって高精度に推定されているので、補正後のＧＮＳＳ位置Ｐｇｊｃは、高精度な値となる。
　このように、特徴点９１１、特徴点９１２の位置が高精度に算出され、自装置９１の位置が高精度に算出されることによって、自装置９１すなわち航法装置１０を装着した船舶９００と、特徴点９１１および特徴点９１２との位置とを高精度に把握することができる。
　すなわち、本実施形態の構成を用いることによって、航法装置１０は、ＶＳＬＡＭ演算を用いた空間情報の誤差を高精度に補正でき、船舶９００の周囲の空間情報を、高精度な実スケールの値によって取得できる。さらに、航法装置１０は、船舶９００の位置を高精度に算出できる。これにより、航法装置１０は、船舶９００と、船舶９００の周囲の特徴点との位置関係、すなわち航法支援情報を、高精度にマッピングできる。
　なお、上述の説明では、カルマンフィルタを用いる態様を示したが、離散時間確率システムの他の方程式を用いて、スケール誤差δＳ、および、ＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇを推定してもよい。
　また、補正情報推定部１２３は、ＧＮＳＳ姿勢角ＡＡｇとＶＳＬＡＭ姿勢角ＡＡｖとを用いて、状態変数にＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角ＡＡｇとの角度差（ＶＳＬＡＭ姿勢角のオフセット角）の誤差を加え、当該角度差の誤差を推定することもできる。
　上述の説明では、演算部１２を複数の機能ブロックに分けて実現する態様を示したが、図６に示す処理を実現するプログラムおよび当該プログラムを実行する演算処理装置のみによって実現してもよい。図６は、航法支援情報の生成方法の処理フローを示すフローチャートである。なお、各処理の具体的な内容は、上述しており、説明は省略する。
　演算処理装置は、ＧＮＳＳ信号における搬送波位相を用いて、ＧＮＳＳ演算値を算出する（Ｓ１１）。ＧＮＳＳ演算値は、ＧＮＳＳ速度ＶｇおよびＧＮＳＳ姿勢角ＡＡｇを含む。
　演算処理装置は、カメラから入力された動画像からＶＳＬＡＭ演算を実行し、ＶＳＬＡＭ位置、ＶＳＬＡＭ姿勢角、および、空間情報を推定する（Ｓ１２）。
　演算処理装置は、ＧＮＳＳ演算値の算出と、ＶＳＬＡＭ位置および空間情報の推定とを、時刻同期して実行する。ここで、時刻同期とは、同時並行に実行するものに限らず、ＧＮＳＳ演算値の算出と、ＶＳＬＡＭ位置および空間情報の推定とを、それぞれ時刻情報を添付して実行し、後に同じ時刻のものを合わせてもよい。
　演算処理装置は、スケール誤差δＳ、および、観測値誤差を、カルマンフィルタ等のフィルタ処理によって推定する（Ｓ１３）。ここで、観測値誤差は、例えば、ＧＮＳＳ速度Ｖｇの誤差δＶｇ、ＧＮＳＳ位置Ｐｇの誤差δＰｇである。
　演算処理装置は、観測値誤差を用いてＧＮＳＳ観測値を補正し（Ｓ１４）、スケール誤差δＳによって補正したスケールＳによって空間情報を補正する（Ｓ１５）。
　なお、図６では、スケール誤差δおよび観測値誤差による補正を逐次的に実行する態様　　を示したが、予め設定した時間間隔で行ってもよい。また、ここでは、スケール誤差δＳを推定する方法を示したが、スケールＳを推定してもよい。
　また、上述の説明では、ＧＮＳＳ位置Ｐｇの誤差δＰｇによって補正されたＧＮＳＳ位置Ｐｇは、ＶＳＬＡＭ演算部１２１にフィードバックしてもよい。この場合、ＶＳＬＡＭ演算部１２１は、この補正されたＧＮＳＳ位置Ｐｇ、すなわち、高精度なＧＮＳＳ位置Ｐｇを拘束条件に適用して、ＶＳＬＡＭ位置ＰｖおよびＶＳＬＡＭ姿勢角ＡＡｖを推定する。これにより、ＶＳＬＡＭ位置ＰｖおよびＶＳＬＡＭ姿勢角ＡＡｖを、さらに高精度に推定できる。
　次に、本発明の第２の実施形態に係る航法装置について、図を参照して説明する。図７は、本発明の第２の実施形態に係る航法装置の構成を示す機能ブロック図である。図８は、本発明の第２の実施形態に係る演算部の構成を示す機能ブロック図である。
　図７、図８に示すように、第２の実施形態に係る航法装置１０Ａは、第１の実施形態に係る航法装置１０に対して、ＧＮＳＳ－ＩＭＵ複合センサ２４を用いる点、これに伴う演算部１２Ａの構成において異なる。航法装置１０Ａの他の構成は、航法装置１０と同様であり、同様の箇所の説明は省略する。
　図７に示すように、航法装置１０Ａは、演算部１２Ａ、ＧＮＳＳ－ＩＭＵ複合センサ２４、および、カメラ３０を備える。ＧＮＳＳ－ＩＭＵ複合センサ２４、および、カメラ３０は、演算部１２Ａに接続されている。
　ＧＮＳＳ－ＩＭＵ複合センサ２４は、例えば、例えば、２個のＧＮＳＳアンテナおよびＧＮＳＳ受信部の組と、加速度センサおよび角速度センサを実現する慣性センサとを備える。ＧＮＳＳ受信部は、少なくとも搬送波位相の積算値を含むＧＮＳＳ観測データを出力する。慣性センサは、加速度（ＩＭＵ加速度）および角速度（ＩＭＵ角速度）を含むＩＭＵ観測データを出力する。
　演算部１２Ａは、ＶＳＬＡＭ演算部１２１、ＧＮＳＳ演算部１２２、補正情報推定部１２３Ａ、空間情報補正部１２４、ＧＮＳＳ位置補正部１２５、および、ＩＭＵ演算部１２６を備える。
　ＶＳＬＡＭ演算部１２１は、上述のように、ＶＳＬＡＭ位置、ＶＳＬＡＭ姿勢角、および、空間情報を推定する。ＶＳＬＡＭ演算部１２１は、ＶＳＬＡＭ位置、および、ＶＳＬＡＭ姿勢角を、ＶＳＬＡＭ推定データとして補正情報推定部１２３Ａに出力する。
　ＧＮＳＳ演算部１２２は、上述のように、ＧＮＳＳ位置、および、ＧＮＳＳ速度を算出し、ＧＮＳＳ観測データとして補正情報推定部１２３Ａに出力する。
　ＩＭＵ演算部１２６は、ＩＭＵ観測データに含まれる角速度を用いて、ＩＭＵ姿勢角を算出する。ＩＭＵ演算部１２６は、ＩＭＵ観測データに含まれる加速度を用いて、ＩＭＵ速度、および、ＩＭＵ位置を算出する。ＩＭＵ演算部１２６は、ＩＭＵ姿勢角、ＩＭＵ速度、および、ＩＭＵ位置を、ＩＭＵ観測データ（ＩＭＵ演算値）として補正情報推定部１２３Ａに出力する。
　補正情報推定部１２３Ａは、システム方程式を設定する。このシステム方程式の状態変数は、上述のスケール誤差δＳおよびＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇとともに、ＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角との角度差の算出誤差、ＧＮＳＳ観測データの算出誤差、および、ＩＭＵ観測データの算出誤差を含む。また、補正情報推定部１２３Ａは、これらを状態変数とし、ＶＳＬＡＭ推定データ、ＧＮＳＳ観測データ、および、ＩＭＵ観測データを観測値として、観測方程式を設定する。
　補正情報推定部１２３Ａは、これらのシステム方程式と観測方程式とを用いて、カルマンフィルタ処理を実行する。これにより、補正情報推定部１２３Ａは、スケール誤差δＳ、ＧＮＳＳ速度Ｖｇの誤差δＶｇ、ＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角との角度差の算出誤差、ＧＮＳＳ観測データの算出誤差、および、ＩＭＵ観測データの算出誤差を推定する。
　補正情報推定部１２３Ａは、スケール誤差δＳを用いてスケールＳを補正し、補正後のスケールＳを、空間情報補正部１２４に出力する。空間情報補正部１２４は、補正後のスケールＳを用いて、空間情報を補正する。なお、この説明では、スケール誤差δＳを推定する態様を示したが、スケールＳを推定してもよい。
　補正情報推定部１２３Ａは、ＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇを、ＧＮＳＳ位置補正部１２５に出力する。ＧＮＳＳ位置補正部１２５は、ＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇを用いて、ＧＮＳＳ位置Ｐｇを補正する。
　補正情報推定部１２３Ａは、スケール誤差δＳ、および、ＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角との角度差の算出誤差を、ＶＳＬＡＭ演算部１２１にフィードバックする。ＶＳＬＡＭ演算部１２１は、スケール誤差δＳ、および、ＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角との角度差の算出誤差、すなわちＶＳＬＡＭ推定データの推定誤差を、ＶＳＬＡＭ推定データの推定に反映させる。これにより、ＶＳＬＡＭ推定データは、高精度な値に収束していく。
　補正情報推定部１２３Ａは、ＧＮＳＳ観測データの算出誤差を、ＧＮＳＳ演算部１２２にフィードバックする。ＧＮＳＳ演算部１２２は、ＧＮＳＳ観測データの算出誤差を、ＧＮＳＳ観測データの算出に反映させる。これにより、ＧＮＳＳ観測データは、高精度な値に収束していく。
　補正情報推定部１２３Ａは、ＩＭＵ観測データの算出誤差を、ＩＭＵ演算部１２６にフィードバックする。ＩＭＵ演算部１２６は、ＩＭＵ観測データの算出誤差を、ＩＭＵ観測データの算出に反映させる。これにより、ＩＭＵ観測データは、高精度な値に収束していく。
　このような構成および処理を用いることによって、ＶＳＬＡＭ推定データ、ＧＮＳＳ観測データおよびＩＭＵ観測データは、高精度化する。したがって、スケール誤差δＳおよびＧＮＳＳ速度Ｖｇの誤差δＶｇまたはＧＮＳＳ位置Ｐｇの誤差δＰｇも高精度に推定できる。
　これにより、航法装置１０Ａは、空間情報をさらに高精度に補正し、自装置位置をさらに高精度に補正できる。よって、航法装置１０Ａは、自装置の位置と空間情報の各特徴点との位置関係、すなわち航法支援情報を、さらに高精度に算出できる。
　なお、補正情報推定部１２３は、ＩＭＵ姿勢角とＶＳＬＡＭ姿勢角ＡＡｖとを用いて、状態変数にＶＳＬＡＭ姿勢角ＡＡｖとＩＭＵ姿勢角との角度差（オフセット角）の誤差を加え、当該角度差の誤差を推定することもできる。
　次に、本発明の第３の実施形態に係る航法装置について、図を参照して説明する。図９は、本発明の第３の実施形態に係る航法装置の構成を示す機能ブロック図である。図１０は、本発明の第３の実施形態に係る演算部の構成を示す機能ブロック図である。
　図９に示すように、第３の実施形態に係る航法装置１０Ｂは、第１の実施形態に係る航法装置１０に対して、演算部１２Ｂの構成と処理、および、表示器１３を備える点において異なる。航法装置１０Ｂの他の構成は、航法装置１０と同様であり、同様の箇所の説明は省略する。
　表示器１３は、例えば、液晶ディスプレイ等からなり、演算部１２Ｂに接続している。
　図１０に示すように、演算部１２Ｂは、ＶＳＬＡＭ演算部１２１、ＧＮＳＳ演算部１２２、補正情報推定部１２３、空間情報補正部１２４、ＧＮＳＳ位置補正部１２５、航法支援情報生成部１２７、および、表示画像生成部１２８を備える。演算部１２ＢにおけるＶＳＬＡＭ演算部１２１、ＧＮＳＳ演算部１２２、補正情報推定部１２３、空間情報補正部１２４、およびＧＮＳＳ位置補正部１２５は、演算部１２のこれらと同様であり、説明は省略する。
　航法支援情報生成部１２７は、スケールＳによって補正後の空間情報、誤差δＶｇまたは誤差δＰｇによって補正後のＧＮＳＳ位置、ＧＮＳＳ速度、および、ＧＮＳＳ姿勢角を用いて、航法支援情報を生成する。航法支援情報とは、水平グリッド、予測航跡等を有する。
　航法支援情報生成部１２７は、ＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角とを用いて水平面を設定する。より具体的には、航法支援情報生成部１２７は、ＶＳＬＡＭ姿勢角とＧＮＳＳ姿勢角との角度差（オフセット角）を算出する。航法支援情報生成部１２７は、この角度差を相殺するように補正角を設定する。航法支援情報生成部１２７は、ＧＮＳＳ座標系で設定されている水平面を補正角で回転させることで、ＶＳＬＡＭ座標系の水平面（撮像画像上の水平面）を設定する。すなわち、航法支援情報生成部１２７は、水平面の姿勢角を補正角で補正することによって、ＶＳＬＡＭ座標系の水平面を設定する。
　航法支援情報生成部１２７は、さらに水平面を基準として、水平グリッドを設定する。水平グリッドは、水平面に平行な線分によって表される。この際、航法支援情報生成部１２７は、上述のスケールＳを用いることによって、撮像画像に撮像された空間の実寸法に合わせた間隔で水平グリッドを設定できる。そして、上述のようにスケールＳが高精度に補正されているので、航法支援情報生成部１２７は、撮像画像に撮像された空間の実寸法に正確に対応した水平グリッドを生成できる。
　また、航法支援情報生成部１２７は、補正後のＧＮＳＳ位置、ＧＮＳＳ速度、および、ＧＮＳＳ姿勢角を用いて、既知の方法から予測航跡を算出する。航法支援情報生成部１２７は、予測航跡を上述の補正角で回転させることによって、ＶＳＬＡＭ座標系に座標変換する。これにより、航法支援情報生成部１２７は、水平面に対して予測航跡を高精度に配置できる。さらに、この構成および処理を用いることによって、船舶９００に動揺が生じても、水平面に対して予測航跡を高精度に配置できる。
　また、角度差に対する補正が行われるとともに、スケールＳが高精度であることによって、水平グリッドと予測航跡とは、高精度に位置関係が表示される。
　航法支援情報生成部１２７は、水平グリッド、予測航跡、および、空間情報に含まれる特徴点の座標を航法支援情報として、表示画像生成部１２８に出力する。
　表示画像生成部１２８は、航法支援情報と撮像画像とを用いて、図１１（Ａ）または図１１（Ｂ）に示すような航法支援画像を生成し、表示器１３に出力する。表示器１３は、航法支援画像を表示する。
　図１１（Ａ）、図１１（Ｂ）は、第３の実施形態に係る航法装置で表示する航法支援画像の例を示す図である。図１１（Ａ）、図１１（Ｂ）において、実線は水平グリッドを表し、破線は予測航跡を表し、●は特徴点を表す。
　図１１（Ａ）は、撮像画像に、水平グリッド、予測航跡、および、特徴点を重畳した航法支援画像である。上述のように、航法装置１０Ｂでは、水平面は正確に設定され、水平グリッドは、撮像画像のスケールに高精度に合わせて設定され、予測航跡と水平面との角度差（オフセット角）は補正されている。したがって、図１１（Ａ）のような航法支援画像の場合に、水平グリッド３１、予測航跡３２、および、特徴点３３は、撮像画像３０１に正確に重畳される。
　図１１（Ｂ）は、撮像画像に基づく水平面を基準としたオルソ画像を用いた航法支援画像である。上述のように、航法装置１０Ｂでは、水平面は正確に設定され、水平グリッドは、撮像画像のスケールに高精度に合わせて設定され、予測航跡と水平面との角度差（オフセット角）は補正されている。したがって、図１１（Ｂ）のような航法支援画像の場合、オルソ画像の基準となる水平面が正確に設定され、水平グリッド３１、予測航跡３２、および、特徴点３３は、オルソ画像３０２に正確に重畳される。
　このように、本実施形態の構成および処理を用いることによって、航法装置１０Ｂは、船舶９００の操舵者、オペレータに対して、正確な航法支援情報を提供できる。
　なお、図１１（Ａ）の航法支援画像の一部にウィンドウを設定し、当該ウィンドウに図１１（Ｂ）に示すオルソ画像をはめ込んでもよい。これにより、航法装置１０Ｂは、船舶９００の操舵者、オペレータに対して、多種で正確な航法支援の情報を、一画面で提供できる。
　なお、上述の説明では、ＶＳＬＡＭ姿勢角ＡＡｖとＧＮＳＳ姿勢角ＡＡｇとの角度差（オフセット角）を用いて、推定された水平面を補正する態様を示した。しかしながら、この角度差を省略することも可能である。この場合、例えば、船舶９００が殆ど動揺しない静水状態の時に、水平面に対して予測航跡を高精度に配置できる。
　上述の説明では、演算部１２Ｂを複数の機能ブロックに分けて実現する態様を示したが、図１２に示す処理を実現するプログラムおよび当該プログラムを実行する演算処理装置のみによって実現してもよい。図１２は、航法支援情報の生成方法の処理フローを示すフローチャートである。なお、各処理の具体的な内容は、上述しており、説明は省略する。
　演算処理装置は、上述の方法によって算出されたＧＮＳＳ姿勢角またはＩＭＵ姿勢角とＶＳＬＡＭ姿勢角との角度差（オフセット角）を算出する（Ｓ２１）。演算処理装置は、角度差を相殺する補正角を設定する（Ｓ２２）。
　演算処理装置は、補正角を用いて、撮像画像上の水平面（ＶＳＬＡＭ座標系での水平面）を設定する（Ｓ２３）。
　演算処理装置は、上述のように算出され補正されたスケールＳを用いて、水平グリッドを設定する（Ｓ２４）。
　演算処理装置は、ＧＮＳＳ座標系で予測航跡を算出する（Ｓ２５）。具体的には、ＧＮＳＳ速度、ＧＮＳＳ姿勢角を用いて、既知の方法から予測航跡を算出する。
　演算処理装置は、補正角を用いて、予測航跡の座標変換を行う（Ｓ２６）。
　演算処理装置は、水平グリッド、予測航跡、および、特徴点を含む航法支援情報を生成し、航法支援情報と撮像画像とを用いて、表示画像を生成する。
　なお、上述の説明では、船首から真っ直ぐ延びる方向を中心方向とするヘッドアップ表示を行う態様を示したが、東西南北を基準とするナビゲーション座標系の表示を行うことも可能である。この場合、ＧＮＳＳ姿勢角等のＧＮＳＳ観測値を用いて、水平グリッド、予測航跡、特徴点の座標変換を行えばよい。
　また、ＶＳＬＡＭ演算の初期化が終了していない場合、特徴点が少ない（空間情報が少ない）場合は、カメラ３０の高さとＧＮＳＳ姿勢角とを用いたホモグラフィ変換を用いて、水平面を検出してもよい。この場合、ＶＳＬＡＭ演算の初期化後には、上述のように、空間情報を用いて水平面を検出する。
　また、水平面は、例えば、空間情報に含まれる特徴点と当該特徴点に関連づけられた水平面に関する既知の情報とを用いて設定してもよく、ステレオカメラ、レーダ、レーザスキャナ、モーションセンサ等の他のセンサの出力データを用いて設定してもよい。
　また、上述の説明では、第３の実施形態に係る航法装置１０Ｂの一部に、第１の実施形態に係る航法装置１０の構成を用いる態様を示した。しかしながら、第３の実施形態に係る航法装置１０Ｂの一部に、第２の実施形態に係る航法装置１０Ａの構成を用いてもよい。この場合、ＧＮＳＳ速度、ＩＭＵ姿勢角は、より高精度に算出されるので、水平面に対して予測航跡をより高精度に配置でき、水平グリッドと予測航跡との位置関係もより高精度になる。
１０、１０Ａ、１０Ｂ：航法装置
１２、１２Ａ、１２Ｂ：演算部
１３：表示器
２１、２２、２３：ＧＮＳＳアンテナ
２４：ＧＮＳＳ－ＩＭＵ複合センサ
３０：カメラ
３１：水平グリッド
３２：予測航跡
３３：特徴点
９０：基準点
９１：自装置
１１１、１１２、１１３：ＧＮＳＳ受信部
１２１：ＶＳＬＡＭ演算部
１２２：ＧＮＳＳ演算部
１２３、１２３Ａ：補正情報推定部
１２４：空間情報補正部
１２５：ＧＮＳＳ位置補正部
１２６：ＩＭＵ演算部
１２７：航法支援情報生成部
１２８：表示画像生成部
３０１：撮像画像
３０２：オルソ画像
９００：船舶
９１０、９１０ｖ：岸壁
９１１、９１１ｖ、９１２、９２１、９２１ｖ：特徴点
９２０、９２０ｖ：建築物
１２２１：ＧＮＳＳ速度算出部
１２２２：ＧＮＳＳ姿勢角算出部
１２３１：回転行列算出部
１２３２：推定演算部
ＡＡｇ：ＧＮＳＳ姿勢角
ＡＡｖ：ＶＳＬＡＭ姿勢角
Ｃ：回転行列
Ｄｇｉｊ：ＧＮＳＳ距離
Ｄｖｉｊ：ＶＳＬＡＭ距離
Ｐｇ、Ｐｇｃ、Ｐｇｊ、Ｐｇｊｃ：ＧＮＳＳ位置
Ｐｏ：座標
Ｐｖ：ＶＳＬＡＭ位置
Ｓ：スケール
Ｖｅ９１１ｃ、Ｖｅ９１１ｖ、Ｖｅ９２１ｃ、Ｖｅ９２１ｖ：空間ベクトル
Ｖｇ：ＧＮＳＳ速度
Ｖｇｃ：速度
用語
　必ずしも全ての目的または効果・利点が、本明細書中に記載される任意の特定の実施形態に則って達成され得るわけではない。従って、例えば当業者であれば、特定の実施形態は、本明細書中で教示または示唆されるような他の目的または効果・利点を必ずしも達成することなく、本明細書中で教示されるような1つまたは複数の効果・利点を達成または最適化するように動作するように構成され得ることを想到するであろう。
　本明細書中に記載される全ての処理は、1つまたは複数のコンピュータまたはプロセッサを含むコンピューティングシステムによって実行されるソフトウェアコードモジュールにより具現化され、完全に自動化され得る。コードモジュールは、任意のタイプの非一時的なコンピュータ可読媒体または他のコンピュータ記憶装置に記憶することができる。一部または全ての方法は、専用のコンピュータハードウェアで具現化され得る。
　本明細書中に記載されるもの以外でも、多くの他の変形例があることは、本開示から明らかである。例えば、実施形態に応じて、本明細書中に記載されるアルゴリズムのいずれかの特定の動作、イベント、または機能は、異なるシーケンスで実行することができ、追加、併合、または完全に除外することができる (例えば、記述された全ての行為または事象がアルゴリズムの実行に必要というわけではない)。さらに、特定の実施形態では、動作またはイベントは、例えば、マルチスレッド処理、割り込み処理、または複数のプロセッサまたはプロセッサコアを介して、または他の並列アーキテクチャ上で、逐次ではなく、並列に実行することができる。さらに、異なるタスクまたはプロセスは、一緒に機能し得る異なるマシンおよび/またはコンピューティングシステムによっても実行され得る。
　本明細書中に開示された実施形態に関連して説明された様々な例示的論理ブロックおよびモジュールは、プロセッサなどのマシンによって実施または実行することができる。プロセッサは、マイクロプロセッサであってもよいが、代替的に、プロセッサは、コントローラ、マイクロコントローラ、またはステートマシン、またはそれらの組み合わせなどであってもよい。プロセッサは、コンピュータ実行可能命令を処理するように構成された電気回路を含むことができる。別の実施形態では、プロセッサは、特定用途向け集積回路(ASIC)、フィールドプログラマブルゲートアレイ(FPGA)、またはコンピュータ実行可能命令を処理することなく論理演算を実行する他のプログラマブルデバイスを含む。プロセッサはまた、コンピューティングデバイスの組み合わせ、例えば、デジタル信号プロセッサ(デジタル信号処理装置)とマイクロプロセッサの組み合わせ、複数のマイクロプロセッサ、DSPコアと組み合わせた1つ以上のマイクロプロセッサ、または任意の他のそのような構成として実装することができる。本明細書中では、主にデジタル技術に関して説明するが、プロセッサは、主にアナログ素子を含むこともできる。例えば、本明細書中に記載される信号処理アルゴリズムの一部または全部は、アナログ回路またはアナログとデジタルの混合回路により実装することができる。コンピューティング環境は、マイクロプロセッサ、メインフレームコンピュータ、デジタル信号プロセッサ、ポータブルコンピューティングデバイス、デバイスコントローラ、または装置内の計算エンジンに基づくコンピュータシステムを含むが、これらに限定されない任意のタイプのコンピュータシステムを含むことができる。
　特に明記しない限り、「できる」「できた」「だろう」または「可能性がある」などの条件付き言語は、特定の実施形態が特定の特徴、要素および/またはステップを含むが、他の実施形態は含まないことを伝達するために一般に使用される文脈内での意味で理解される。従って、このような条件付き言語は、一般に、特徴、要素および/またはステップが1つ以上の実施形態に必要とされる任意の方法であること、または1つ以上の実施形態が、これらの特徴、要素および/またはステップが任意の特定の実施形態に含まれるか、または実行されるかどうかを決定するための論理を必然的に含むことを意味するという訳ではない。
　語句「X、Y、Zの少なくとも1つ」のような選言的言語は、特に別段の記載がない限り、項目、用語等が X, Y, Z、のいずれか、又はそれらの任意の組み合わせであり得ることを示すために一般的に使用されている文脈で理解される(例: X、Y、Z)。従って、このような選言的言語は、一般的には、特定の実施形態がそれぞれ存在するXの少なくとも1つ、Yの少なくとも1つ、またはZの少なくとも1つ、の各々を必要とすることを意味するものではない。
　本明細書中に記載されかつ/または添付の図面に示されたフロー図における任意のプロセス記述、要素またはブロックは、プロセスにおける特定の論理機能または要素を実装するための1つ以上の実行可能命令を含む、潜在的にモジュール、セグメント、またはコードの一部を表すものとして理解されるべきである。代替の実施形態は、本明細書中に記載された実施形態の範囲内に含まれ、ここでは、要素または機能は、当業者に理解されるように、関連する機能性に応じて、実質的に同時にまたは逆の順序で、図示または説明されたものから削除、順不同で実行され得る。
　特に明示されていない限り、「一つ」のような数詞は、一般的に、1つ以上の記述された項目を含むと解釈されるべきである。従って、「～するように設定された一つのデバイス」などの語句は、1つ以上の列挙されたデバイスを含むことを意図している。このような1つまたは複数の列挙されたデバイスは、記載された引用を実行するように集合的に構成することもできる。例えば、「以下のA、BおよびCを実行するように構成されたプロセッサ」は、Aを実行するように構成された第1のプロセッサと、BおよびCを実行するように構成された第2のプロセッサとを含むことができる。加えて、導入された実施例の具体的な数の列挙が明示的に列挙されたとしても、当業者は、このような列挙が典型的には少なくとも列挙された数(例えば、他の修飾語を用いない「2つの列挙と」の単なる列挙は、通常、少なくとも2つの列挙、または2つ以上の列挙を意味する)を意味すると解釈されるべきである。
　一般に、本明細書中で使用される用語は、一般に、「非限定」用語(例えば、「～を含む」という用語は「それだけでなく、少なくとも～を含む」と解釈すべきであり、「～を持つ」という用語は「少なくとも～を持っている」と解釈すべきであり、「含む」という用語は「以下を含むが、これらに限定されない。」などと解釈すべきである。) を意図していると、当業者には判断される。
　説明の目的のために、本明細書中で使用される「水平」という用語は、その方向に関係なく、説明されるシステムが使用される領域の床の平面または表面に平行な平面、または説明される方法が実施される平面として定義される。「床」という用語は、「地面」または「水面」という用語と置き換えることができる。「垂直/鉛直」という用語は、定義された水平線に垂直/鉛直な方向を指します。「上側」「下側」「下」「上」「側面」「より高く」「より低く」「上の方に」「～を越えて」「下の」などの用語は水平面に対して定義されている。
　本明細書中で使用される用語の「付着する」、「接続する」、「対になる」及び他の関連用語は、別段の注記がない限り、取り外し可能、移動可能、固定、調節可能、及び/または、取り外し可能な接続または連結を含むと解釈されるべきである。接続/連結は、直接接続及び/または説明した2つの構成要素間の中間構造を有する接続を含む。
　特に明示されていない限り、本明細書中で使用される、「およそ」、「約」、および「実質的に」のような用語が先行する数は、列挙された数を含み、また、さらに所望の機能を実行するか、または所望の結果を達成する、記載された量に近い量を表す。例えば、「およそ」、「約」及び「実質的に」とは、特に明示されていない限り、記載された数値の10%未満の値をいう。本明細書中で使用されているように、「およそ」、「約」、および「実質的に」などの用語が先行して開示されている実施形態の特徴は、さらに所望の機能を実行するか、またはその特徴について所望の結果を達成するいくつかの可変性を有する特徴を表す。
　上述した実施形態には、多くの変形例および修正例を加えることができ、それらの要素は、他の許容可能な例の中にあるものとして理解されるべきである。そのような全ての修正および変形は、本開示の範囲内に含まれることを意図し、以下の特許請求の範囲によって保護される。

Claims (20)

  1. 　測位衛星から送信されるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いてＧＮＳＳ速度およびＧＮＳＳ姿勢角を算出するＧＮＳＳ演算部と、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定するＶＳＬＡＭ演算部と、
    　前記ＧＮＳＳ速度、前記ＧＮＳＳ姿勢角、前記複数の時刻での前記ＶＳＬＡＭ位置、および、前記複数の時刻での前記ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対する前記ＶＳＬＡＭ位置のスケールまたはスケール誤差と、前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差と、を推定する補正情報推定部と、
    を備える、航法装置。
  2. 　測位衛星から送信されるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度またはＧＮＳＳ位置を含むＧＮＳＳ演算値を算出するＧＮＳＳ演算部と、
    　慣性センサの観測データを用いて、ＩＭＵ姿勢角またはＩＭＵ速度を含むＩＭＵ演算値を算出するＩＭＵ演算部と、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定するＶＳＬＡＭ演算部と、
    　前記ＧＮＳＳ演算値、前記ＩＭＵ演算値、前記ＶＳＬＡＭ位置、および、前記ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対する前記ＶＳＬＡＭ位置のスケールまたはスケール誤差、前記ＶＳＬＡＭ姿勢角の推定誤差、前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差、前記ＧＮＳＳ演算値の誤差、および、前記ＩＭＵ演算値の誤差を推定する補正情報推定部と、
    を備える、航法装置。
  3. 　請求項２に記載の航法装置であって、
    　前記補正情報推定部は、
    　前記スケールまたは前記スケール誤差および前記ＶＳＬＡＭ姿勢角の推定誤差を、前記ＶＳＬＡＭ演算部にフィードバックし、
    　前記ＧＮＳＳ演算値の誤差を、前記ＧＮＳＳ演算部にフィードバックし、
    　前記ＩＭＵ演算値の誤差を、前記ＩＭＵ演算部にフィードバックする、
    　航法装置。
  4. 　請求項１乃至請求項３のいずれかに記載の航法装置であって、
    　前記スケールまたは前記スケール誤差によって補正したスケールを用いて、前記ＶＳＬＡＭ演算部で推定された空間情報を補正する空間情報補正部を備える、航法装置。
  5. 　請求項１乃至請求項４のいずれかに記載の航法装置であって、
    　前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差を用いて、自装置の前記ＧＮＳＳ位置を補正するＧＮＳＳ位置補正部を備える、航法装置。
  6. 　請求項１乃至請求項５のいずれかに記載の航法装置であって、
    　前記補正情報推定部は、
    　前記ＶＳＬＡＭ姿勢角のオフセット角の算出誤差を推定する、
    　航法装置。
  7. 　請求項６に記載の航法装置であって、
    　前記オフセット角を用いて前記動画像の水平面を検出する航法支援情報生成部を備える、航法装置。
  8. 　請求項７に記載の航法装置であって、
    　前記航法支援情報生成部は、
    　前記スケールまたは前記スケール誤差によって補正したスケールを用いて、前記水平面に平行な水平グリッドを含む航法支援情報を生成する、航法装置。
  9. 　請求項８に記載の航法装置であって、
    　前記航法支援情報生成部は、
    　前記ＧＮＳＳ位置および前記ＧＮＳＳ速度を用いて予測航跡を算出し、前記オフセット角を用いて座標変換を行った前記予測航跡を、前記航法支援情報に含む、
    　航法装置。
  10. 　請求項８または請求項９に記載の航法装置であって、
    　前記動画像と前記航法支援情報とを用いて表示画像を形成する表示画像形成部を備える、航法装置。
  11. 　測位衛星から送信されるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いてＧＮＳＳ速度およびＧＮＳＳ姿勢角を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定し、
    　前記ＧＮＳＳ速度、前記ＧＮＳＳ姿勢角、前記複数の時刻での前記ＶＳＬＡＭ位置、および、前記複数の時刻での前記ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対する前記ＶＳＬＡＭ位置のスケールまたはスケール誤差と、前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差と、を推定する、
    　航法支援情報の生成方法。
  12. 　測位衛星から送信されるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度またはＧＮＳＳ位置を含むＧＮＳＳ演算値を算出し、
    　慣性センサの観測データを用いて、ＩＭＵ加速度またはＩＭＵ角速度を含むＩＭＵ演算値を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定し、
    　前記ＧＮＳＳ演算値、前記ＩＭＵ演算値、前記ＶＳＬＡＭ位置、および、前記ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対する前記ＶＳＬＡＭ位置のスケールまたはスケール誤差、前記ＶＳＬＡＭ姿勢角の推定誤差、前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差、前記ＧＮＳＳ演算値の誤差、および、前記ＩＭＵ演算値の誤差を推定する、
    　航法支援情報の生成方法。
  13. 　請求項１０または請求項１１に記載の航法支援情報の生成方法であって、
    　前記ＶＳＬＡＭ姿勢角のオフセット角の算出誤差を推定し、
    　前記オフセット角を用いて前記動画像の水平面を検出する、
    　航法支援情報の生成方法。
  14. 　請求項１３に記載の航法支援情報の生成方法であって、
    　前記スケールまたは前記スケール誤差によって補正したスケールを用いて、前記水平面に平行な水平グリッドを含む航法支援情報を生成する、
    　航法支援情報の生成方法。
  15. 　請求項１４に記載の航法支援情報の生成方法であって、
    　前記ＧＮＳＳ位置および前記ＧＮＳＳ速度を用いて予測航跡を算出し、
    　前記オフセット角を用いて座標変換を行った前記予測航跡を、前記航法支援情報に含む、
    　航法支援情報の生成方法。
  16. 　測位衛星から送信されるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いてＧＮＳＳ速度およびＧＮＳＳ姿勢角を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定し、
    　前記ＧＮＳＳ速度、前記ＧＮＳＳ姿勢角、前記複数の時刻での前記ＶＳＬＡＭ位置、および、前記複数の時刻での前記ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対する前記ＶＳＬＡＭ位置のスケールまたはスケール誤差と、前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差と、を推定する、
    　処理を演算処理装置に実行させる航法支援情報の生成プログラム。
  17. 　測位衛星から送信されるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度またはＧＮＳＳ位置を含むＧＮＳＳ演算値を算出し、
    　慣性センサの観測データを用いて、ＩＭＵ加速度またはＩＭＵ角速度を含むＩＭＵ演算値を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置およびＶＳＬＡＭ姿勢角を推定し、
    　前記ＧＮＳＳ演算値、前記ＩＭＵ演算値、前記ＶＳＬＡＭ位置、および、前記ＶＳＬＡＭ姿勢角を用いて、絶対座標系の位置に対する前記ＶＳＬＡＭ位置のスケールまたはスケール誤差、前記ＶＳＬＡＭ姿勢角の推定誤差、前記ＧＮＳＳ速度の誤差または前記ＧＮＳＳ速度から得られるＧＮＳＳ位置の誤差、前記ＧＮＳＳ演算値の誤差、および、前記ＩＭＵ演算値の誤差を推定する、
    　処理を演算処理装置に実行させる航法支援情報の生成プログラム。
  18. 　請求項１６または請求項１７に記載の航法支援情報の生成プログラムであって、
    　前記ＶＳＬＡＭ姿勢角のオフセット角の算出誤差を推定し、
    　前記オフセット角を用いて前記動画像の水平面を検出する、
    　処理を演算処理装置に実行させる航法支援情報の生成プログラム。
  19. 　請求項１８に記載の航法支援情報の生成プログラムであって、
    　前記スケールまたは前記スケール誤差によって補正したスケールを用いて、前記水平面に平行な水平グリッドを含む航法支援情報を生成する、
    　処理を演算処理装置に実行させる航法支援情報の生成プログラム。
  20. 　請求項１９に記載の航法支援情報の生成プログラムであって、
    　前記ＧＮＳＳ位置および前記ＧＮＳＳ速度を用いて予測航跡を算出し、
    　前記オフセット角を用いて座標変換を行った前記予測航跡を、前記航法支援情報に含む、
    　処理を演算処理装置に実行させる航法支援情報の生成プログラム。
PCT/JP2019/032415 2018-09-21 2019-08-20 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム WO2020059383A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
JP2020548153A JP7190500B2 (ja) 2018-09-21 2019-08-20 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム
CN201980058853.1A CN112840235A (zh) 2018-09-21 2019-08-20 导航装置、导航支援信息的生成方法及生成程序
EP19861916.5A EP3855216A4 (en) 2018-09-21 2019-08-20 NAVIGATION DEVICE AND METHOD AND PROGRAM FOR GENERATION OF NAVIGATION ASSISTANCE INFORMATION

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-176737 2018-09-21
JP2018176737 2018-09-21

Publications (1)

Publication Number Publication Date
WO2020059383A1 true WO2020059383A1 (ja) 2020-03-26

Family

ID=69887122

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/032415 WO2020059383A1 (ja) 2018-09-21 2019-08-20 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム

Country Status (4)

Country Link
EP (1) EP3855216A4 (ja)
JP (1) JP7190500B2 (ja)
CN (1) CN112840235A (ja)
WO (1) WO2020059383A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113484892A (zh) * 2021-06-30 2021-10-08 中国船舶重工集团公司第七一九研究所 一种基于gnss的船体姿态计算方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110141485A1 (en) * 2009-12-16 2011-06-16 Industrial Technology Research Institute System and Method for Localizing a Carrier, Estimating a Posture of the Carrier and Establishing a Map
US20150219767A1 (en) * 2014-02-03 2015-08-06 Board Of Regents, The University Of Texas System System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates
JP2015230218A (ja) * 2014-06-04 2015-12-21 株式会社デンソー 移動距離推定装置
JP2016540187A (ja) * 2013-09-27 2016-12-22 クゥアルコム・インコーポレイテッドＱｕａｌｃｏｍｍ Ｉｎｃｏｒｐｏｒａｔｅｄ 外部ハイブリッドフォトマッピング
US20180188384A1 (en) * 2017-01-04 2018-07-05 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry
JP2018526626A (ja) * 2015-07-27 2018-09-13 クゥアルコム・インコーポレイテッドＱｕａｌｃｏｍｍ Ｉｎｃｏｒｐｏｒａｔｅｄ 視覚慣性オドメトリ姿勢ドリフト較正

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9031782B1 (en) * 2012-01-23 2015-05-12 The United States Of America As Represented By The Secretary Of The Navy System to use digital cameras and other sensors in navigation
CN107110978B (zh) * 2014-12-26 2021-02-02 古野电气株式会社 状态计算装置、状态计算方法及存储介质
WO2016195527A1 (ru) * 2015-06-05 2016-12-08 Общество с ограниченной ответственностью "Навигационные решения" Способ и система навигации внутри помещения
CN106780699B (zh) * 2017-01-09 2020-10-16 东南大学 一种基于sins/gps和里程计辅助的视觉slam方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110141485A1 (en) * 2009-12-16 2011-06-16 Industrial Technology Research Institute System and Method for Localizing a Carrier, Estimating a Posture of the Carrier and Establishing a Map
JP2016540187A (ja) * 2013-09-27 2016-12-22 クゥアルコム・インコーポレイテッドＱｕａｌｃｏｍｍ Ｉｎｃｏｒｐｏｒａｔｅｄ 外部ハイブリッドフォトマッピング
US20150219767A1 (en) * 2014-02-03 2015-08-06 Board Of Regents, The University Of Texas System System and method for using global navigation satellite system (gnss) navigation and visual navigation to recover absolute position and attitude without any prior association of visual features with known coordinates
JP2015230218A (ja) * 2014-06-04 2015-12-21 株式会社デンソー 移動距離推定装置
JP2018526626A (ja) * 2015-07-27 2018-09-13 クゥアルコム・インコーポレイテッドＱｕａｌｃｏｍｍ Ｉｎｃｏｒｐｏｒａｔｅｄ 視覚慣性オドメトリ姿勢ドリフト較正
US20180188384A1 (en) * 2017-01-04 2018-07-05 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
BEN AFIA, AMANI ET AL.: "A Low-cost GNSS/IMU/Visual monoSLAM/WSS Integration Based on Kalman Filtering for Navigation in Urban Environments", PROCEEDINGS OF THE 2 8TH INTERNATIONAL TECHNICAL MEETING OF THE SATELLITE DIVISION OF THE INSTITUTE OF NAVIGATION (ION GNSS+ 2015, 18 September 2015 (2015-09-18), pages 618 - 628, XP056011031, ISSN: 2331-5911 *
GEORG KLEINDAVID MURRAY: "Active Vision Laboratory Department of Engineering Science", UNIVERSITY OF OXFORD, article "Parallel Tracking and Mapping for Small AR Workspace"
NAKANO, JUNICHI ET AL.: "Recording Urban Spaces by Images with Position Information Estimated from Visual SLAM and GPS", PROCEEDINGS OF IEICE, vol. J93-D, no. 6, 1 June 2010 (2010-06-01), pages 901 - 908, XP055736852, ISSN: 1880-4535 *
See also references of EP3855216A4
TAKAFUMI TAKETOMIHIDEAKI UCHIYAMASEI IKEDA, VISUAL-SLAM ALGORITHMS: A SURVEY FROM 2010 TO 2016

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113484892A (zh) * 2021-06-30 2021-10-08 中国船舶重工集团公司第七一九研究所 一种基于gnss的船体姿态计算方法及系统
CN113484892B (zh) * 2021-06-30 2023-06-20 中国船舶重工集团公司第七一九研究所 一种基于gnss的船体姿态计算方法及系统

Also Published As

Publication number Publication date
JPWO2020059383A1 (ja) 2021-09-09
JP7190500B2 (ja) 2022-12-15
CN112840235A (zh) 2021-05-25
EP3855216A4 (en) 2022-07-06
EP3855216A1 (en) 2021-07-28

Similar Documents

Publication Publication Date Title
US11466990B2 (en) Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system
US20190368879A1 (en) Vision-aided inertial navigation system for ground vehicle localization
CA2923988C (en) Navigation system with rapid gnss and inertial initialization
Gowda et al. Tracking drone orientation with multiple GPS receivers
US9182237B2 (en) Navigation system with rapid GNSS and inertial initialization
US9411053B2 (en) Method for using partially occluded images for navigation and positioning
JP6470314B2 (ja) 姿勢角算出装置、姿勢角算出方法、および姿勢角算出プログラム
CA3003298A1 (en) Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter
EP3470961B1 (en) Augmented reality device for leveraging high-accuracy gnss data
CN105783922A (zh) 针对基于磁性校准测量的混合导航方案的航向
US11333501B2 (en) Navigation device, method of generating navigation support information, and navigation support information generating program
WO2021049221A1 (ja) 船舶情報表示システム、船舶情報表示方法、画像生成装置、及びプログラム
US20210215831A1 (en) Positioning apparatus and positioning method
CA2720437C (en) Systems and methods for determining heading
JP6923739B2 (ja) 航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム
WO2020059383A1 (ja) 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム
CA2908754A1 (en) Navigation system with rapid gnss and inertial initialization
US20170030718A1 (en) Positioning system having a master-slave configuration
JP7291775B2 (ja) 移動情報算出装置、および、移動情報算出方法
JP2015102330A (ja) 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体
US11693126B2 (en) Augmented reality distance measurement
JP2014145613A (ja) 移動体角速度推定装置、移動体速度算出装置、移動体、移動体角速度推定方法、移動体速度算出方法、移動体角速度推定プログラム、および移動体速度算出プログラム
Nakamura et al. Fusion of VSLAM/GNSS/INS for Augmented Reality Navigation in Ports
JP2014145611A (ja) 移動体角速度算出装置、移動体速度算出装置、移動体、移動体角速度算出方法、移動体速度算出方法、移動体角速度算出プログラム、および移動体速度算出プログラム
JP2014145612A (ja) 動揺周期推定装置、移動体角速度算出装置、移動体速度算出装置、移動体、動揺周期推定方法、移動体角速度算出方法、移動体速度算出方法、動揺周期推定プログラム、移動体角速度算出プログラム、および移動体速度算出プログラム

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19861916

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2020548153

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2019861916

Country of ref document: EP

Effective date: 20210421