WO2019167517A1 - 航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム - Google Patents

航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム Download PDF

Info

Publication number
WO2019167517A1
WO2019167517A1 PCT/JP2019/002832 JP2019002832W WO2019167517A1 WO 2019167517 A1 WO2019167517 A1 WO 2019167517A1 JP 2019002832 W JP2019002832 W JP 2019002832W WO 2019167517 A1 WO2019167517 A1 WO 2019167517A1
Authority
WO
WIPO (PCT)
Prior art keywords
vslam
gnss
calculation unit
correction value
speed
Prior art date
Application number
PCT/JP2019/002832
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 CN201980015959.3A priority Critical patent/CN111819418A/zh
Priority to EP19761398.7A priority patent/EP3760974A4/en
Priority to JP2020502876A priority patent/JP6923739B2/ja
Publication of WO2019167517A1 publication Critical patent/WO2019167517A1/ja
Priority to US17/005,404 priority patent/US20200393574A1/en

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/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
    • G01C21/203—Specially adapted for sailing ships
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/03—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers
    • G01S19/05—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing aiding data
    • G01S19/06—Cooperating elements; Interaction or communication between different cooperating elements or between cooperating elements and receivers providing aiding data employing an initial estimate of the location of the receiver as aiding data or in generating aiding data
    • 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
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence

Definitions

  • the present invention relates to a navigation apparatus using a technology of VSLAM (Visual Simulaneous Localization and Mapping).
  • VSLAM Visual Simulaneous Localization and Mapping
  • VSLAM Vehicle Simulaneous Localization and Mapping
  • VSLAM estimates spatial information such as feature points around its own device using moving images acquired by a monocular camera, and performs mapping, for example. Further, 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 device itself and other feature points can be calculated, but the absolute positional relationship is indefinite.
  • an object of the present invention is to provide a technique for calculating the absolute positional relationship between the own apparatus and other feature points with high accuracy while using VSLAM.
  • the navigation device of the present invention includes a GNSS speed calculation unit, a VSLAM calculation unit, and a scale correction value calculation unit.
  • the GNSS speed calculation unit calculates the GNSS speed using the phase change amount of the GNSS signal or the frequency change amount of the GNSS signal.
  • the VSLAM calculation unit performs VSLAM calculation using moving images, and estimates VSLAM positions at a plurality of times.
  • the scale correction value calculation unit calculates a scale correction value for the VSLAM position using the VSLAM position at two or more times and the GNSS speed estimated by the VSLAM calculation unit.
  • the GNSS speed is calculated with high accuracy by using the phase change amount or frequency change amount of the GNSS signal. Therefore, the distance based on the GNSS speed corresponding to the distance between the plurality of VSLAM positions is calculated with high accuracy, and the ratio between the distance between the plurality of VSLAM positions and the distance based on the GNSS speed, that is, the scale correction value is calculated with high accuracy.
  • the absolute positional relationship between the own apparatus and other feature points can be calculated with high accuracy while using VSLAM.
  • the navigation apparatus, the VSLAM correction method, and the VSLAM correction program according to the first embodiment of the present invention will be described with reference to the drawings.
  • the structure of this invention is applicable also to other water, an underwater moving body, a land moving body, or an airborne moving body.
  • FIG. 1 is a functional block diagram showing the configuration of the navigation device according to the first embodiment of the present invention.
  • FIG. 2 is a functional block diagram showing the configuration of the calculation unit according to the first embodiment of the present invention.
  • the navigation device 10 includes a GNSS receiver 111 and a calculator 12.
  • the GNSS receiver 111 is connected to the GNSS antenna 21.
  • the computing unit 12 is connected to the GNSS receiving unit 111 and the camera 30.
  • GNSS antenna 21 is installed in a ship (not shown) to which navigation device 10 is mounted.
  • the GNSS antenna 21 is installed in a so-called open sky environment.
  • the GNSS antenna 21 receives a GNSS signal from a GNSS satellite (not shown) and outputs the GNSS signal to the GNSS receiver 111.
  • the GNSS signal is a signal in which a PRN (pseudo random noise) code and navigation message data are superimposed on a carrier wave signal having a predetermined frequency.
  • the PRN code is a code for identifying a positioning satellite as a transmission source.
  • the navigation message data is data including orbit information of the positioning satellite, correction information, and the like.
  • the GNSS receiving unit 111 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.
  • the GNSS receiver 111 captures and tracks the acquired GNSS signal by a known method. At this time, the GNSS receiver 111 captures and tracks a code and a carrier signal (carrier signal).
  • the GNSS receiving unit 111 acquires observation data by capturing and tracking, and outputs the observation data to the calculation unit 12.
  • the observation data includes at least an integrated value (ADR) of the carrier phase.
  • the GNSS reception unit 111 sequentially acquires the integrated value of the carrier wave phase at predetermined time intervals in synchronization with the GNSS time, for example, for each epoch. Note that the GNSS receiver 111 can also output the Doppler frequency, the code phase, the pseudo distance calculated by the code phase, the positioning position, and the like of the carrier wave signal as observation data to the computing unit 12.
  • the camera 30 is, for example, a monocular camera.
  • the camera 30 captures an image of the periphery of the ship on which the navigation device 10 is mounted, and generates a moving image.
  • the camera 30 outputs a moving image to the calculation unit 12.
  • the calculation unit 12 includes a memory that stores a program for processing such as VSLAM calculation, calculation of a GNSS speed, calculation of a scale correction value, and correction of spatial information, which will be described later, and a calculation element such as a CPU that executes the program. It is configured.
  • the calculation unit 12 includes a VSLAM calculation unit 121, a GNSS speed calculation unit 122, a scale correction value calculation unit 123, and a spatial information correction unit 124.
  • the VSLAM calculation unit 121 uses the acquired moving image to determine the position of the device (VSLAM position) with respect to a reference point (see 90 (coordinate Po) in FIGS. 3 and 4) and each feature point by a known method. Estimate spatial information. At this time, the VSLAM calculation unit 121 estimates the position of the device (VSLAM position) with respect to the reference point and the spatial information of each feature point in synchronization with the GNSS time. That is, the VSLAM calculation unit 121 synchronizes with the acquisition time of the integrated value of the carrier phase, and obtains the position of the device (VSLAM position) with respect to the reference point and the spatial information of each feature point for each moving image acquisition time. presume.
  • 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 a calculation time of a GNSS speed described later. Spatial information is represented by a space vector of feature points based on a VSLAM calculation reference point.
  • the VSLAM calculation unit 121 outputs the VSLAM position to the scale correction value calculation unit 123 and the spatial information correction unit 124. Further, the VSLAM calculation 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 and the spatial information in association with the estimated time.
  • the GNSS speed calculation unit 122 calculates the GNSS speed using the change amount of the integrated value of the carrier wave phase. At this time, the GNSS speed calculation unit 122 calculates the GNSS speed at a predetermined time interval synchronized with the GNSS time.
  • the GNSS speed calculation unit 122 can calculate the GNSS speed at high speed and with high accuracy. That is, the GNSS speed calculation unit 122 can calculate the GNSS speed at high speed and high accuracy without performing processing such as determination of an integer value bias and acquisition of correction information from the outside.
  • the GNSS speed calculation unit 122 outputs the GNSS speed to the scale correction value calculation unit 123.
  • the GNSS speed calculation unit 122 outputs the GNSS speed in association with the calculation time.
  • the scale correction value calculation unit 123 calculates the scale correction value S related to the VSLAM position using the plurality of VSLAM positions and the GNSS speed.
  • the scale correction value calculation unit 123 outputs the scale correction value S to the spatial information correction unit 124. As described above, since the GNSS speed is highly accurate, the scale correction value S is calculated with high accuracy.
  • the spatial information correction unit 124 corrects the spatial information using the scale correction value S. At this time, the spatial information correction unit 124 also corrects the VSLAM position.
  • FIG. 3 is a diagram for explaining the principle of scale correction according to the first embodiment of the present invention.
  • FIG. 4 is a diagram for explaining the principle of spatial information correction according to the first embodiment of the present invention.
  • the GNSS antenna 21 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 GNSS antenna 21 and the camera 30 are arranged at the same position at least on the horizontal plane of the hull coordinate system.
  • FIG. 3 for easy understanding, the VSLAM position and the GNSS position at the first time ti are described so as to coincide with each other.
  • the present invention is not limited to this, and the following processing can be realized.
  • the ship 900 is moving from the first time ti to the second time tj.
  • the camera 30 captures an image and outputs a moving image to the VSLAM calculation unit 121.
  • the VSLAM calculation unit 121 sets the reference point 90 and its position Po using the moving image, and estimates the VSLAM position P1vi at the first time ti and the VSLAM position P1vj at the second time tj.
  • the first time ti and the second time tj are not limited to consecutive times, and are set by two times of a plurality of two or more times.
  • the VSLAM calculation unit 121 outputs the VSLAM position P1vi at the first time ti and the VSLAM position P1vj at the second time tj to the scale correction value calculation unit 123.
  • the scale correction value calculation unit 123 calculates the VSLAM distance Dvij by calculating the geometric difference between the VSLAM position P1vi at the first time ti and the VSLAM position P1vj at the second time tj.
  • the distance is calculated from the positional difference between two times, but the distance can also be calculated using the positional difference between three or more times.
  • the scale correction value calculation unit 123 integrates the GNSS speed Vg from the first time ti calculated by the GNSS speed calculation unit 122 to the second time tj (more precisely, the calculation time immediately before the second time tj). By calculating the value, the GNSS distance Dgij, which is the movement distance of the ship 900 (observation position 91 on the ship 900) from the first time ti to the second time tj based on the GNSS signal, is calculated.
  • the scale correction value calculation unit 123 can calculate the scale correction value S with high accuracy by using the GNSS speed calculated with high accuracy. Furthermore, in this configuration, since the calculation time of the GNSS speed Vj and the estimated time of the VSLAM position P1v are synchronized with the high-accuracy GNSS time, the scale correction value calculation unit 123 further increases the accuracy of the scale correction value S. Can be calculated.
  • the spatial information correction unit 124 corrects the spatial information by multiplying each spatial information, that is, the spatial vector of each feature point by the scale correction value S.
  • each spatial information that is, the spatial vector of each feature point by the scale correction value S.
  • 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 with the feature point 911v.
  • the space vector Ve911v includes indefiniteness inherent to the camera 30 as described in the above-described problem.
  • the scale correction value 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 high-accuracy GNSS speed. Therefore, the space vector Ve911c is a value obtained by correcting the space vector Ve911v obtained by the VSLAM calculation with a high accuracy value according to the absolute coordinate system.
  • the navigation apparatus 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 calculation is the space vector Ve921v starting from the reference point 90 (coordinate Po) and ending with the feature point 921v.
  • the space vector Ve921v includes indefiniteness inherent to the camera 30 as shown in the above-described problem.
  • the space vector Ve921c becomes a value obtained by accurately correcting the space vector Ve921v obtained by the VSLAM calculation to a high-accuracy 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. Can be calculated with high accuracy.
  • the navigation apparatus 10 can correct the error of the spatial information using the VSLAM calculation with high accuracy, and the spatial information around the ship 900 can be converted into a highly accurate real scale. Can be obtained by value. Thereby, the navigation apparatus 10 can perform highly accurate mapping with respect to the periphery of the ship 900.
  • FIG. 5 is a flowchart showing a first processing flow of the VSLAM correction method. The specific contents of each process have been described above and will not be described.
  • the arithmetic processing unit calculates the GNSS speed from the amount of change in the integrated value of the carrier phase in the GNSS signal (S11).
  • the arithmetic processing unit executes VSLAM calculation from the moving image input from the camera, and estimates the VSLAM position and spatial information (S12).
  • the arithmetic processing unit executes the calculation of the GNSS speed and the estimation of the VSLAM position and the spatial information in time synchronization.
  • the time synchronization is not limited to the one that is executed in parallel, but the calculation of the GNSS speed and the estimation of the VSLAM position and the spatial information are executed by attaching the time information, respectively, and those having the same time later May be combined.
  • the arithmetic processing unit calculates the scale correction value S using the GNSS speed and the plurality of VSLAM positions (S13).
  • the arithmetic processing unit corrects and updates the spatial information using the scale correction value S (S14).
  • FIG. 6 is a flowchart showing a second processing flow of the VSLAM correction method.
  • the arithmetic processing unit performs VSLAM calculation from the moving image input from the camera, and estimates the VSLAM position and spatial information (S21).
  • the arithmetic processing unit executes the estimation of the VSLAM position and the spatial information in synchronization with the GNSS time.
  • the arithmetic processing unit continuously executes the VSLAM calculation until the scale correction value update timing comes (S22: NO).
  • the arithmetic processing unit acquires the GNSS speed (S23).
  • the arithmetic processing unit calculates the scale correction value S using the GNSS speed and the plurality of VSLAM positions (S24).
  • the arithmetic processing unit corrects and updates the spatial information using the scale correction value S (S25).
  • the calculation of the scale correction value and the update of the spatial information can be executed at appropriately set intervals, and the calculation load can be reduced while appropriately correcting the spatial information. This is particularly effective when, for example, there are many feature points and the spatial information is enormous.
  • FIG. 7 is a functional block diagram showing the configuration of the navigation device according to the second embodiment of the present invention.
  • FIG. 8 is a functional block diagram showing a configuration of a calculation unit according to the second embodiment of the present invention.
  • the navigation device 10A and the calculation unit 12A according to the second embodiment are different from the navigation device 10 and the calculation unit 12 in that the posture angle is corrected together with the scale correction.
  • Other configurations of the navigation device 10A and the calculation unit 12A are the same as those of the navigation device 10 and the calculation unit 12, and the description of the same parts is omitted.
  • the navigation apparatus 10A includes a GNSS receiving unit 111, a GNSS receiving unit 112, a GNSS receiving unit 113, and a calculation unit 12A.
  • the GNSS receiver 111 is connected to the GNSS antenna 21, the GNSS receiver 112 is connected to the GNSS antenna 22, and the GNSS receiver 113 is connected to the GNSS antenna 23.
  • the calculation unit 12A is connected to the GNSS reception unit 111, the GNSS reception unit 112, and the GNSS reception unit 113, and is connected to the camera 30.
  • the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23 are arranged so that there is no one on the straight line connecting the other two.
  • the positional relationship of the GNSS antenna 21, GNSS antenna 22, and GNSS antenna 23 in the hull coordinate system is acquired in advance and stored in the calculation unit 12A.
  • the GNSS antenna 21, the GNSS antenna 22, and the GNSS antenna 23 each receive and output a GNSS signal.
  • the GNSS receiving unit 111 acquires observation data by capturing and tracking the GNSS signal received by the GNSS antenna 21, and outputs the observation data to the calculation unit 12A.
  • the GNSS receiving unit 112 acquires observation data by capturing and tracking the GNSS signal received by the GNSS antenna 22, and outputs the observation data to the calculation unit 12A.
  • the GNSS reception unit 113 acquires observation data by capturing and tracking the GNSS signal received by the GNSS antenna 23, and outputs the observation data to the calculation unit 12A.
  • Each observation data includes at least the integrated value (ADR) of the carrier phase.
  • the calculation unit 12A includes a VSLAM calculation unit 121A, a GNSS speed calculation unit 122, a scale correction value calculation unit 123, a spatial information correction unit 124, a GNSS posture angle calculation unit 125, and a posture angle correction value calculation.
  • Part 126 is provided.
  • the VSLAM calculation unit 121A estimates the VSLAM position and the spatial information of each feature point by a known method using the acquired moving image, similarly to the VSLAM calculation unit 121 described above. In addition, the VSLAM calculation unit 121A estimates a VSLAM posture angle with respect to a preset reference axis through a reference point by a known method using the acquired moving image. At this time, the VSLAM calculation unit 121A estimates the VSLAM position, the VSLAM posture angle, and the spatial information of each feature point at a plurality of times in synchronization with the GNSS time.
  • the VSLAM calculation unit 121A outputs the VSLAM position to the scale correction value calculation unit 123 and the spatial information correction unit 124A.
  • the VSLAM calculation unit 121A outputs the VSLAM posture angle to the posture angle correction value calculation unit 126 and the spatial information correction unit 124A.
  • the VSLAM calculation unit 121A outputs the spatial information to the spatial information correction unit 124A. At this time, the VSLAM calculation unit 121A outputs the VSLAM position, the VSLAM posture angle, and the spatial information associated with the estimated time.
  • the GNSS speed calculation unit 122 calculates the GNSS speed using the change amount of the integrated value of the carrier phase acquired by any of the GNSS reception unit 111, the GNSS reception unit 112, and the GNSS reception unit 113.
  • the scale correction value calculation unit 123 calculates the scale correction value S related to the VSLAM position using the plurality of VSLAM positions and the GNSS speed.
  • the scale correction value calculation unit 123 outputs the scale correction value S to the spatial information correction unit 124A. As described above, since the GNSS speed is highly accurate, the scale correction value S is calculated with high accuracy.
  • the GNSS attitude angle calculation unit 125 uses a known method (for example, a base line vector between GNSS antennas) using the integrated value of the carrier phase obtained by each of the GNSS reception unit 111, the GNSS reception unit 112, and the GNSS reception unit 113. GNSS attitude angle is calculated from the above-mentioned method.
  • the GNSS attitude angle is calculated with high accuracy.
  • the GNSS posture angle calculation unit 125 outputs the GNSS posture angle to the posture angle correction value calculation unit 126. At this time, the GNSS posture angle calculation unit 125 outputs the GNSS posture angle in association with the calculation time.
  • the posture angle correction value calculation unit 126 calculates a posture angle correction value for the VSLAM posture angle using the synchronized VSLAM posture angle and the GNSS posture angle.
  • the attitude angle correction value is defined by a value that suppresses an error included in the VSLAM attitude angle by the GNSS attitude angle.
  • the attitude angle correction value calculation unit 126 is represented by, for example, a coordinate system in which the coordinate system set in the VSLAM is used as a reference for the GNSS attitude angle (the inclination of the hull coordinates with respect to the local horizontal coordinates is the GNSS attitude angle). A correction value for making it so is calculated as a posture angle correction value.
  • the posture angle correction value calculation unit 126 outputs the posture angle correction value to the spatial information correction unit 124A.
  • the posture angle correction value is calculated with high accuracy.
  • the spatial information correction unit 124A corrects the spatial information using the scale correction value S and the attitude angle correction value. At this time, the spatial information correction unit 124A also corrects the VSLAM position and the VSLAM posture angle.
  • the spatial information is corrected with high accuracy.
  • the spatial information can be corrected with higher accuracy than the aspect using only the scale correction value by correcting the spatial information using the posture angle information.
  • FIG. 9 is a flowchart showing a third processing flow of the VSLAM correction method. The specific contents of each process have been described above and will not be described.
  • the arithmetic processing unit calculates the GNSS speed and the GNSS attitude angle using the integrated value of the carrier phase in the GNSS signals received by the plurality of GNSS antennas (S31).
  • the arithmetic processing unit executes VSLAM calculation from the moving image input from the camera, and estimates the VSLAM position, VSLAM posture angle, and spatial information (S32).
  • the arithmetic processing unit executes the calculation of the GNSS speed, the calculation of the GNSS attitude angle, and the estimation of the VSLAM position, the VSLAM attitude angle, and the spatial information in time synchronization.
  • the arithmetic processing unit calculates the scale correction value S using the GNSS speed and the plurality of VSLAM positions, and calculates the attitude angle correction value using the GNSS attitude angle and the VSLAM attitude angle (S33).
  • the arithmetic processing unit corrects and updates the spatial information using the scale correction value S and the posture angle correction value (S34).
  • FIG. 9 illustrates a mode in which the scale correction is sequentially performed
  • the scale correction may be performed at a preset time interval as illustrated in FIG.
  • FIG. 10 is a flowchart showing a fourth processing flow of the VSLAM correction method.
  • the arithmetic processing unit performs VSLAM calculation from the moving image input from the camera, and estimates the VSLAM position and spatial information (S41).
  • the arithmetic processing unit executes the estimation of the VSLAM position and the spatial information in synchronization with the GNSS time.
  • the arithmetic processing unit continuously executes the VSLAM calculation until the correction value update timing comes (S42: NO).
  • the arithmetic processing unit acquires the GNSS speed and the GNSS attitude angle (S43).
  • the arithmetic processing unit calculates the scale correction value S using the GNSS speed and the plurality of VSLAM positions, and calculates the attitude angle correction value using the GNSS attitude angle and the VSLAM attitude angle (S44).
  • the arithmetic processing unit corrects and updates the spatial information using the scale correction value S and the posture angle correction value (S45).
  • FIG. 11 is a functional block diagram showing the configuration of the navigation device according to the third embodiment of the present invention.
  • FIG. 12 is a functional block diagram showing a configuration of a calculation unit according to the third embodiment of the present invention.
  • the navigation device 10B and the calculation unit 12B according to the third embodiment use the observation data of the GNSS signal and the output data of the IMU, so that the navigation device according to the second embodiment. 10A and the calculation unit 12A are different.
  • the other configurations of the navigation device 10B and the calculation unit 12B are the same as those of the navigation device 10A and the calculation unit 12A, and description of similar parts is omitted.
  • the navigation device 10B includes a GNSS receiver 111, a GNSS receiver 112, and a calculator 12B.
  • the GNSS receiver 111 is connected to the GNSS antenna 21, and the GNSS receiver 112 is connected to the GNSS antenna 22.
  • the calculation unit 12B is connected to the GNSS reception unit 111 and the GNSS reception unit 112, and is connected to the camera 30.
  • the calculation unit 12B is connected to the IMU 40.
  • the GNSS antenna 21 and the GNSS antenna 22 are arranged, for example, in parallel to the direction connecting the bow and stern of the ship 900.
  • the arrangement of the GNSS antenna 21 and the GNSS antenna 22 is not limited to this, but is preferably parallel to the direction connecting the bow and the stern.
  • the positional relationship of the GNSS antenna 21 and the GNSS antenna 22 in the hull coordinate system is acquired in advance and stored in the calculation unit 12B.
  • the GNSS antenna 21 and the GNSS antenna 22 each receive and output a GNSS signal.
  • the GNSS reception unit 111 acquires observation data by capturing and tracking the GNSS signal received by the GNSS antenna 21 and outputs the observation data to the calculation unit 12B.
  • the GNSS receiving unit 112 acquires observation data by capturing and tracking the GNSS signal received by the GNSS antenna 22, and outputs the observation data to the calculation unit 12B.
  • Each observation data includes at least an integrated value (ADR) of the carrier phase.
  • the IMU 40 is a so-called inertial sensor, for example, an angular velocity sensor.
  • the IMU 40 detects an angular velocity around three orthogonal axes, and outputs the detected angular velocity to the calculation unit 12B.
  • the IMU 40 may include an acceleration sensor.
  • the IMU 40 detects the same three-axis acceleration as that of the angular velocity sensor, and outputs it to the calculation unit 12B as sensor data.
  • the IMU 40 generates sensor data at a predetermined time interval and outputs the sensor data to the calculation unit 12B in the same manner as the GNSS reception unit 111 and the GNSS reception unit 112.
  • the calculation unit 12B includes a VSLAM calculation unit 121B, a scale correction value calculation unit 123B, a spatial information correction unit 124B, a posture angle correction value calculation unit 126B, and an integrated calculation unit 127.
  • the VSLAM calculation unit 121B estimates the VSLAM position, the VSLAM posture angle, and the spatial information of each feature point by a known method using the acquired moving image in the same manner as the VSLAM calculation unit 121A described above.
  • the VSLAM calculation unit 121B outputs the VSLAM position to the scale correction value calculation unit 123B and the spatial information correction unit 124B.
  • the VSLAM calculation unit 121B outputs the VSLAM posture angle to the posture angle correction value calculation unit 126B and the spatial information correction unit 124B.
  • the VSLAM calculation unit 121B outputs the spatial information to the spatial information correction unit 124B.
  • the integration calculation unit 127 corrects the sensor data output from the IMU 40 with the observation data output from the GNSS reception unit 111 and the GNSS reception unit 112. Thereby, the integration calculation unit 127 calculates the integration speed and the integration attitude angle.
  • the integrated calculation unit 127 estimates an observation error included in the IMU angular velocity data with observation data from the GNSS reception unit 111 and the GNSS reception unit 112. To do. Then, the integrated calculation unit 127 calculates the integrated angular velocity by correcting the IMU angular velocity data with the estimated observation error. As a result, the integrated angular velocity is more accurate than the IMU angular velocity.
  • the integrated calculation unit 127 calculates the integrated posture angle by, for example, integrating the integrated angular velocity.
  • the above-mentioned GNSS speed may be used as the integration speed.
  • the integrated calculation unit 127 estimates an observation error included in the IMU acceleration using observation data from the GNSS reception unit 111 and the GNSS reception unit 112. . Then, the integrated calculation unit 127 corrects the IMU acceleration data with the estimated observation error, and calculates the integrated acceleration. Thereby, the integrated acceleration becomes more accurate than the IMU acceleration.
  • the integration calculation unit 127 calculates the integration speed, for example, by integrating the integration acceleration.
  • the scale correction value calculation unit 123B uses the plurality of VSLAM positions and the GNSS speed or the integration speed to perform the scale correction value S according to the same method as the scale correction value calculation unit 123 and the scale correction value calculation unit 123A. Is calculated.
  • the scale correction value calculation unit 123B outputs the scale correction value S to the spatial information correction unit 124B.
  • the posture angle correction value calculation unit 126B applies the same method as the posture angle correction value calculation unit 126 described above, and calculates a posture angle correction value for the VSLAM posture angle using the VSLAM posture angle and the integrated posture angle.
  • the posture angle correction value is defined by a value that suppresses an error included in the VSLAM posture angle by the integrated posture angle.
  • the spatial information correction unit 124B corrects the spatial information using the scale correction value S and the posture angle correction value, similarly to the spatial information correction unit 124A described above. At this time, the spatial information correction unit 124B also corrects the VSLAM position and the VSLAM posture angle.
  • the spatial information is corrected with high accuracy by the high accuracy of the scale correction value S and the attitude angle correction value.
  • FIG. 13 is a functional block diagram showing the configuration of the navigation device according to the fourth embodiment of the present invention.
  • FIG. 14 is a functional block diagram showing a configuration of a calculation unit according to the fourth embodiment of the present invention.
  • the navigation device 10 ⁇ / b> C and the calculation unit 12 ⁇ / b> C according to the fourth embodiment include a navigation information generation unit 128 with respect to the navigation device 10 and the calculation unit 12 according to the first embodiment. It differs in the point added.
  • the other configurations of the navigation device 10C and the calculation unit 12C are the same as those of the navigation device 10 and the calculation unit 12, and the description of the same parts is omitted.
  • the navigation device 10C includes a calculation unit 12C.
  • the calculation unit 12C includes a navigation information generation unit 128.
  • the navigation information generator 128 receives the GNSS speed from the GNSS speed calculator 122.
  • the navigation information generation unit 128 generates navigation information of the ship 900 equipped with the navigation device 10C based on the GNSS speed.
  • the navigation information includes the speed, position, etc. of the ship 900. And by using the GNSS speed, the navigation information is also highly accurate.
  • the calculation unit 12C can output ship navigation information as well as spatial information. And since these spatial information and navigation information are highly accurate, it becomes possible to perform exact navigation support.
  • FIG. 15 is a functional block diagram showing the configuration of the navigation device according to the fifth embodiment of the present invention.
  • FIG. 16 is a functional block diagram showing a configuration of a calculation unit according to the fifth embodiment of the present invention.
  • the navigation device 10D and the calculation unit 12D according to the fifth embodiment include a navigation information generation unit 128D with respect to the navigation device 10A and the calculation unit 12A according to the second embodiment. It differs in the point added.
  • the other configurations of the navigation device 10D and the calculation unit 12D are the same as those of the navigation device 10A and the calculation unit 12A, and description of similar parts is omitted.
  • the VSLAM calculation unit 121D of the calculation unit 12D is the same as the VSLAM calculation unit 121A of the calculation unit 12A.
  • the navigation device 10D includes a calculation unit 12D.
  • the calculation unit 12D includes a navigation information generation unit 128D.
  • the GNSS speed is input from the GNSS speed calculation unit 122 to the navigation information generation unit 128D. Further, the navigation information generation unit 128D receives the GNSS attitude angle from the GNSS attitude angle calculation unit 125.
  • the navigation information generation unit 128D generates navigation information of the ship 900 equipped with the navigation device 10D based on the GNSS speed and the GNSS attitude angle.
  • the navigation information includes the speed, attitude, turning angular velocity, position, etc. of the ship 900. And by using the GNSS speed and the GNSS attitude angle, the navigation information is also highly accurate.
  • the calculation unit 12D can output more detailed ship navigation information along with the spatial information. And since these spatial information and navigation information are highly accurate, it becomes possible to perform exact navigation support.
  • FIG. 17 is a functional block diagram showing the configuration of the navigation device according to the sixth embodiment of the present invention.
  • FIG. 18 is a functional block diagram showing a configuration of a calculation unit according to the sixth embodiment of the present invention.
  • the navigation device 10E and the calculation unit 12E according to the sixth embodiment are different from the navigation device 10B and the calculation unit 12B according to the third embodiment. It differs in the point added.
  • the other configurations of the navigation device 10E and the calculation unit 12E are the same as those of the navigation device 10B and the calculation unit 12B, and description of the same parts is omitted.
  • the VSLAM calculation unit 121E of the calculation unit 12E is the same as the VSLAM calculation unit 121B of the calculation unit 12B.
  • the navigation device 10E includes a calculation unit 12E.
  • the calculation unit 12E includes a navigation information generation unit 128E.
  • the navigation speed and the integrated attitude angle are input from the integrated calculation unit 127 to the navigation information generation unit 128E.
  • the navigation information generation unit 128E generates navigation information of the ship 900 equipped with the navigation device 10E based on the integrated speed and the integrated attitude angle.
  • the navigation information includes the speed, attitude, turning angular velocity, position, etc. of the ship 900.
  • the navigation information is also highly accurate.
  • the calculation unit 12E can output more detailed ship navigation information along with spatial information. And since these spatial information and navigation information are highly accurate, it becomes possible to perform exact navigation support.
  • FIG. 19 is a functional block diagram showing a configuration of a calculation unit according to the seventh embodiment of the present invention.
  • the navigation device 10F and the calculation unit 12F according to the seventh embodiment are obtained by adding a lever arm correction unit 129 to the navigation device 10D and the calculation unit 12D according to the fifth embodiment. It is different.
  • the other configurations of the navigation device 10F and the calculation unit 12F are the same as those of the navigation device 10D and the calculation unit 12D, and description of similar parts is omitted.
  • the VSLAM calculation unit 121F of the calculation unit 12F is the same as the VSLAM calculation unit 121D of the calculation unit 12D
  • the navigation information generation unit 128F is the same as the navigation information generation unit 128D of the calculation unit 12D.
  • the lever arm correction unit 129 receives the GNSS speed and the GNSS attitude angle.
  • the lever arm correction unit 129 stores the positional relationship between the camera 30 and the GNSS antenna 21, GNSS antenna 22, GNSS antenna 23 in the hull coordinate system.
  • the lever arm correction unit 129 performs the lever arm correction of the GNSS speed by a known method using the positional relationship between the GNSS antenna that has calculated the GNSS speed and the camera 30 and the GNSS attitude angle.
  • the lever arm correction unit 129 outputs the GNSS speed after lever arm correction to the scale correction value calculation unit 123.
  • the scale correction value calculation unit 123 calculates the scale correction value S from the above-described method using the GNSS speed after lever arm correction.
  • lever arm correction can also be applied to the other embodiments described above.
  • the lever arm correction is not limited to this.
  • the lever arm correction can also be used to correct an error due to the positional relationship between the GNSS antenna and the IMU.
  • FIG. 20 is a functional block diagram showing a configuration of a calculation unit according to the eighth embodiment of the present invention.
  • the calculation unit 12G according to the seventh embodiment is different from the navigation device 10D and the calculation unit 12D according to the fifth embodiment in that the VSLAM calculation is performed with a constraint condition. Further, since it is not necessary to calculate the scale correction value, the calculation of the posture angle correction value, and the correction of the spatial information in the calculation unit 12G, the scale correction value calculation unit, the posture angle correction value calculation unit, and the spatial information The correction unit is omitted.
  • the other configuration of the calculation unit 12G is the same as that of the calculation unit 12D, and the description of the same part is omitted.
  • the navigation information generation unit 128G is the same as the navigation information generation unit 128D of the calculation unit 12D.
  • the VSLAM calculation unit 121G executes the VSLAM calculation using the GNSS speed and the GNSS attitude angle as constraint conditions. Specifically, the VSLAM calculation unit 121G executes, for example, the processes shown in FIGS.
  • FIG. 21 is a flowchart showing a main flow of VSLAM calculation with constraint conditions.
  • FIG. 22 is a flowchart showing the overall optimization process with constraint conditions.
  • the VSLAM computing unit 121G detects feature points in a plurality of frames using moving images and tracks the feature points for each frame (S51).
  • the VSLAM calculation unit 121G performs matching of feature points of a plurality of frames being tracked (S52).
  • the VSLAM computing unit 121G executes the VSLAM sequential process (S54) if the initialization by the above steps has been completed (S53: YES). On the other hand, if the initialization has not been completed (S53: NO), the VSLAM operation unit 121G completes the initialization (S55).
  • the VSLAM computing unit 121G detects that it is a key frame or just after initialization (S56: YES), it registers as a key frame (S57).
  • the key frame is set at a predetermined frame interval, for example.
  • the VSLAM calculation unit 121G performs overall optimization of the VSLAM calculation using the GNSS calculation result (for example, the GNSS speed and the GNSS attitude angle) after registering the key frame (S58).
  • the GNSS calculation result for example, the GNSS speed and the GNSS attitude angle
  • VSLAM operation unit 121G continues the VSLAM process without performing optimization unless it is not a key frame and is not immediately after initialization (S56: NO).
  • the VSLAM operation unit 121G first sets the VSLAM parameter to be optimized and the GNSS operation result as the overall optimization process of the VSLAM operation (S581).
  • the VSLAM parameters include, for example, internal parameters of the camera 30, external parameters of the camera 30, lens distortion coefficients, and three-dimensional positions of feature points.
  • the GNSS calculation result includes a displacement from the reference point based on the GNSS speed and a posture angle with respect to the reference coordinate axis based on the GNSS posture angle.
  • the VSLAM calculation unit 121G sets a cost function corresponding to the constraint condition (S582).
  • the cost function is set by, for example, a penalty based on the sum of the reprojection errors and the GNSS calculation result in the VSLAM calculation.
  • the VSLAM calculation unit 121G optimizes the parameter for VSLAM that minimizes the cost function using the least square method, various filter calculations, and the like (S583).
  • the calculation unit 12G can output highly accurate spatial information without performing scale correction and posture angle correction.
  • FIG. 23 is a functional block diagram showing the configuration of the navigation device according to the ninth embodiment of the present invention.
  • the navigation device 10H according to the ninth embodiment differs from the navigation device 10D according to the fifth embodiment in that a support information generation unit 13 is added.
  • the other configuration of the navigation device 10H is the same as that of the navigation device 10D, and the description of the same parts is omitted.
  • the calculation unit 12H is the same as the calculation unit 12D, and a description thereof will be omitted.
  • the navigation device 10 ⁇ / b> H includes a support information generation unit 13. Spatial information and navigation information are input to the support information generation unit 13 from the calculation unit 12H.
  • the support information generation unit 13 generates and outputs support information using the spatial information and the navigation information.
  • the support information includes, for example, a three-dimensional map outside the ship 900 equipped with the navigation device 10H, the position and speed of the ship 900 in the three-dimensional map, or the speed of a predetermined position in the ship 900.
  • the support information generation unit 13 generates a support image 50 using these support information and displays the support image 50 on a display device (not shown), for example.
  • Assistance images include, for example, images as shown in FIGS. 24 (A) and 24 (B).
  • 24A and 24B are diagrams illustrating an example of a support image.
  • FIG. 24A an image 51 of the ship 900 and an image 52 of the quay building are displayed in the support image 50.
  • the support image 50 is based on a moving image obtained from the camera 30.
  • the support image 50 includes a symbol mark 531 indicating a direction in which the specific position of the image 51 corresponding to the first specific point of the ship 900 approaches the first specific position of the quay, and data indicating the distance and the approach speed (see FIG. 24 (A) LS1, Vs1) 532 are displayed.
  • the support image 50 includes a symbol mark 541 indicating a direction in which the specific position of the image 51 corresponding to the second specific point of the ship 900 approaches the second specific position of the quay, and data indicating the distance and the approach speed (see FIG. 24 (A) LS2, Vs2) 542 are displayed.
  • two specific positions are taken as an example, but the number can be set as appropriate.
  • the support image 50 is a display that shows the actual approach direction, approach speed, and distance with high accuracy while using VSLAM. Thereby, highly accurate support information can be provided.
  • the support image 50 displays an image 55 of the ship 900 and an image 56 of the quay building.
  • the support image 50 is based on a moving image obtained from the camera 30.
  • the image 55 is created based on the viewpoint of the support image 50 from, for example, a set three-dimensional model of the ship.
  • the support image 50 displays a predicted wake 571, a predicted position 572, and a symbol mark and data (Ls57 shown in FIG. 24B) 53 representing the distance between the ship 900 and the quay after a predetermined time.
  • a symbol mark and data Ls57 shown in FIG. 24B
  • the number can be set suitably.
  • the support image 50 is a display that shows the actual navigation state and the predicted navigation state with respect to the quay with high accuracy while using VSLAM. Thereby, highly accurate support information can be provided.
  • the mode of calculating the GNSS calculation result such as the GNSS speed from the change amount of the phase of the carrier signal is shown.
  • the speed of the GNSS signal such as the change amount of the frequency of the carrier signal and the change amount of the code phase is shown. It is also possible to use other elements capable of calculating the posture angle.
  • All processes described herein may be embodied and fully automated by software code modules executed by a computing system including one or more computers or processors.
  • the code module may be stored on any type of non-transitory computer readable media or other computer storage device. Some or all of the methods may be implemented with dedicated computer hardware.
  • the various exemplary logic blocks and modules described in connection with the embodiments disclosed herein can be implemented or executed by machines such as processors.
  • the processor may be a microprocessor, but in the alternative, the processor may be a controller, microcontroller, or state machine, or a combination thereof.
  • the processor can include an electrical circuit configured to process 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 is also 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 It can be implemented as such a configuration. Although described herein primarily with respect to digital technology, a processor may also include primarily analog elements. For example, some or all of the signal processing algorithms described herein can be implemented with analog circuitry or mixed analog and digital circuitry.
  • a computing environment includes any type of computer system including, but not limited to, a microprocessor, mainframe computer, digital signal processor, portable computing device, device controller, or computer system based on a computing engine within the apparatus. be able to.
  • conditional languages such as “done” “done” “would” or “possibly” may mean that certain embodiments include particular features, elements and / or steps, It is understood in a context within the commonly used context to convey that an embodiment is not included. Thus, such a conditional language is generally any feature in which features, elements and / or steps are required for one or more embodiments, or one or more embodiments can be characterized by these features. It does not mean that the elements and / or steps necessarily include logic to determine whether they are included in or implemented in any particular embodiment.
  • a disjunctive language such as the phrase “at least one of X, Y, Z” means that the item, term, etc. is any of X, Y, Z, or any combination thereof, unless otherwise specified. Is understood in the context commonly used to show that it can be (eg, X, Y, Z). Thus, such disjunctive languages generally require at least one of X, at least one of Y, or at least one of Z, each with a particular embodiment. Does not mean.
  • a numeral such as “one” should generally be interpreted as including one or more described items.
  • phrases such as “a device configured to” are intended to include one or more listed devices. Such one or more listed devices can also be collectively configured to perform the recited citations.
  • a processor configured to execute A, B and C below is a first processor configured to execute A and a second processor configured to execute B and C. Processor.
  • an enumeration of a specific number of examples introduced is explicitly listed, those skilled in the art will typically recognize that such an enumeration is at least the number listed (e.g., other modifiers).
  • the simple enumeration of “two enumerations” without “” is usually to be understood as meaning at least two enumerations, or two or more enumerations).
  • the term “horizontal” as used herein, regardless of its direction, is a plane parallel to the plane or surface of the floor or description of the area in which the system being described is used. Is defined as the plane in which the method is performed.
  • the term “floor” can be replaced with the terms “ground” or “water surface”.
  • the term “vertical / vertical” refers to a direction perpendicular / vertical to a defined horizontal line. Terms such as “upper”, “lower”, “lower”, “upper”, “side”, “higher”, “lower”, “upward”, “beyond”, and “below” are defined relative to the horizontal plane. ing.
  • connection includes a direct connection and / or a connection having an intermediate structure between the two described components.

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)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (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 (24)

  1. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度を算出するＧＮＳＳ速度算出部と、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置を推定するＶＳＬＡＭ演算部と、
    　前記ＶＳＬＡＭ演算部で推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ位置と、前記ＧＮＳＳ速度とを用いて、前記ＶＳＬＡＭ位置に対するスケール補正値を算出するスケール補正値算出部と、
    　を備える、航法装置。
  2. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出するＧＮＳＳ姿勢角算出部と、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定するＶＳＬＡＭ演算部と、
    　前記ＶＳＬＡＭ演算部で推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ姿勢角と、前記ＧＮＳＳ姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する姿勢角補正値算出部と、
    　を備える、航法装置。
  3. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度を算出するＧＮＳＳ速度算出部と、
    　前記ＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出するＧＮＳＳ姿勢角算出部と、
    　動画像を用い、前記ＧＮＳＳ速度および前記ＧＮＳＳ姿勢角によって拘束条件が設定されたＶＳＬＡＭ演算を行って、空間情報を推定するＶＳＬＡＭ演算部と、
    　を備える、航法装置。
  4. 　請求項１に記載の航法装置であって、
    　前記ＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出するＧＮＳＳ姿勢角算出部と、
    　前記ＶＳＬＡＭ演算部で推定された、前記複数の時刻における２以上の時刻でのＶＳＬＡＭ姿勢角と、前記ＧＮＳＳ姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する姿勢角補正値算出部と、
    　を備える、航法装置。
  5. 　請求項４に記載の航法装置であって、
    　前記ＧＮＳＳ姿勢角を用いて、前記ＧＮＳＳ速度を前記ＶＳＬＡＭ演算の座標に合わせるレバーアーム補正を行い、レバーアーム補正後のＧＮＳＳ速度を、前記スケール補正値算出部に出力する、レバーアーム補正部を備える、航法装置。
  6. 　請求項４または請求項５に記載の航法装置であって、
    　前記ＶＳＬＡＭ演算部は、
    　前記ＧＮＳＳ速度および前記ＧＮＳＳ姿勢角を用いて、前記ＶＳＬＡＭ演算の最適化を行う、
    　航法装置。
  7. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量と、慣性センサの観測データと、を用いて、統合速度を算出し、前記ＧＮＳＳ信号の位相の積算値と、前記慣性センサの観測データと、を用いて、統合姿勢角を算出する統合算出部と、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置、および、ＶＳＬＡＭ姿勢角を推定するＶＳＬＡＭ演算部と、
    　前記ＶＳＬＡＭ演算部で推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ位置と、前記統合速度とを用いて、前記ＶＳＬＡＭ位置に対するスケール補正値を算出するスケール補正値算出部と、
    　前記ＶＳＬＡＭ演算部で推定されたＶＳＬＡＭ姿勢角と、前記統合姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する姿勢角補正値算出部と、
    　を備える、航法装置。
  8. 　請求項７に記載の航法装置であって、
    　前記ＶＳＬＡＭ演算部は、
    　前記統合速度および前記統合姿勢角を用いて、前記ＶＳＬＡＭ演算の最適化を行う、
    　航法装置。
  9. 　請求項１、４、５、６、７のいずれかに記載の航法装置であって、
    　前記スケール補正値を用いて、前記ＶＳＬＡＭ演算部で推定された空間情報を補正する空間情報補正部を備える、航法装置。
  10. 　請求項４乃至請求項７のいずれかに記載の航法装置であって、
    　前記スケール補正値と前記ＶＳＬＡＭ姿勢角に対する補正値とを用いて、前記ＶＳＬＡＭ演算部で推定された空間情報を補正する空間情報補正部を備える、航法装置。
  11. 　請求項９または請求項１０に記載の航法装置であって、
    　前記空間情報補正部から出力された空間情報を用いて、航行支援情報を生成する支援情報生成部を備える、航法装置。
  12. 　請求項３に記載の航法装置であって、
    　前記ＶＳＬＡＭ演算部から出力された空間情報を用いて、航行支援情報を生成する支援情報生成部を備える、航法装置。
  13. 　請求項１１または請求項１２に記載の航法装置であって、
    　前記航行支援情報は、前記空間情報から得られるマップデータと、ＧＮＳＳ速度を含む前記ＧＮＳＳ信号の観測結果から得られる航行データと、を重ね合わせた画像データである、
    　航法装置。
  14. 　請求項１乃至請求項１３のいずれかに記載の航法装置であって、
    　前記ＧＮＳＳ信号の位相および周波数は、搬送波信号の位相および周波数である、
    　航法装置。
  15. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置を推定し、
    　前記ＶＳＬＡＭ演算で推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ位置と、前記ＧＮＳＳ速度とを用いて、前記ＶＳＬＡＭ位置に対するスケール補正値を算出する、
    　ＶＳＬＡＭ補正方法。
  16. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定し、
    　推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ姿勢角と、前記ＧＮＳＳ姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する、
    　ＶＳＬＡＭ補正方法。
  17. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度を算出し、
    　前記ＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出し、
    　動画像を用い、前記ＧＮＳＳ速度および前記ＧＮＳＳ姿勢角によって拘束条件が設定されたＶＳＬＡＭ演算を行って、空間情報を推定する、
    　空間情報推定方法。
  18. 　請求項１５に記載のＶＳＬＡＭ補正方法であって、
    　前記ＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出し、
    　前記ＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定し、
    　推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ姿勢角と、前記ＧＮＳＳ姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する、
    　ＶＳＬＡＭ補正方法。
  19. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量と、慣性センサの観測データと、を用いて、統合速度を算出し、
    　前記ＧＮＳＳ信号の位相の積算値と、前記慣性センサの観測データと、を用いて、統合姿勢角を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置を推定し、
    　前記動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定し、
    　前記ＶＳＬＡＭ演算で推定された前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ位置と、前記統合速度とを用いて、前記ＶＳＬＡＭ位置に対するスケール補正値を算出し、
    　前記ＶＳＬＡＭ演算で推定された前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ姿勢角と、前記統合姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する、
    　ＶＳＬＡＭ補正方法。
  20. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置を推定し、
    　前記ＶＳＬＡＭ演算で推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ位置と、前記ＧＮＳＳ速度とを用いて、前記ＶＳＬＡＭ位置に対するスケール補正値を算出する、
    　処理を、演算処理装置に実行させるＶＳＬＡＭ補正プログラム。
  21. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定し、
    　推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ姿勢角と、前記ＧＮＳＳ姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する、
    　処理を、演算処理装置に実行させるＶＳＬＡＭ補正プログラム。
  22. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量を用いて、ＧＮＳＳ速度を算出し、
    　前記ＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出し、
    　動画像を用い、前記ＧＮＳＳ速度および前記ＧＮＳＳ姿勢角によって拘束条件が設定されたＶＳＬＡＭ演算を行って、空間情報を推定する、
    　処理を、演算処理装置に実行させる空間情報推定プログラム。
  23. 　請求項２０に記載のＶＳＬＡＭ補正プログラムであって、
    　前記ＧＮＳＳ信号の位相の積算値を用いて、ＧＮＳＳ姿勢角を算出し、
    　前記ＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定し、
    　推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ姿勢角と、前記ＧＮＳＳ姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する、
    　処理を、演算処理装置に実行させるＶＳＬＡＭ補正プログラム。
  24. 　測位衛星から送信される測位信号であるＧＮＳＳ信号の位相の変化量または前記ＧＮＳＳ信号の周波数の変化量と、慣性センサの観測データと、を用いて、統合速度を算出し、
    　前記ＧＮＳＳ信号の位相の積算値と、前記慣性センサの観測データと、を用いて、統合姿勢角を算出し、
    　動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ位置を推定し、
    　前記動画像を用いてＶＳＬＡＭ演算を行って、複数の時刻でのＶＳＬＡＭ姿勢角を推定し、
    　前記ＶＳＬＡＭ演算で推定された、前記複数の時刻における２以上の時刻での前記ＶＳＬＡＭ位置と、前記統合速度とを用いて、前記ＶＳＬＡＭ位置に対するスケール補正値を算出し、
    　前記ＶＳＬＡＭ演算で推定された、前記複数の時刻における２以上の時刻でのＶＳＬＡＭ姿勢角と、前記統合姿勢角とを用いて、前記ＶＳＬＡＭ姿勢角に対する補正値を算出する、
    　処理を、演算処理装置に実行させるＶＳＬＡＭ補正プログラム。
PCT/JP2019/002832 2018-02-28 2019-01-29 航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム WO2019167517A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN201980015959.3A CN111819418A (zh) 2018-02-28 2019-01-29 导航装置、vslam校正方法、空间信息估计方法、vslam校正程序及空间信息估计程序
EP19761398.7A EP3760974A4 (en) 2018-02-28 2019-01-29 NAVIGATION DEVICE, VSLAM CORRECTION PROCEDURE, SPATIAL INFORMATION APPRECIATION PROGRAM, VSLAM CORRECTION PROGRAM AND SPATIAL INFORMATION APPRECIATION PROGRAM
JP2020502876A JP6923739B2 (ja) 2018-02-28 2019-01-29 航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム
US17/005,404 US20200393574A1 (en) 2018-02-28 2020-08-28 Navigation device, vslam correcting method, method of estimating spatial information, vslam correcting program, and spatial information estimating program

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-034787 2018-02-28
JP2018034787 2018-02-28

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US17/005,404 Continuation-In-Part US20200393574A1 (en) 2018-02-28 2020-08-28 Navigation device, vslam correcting method, method of estimating spatial information, vslam correcting program, and spatial information estimating program

Publications (1)

Publication Number Publication Date
WO2019167517A1 true WO2019167517A1 (ja) 2019-09-06

Family

ID=67805699

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/002832 WO2019167517A1 (ja) 2018-02-28 2019-01-29 航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム

Country Status (5)

Country Link
US (1) US20200393574A1 (ja)
EP (1) EP3760974A4 (ja)
JP (1) JP6923739B2 (ja)
CN (1) CN111819418A (ja)
WO (1) WO2019167517A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4053822A4 (en) * 2019-10-29 2023-12-06 Yanmar Power Technology Co., Ltd. SHIP DOCKING AUXILIARY DEVICE

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113188509B (zh) * 2021-04-28 2023-10-24 上海商汤临港智能科技有限公司 一种测距方法、装置、电子设备及存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9070285B1 (en) * 2011-07-25 2015-06-30 UtopiaCompression Corporation Passive camera based cloud detection and avoidance for aircraft systems
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
JP2016540187A (ja) * 2013-09-27 2016-12-22 クゥアルコム・インコーポレイテッドＱｕａｌｃｏｍｍ Ｉｎｃｏｒｐｏｒａｔｅｄ 外部ハイブリッドフォトマッピング

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10324195B2 (en) * 2015-07-27 2019-06-18 Qualcomm Incorporated Visual inertial odometry attitude drift calibration

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9070285B1 (en) * 2011-07-25 2015-06-30 UtopiaCompression Corporation Passive camera based cloud detection and avoidance for aircraft systems
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

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
GEN SATO; ATSUSHI UMEMURA; HAJIME SAKANO; MITSUHARU SONEHARA; HIROYUKI YASHIRO; KIYOSHI IRIE; MASAHIRO TOMONO: "IS3-27: Examination of wide area 3D map generation method using 3D-SLAM", 6 July 2017 (2017-07-06), pages 1 - 4, XP009523653, Retrieved from the Internet <URL:https://confit.atlas.jp/guide/event/ssii2017/subject/IS3-27/detail?lang=ja> *
GEORG KLEINDAVID MURRAY: "Parallel Tracking and Mapping for Small AR Workspace", ACTIVE VISION LABORATORY DEPARTMENT OF ENGINEERING SCIENCE UNIVERSITY OF OXFORD
JUNICHI NAKANO, KUNIHIRO NISHIMURA , TOMOHIRO TANIGAWA , MICHITAKA HIROSE : "Recording Urban Spaces by Images with Position Information Estimated from Visual SLAM and GPS", IEICE TRANSACTIONS ON INFORMATION AND SYSTEMS, vol. J93-D, no. 9, 30 June 2010 (2010-06-30), pages 901 - 908, XP055736852, ISSN: 1880-4535 *
See also references of EP3760974A4
TAKAFUMI TAKETOMIHIDEAKI UCHIYAMASEI IKEDA, VISUAL-SLAM ALGORITHMS: A SURVEY FROM 2010 TO 2016

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP4053822A4 (en) * 2019-10-29 2023-12-06 Yanmar Power Technology Co., Ltd. SHIP DOCKING AUXILIARY DEVICE

Also Published As

Publication number Publication date
CN111819418A (zh) 2020-10-23
JP6923739B2 (ja) 2021-08-25
US20200393574A1 (en) 2020-12-17
JPWO2019167517A1 (ja) 2021-02-25
EP3760974A1 (en) 2021-01-06
EP3760974A4 (en) 2021-12-15

Similar Documents

Publication Publication Date Title
US11940277B2 (en) Vision-aided inertial navigation system for ground vehicle localization
US8761439B1 (en) Method and apparatus for generating three-dimensional pose using monocular visual sensor and inertial measurement unit
CN105783922B (zh) 用于确定针对具有磁力计辅助的混合导航系统的航向的方法
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
JP6470314B2 (ja) 姿勢角算出装置、姿勢角算出方法、および姿勢角算出プログラム
US11333501B2 (en) Navigation device, method of generating navigation support information, and navigation support information generating program
CN110186461B (zh) 一种基于重力梯度信息测距的协同导航方法
WO2021049221A1 (ja) 船舶情報表示システム、船舶情報表示方法、画像生成装置、及びプログラム
WO2019167517A1 (ja) 航法装置、ｖｓｌａｍ補正方法、空間情報推定方法、ｖｓｌａｍ補正プログラム、および、空間情報推定プログラム
CN114136315A (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
US20170030718A1 (en) Positioning system having a master-slave configuration
JP7190500B2 (ja) 航法装置、航法支援情報の生成方法、および、航法支援情報の生成プログラム
Lammas et al. 6-DoF navigation systems for autonomous underwater vehicles
US11953610B2 (en) Device and method for calculating movement information
JP4343581B2 (ja) 移動体の姿勢検出装置
JP2012202749A (ja) 方位測定装置
US11268814B2 (en) Movement information calculating device, movement information calculating method and movement information calculating program
JP2015102330A (ja) 移動情報算出装置、移動情報算出方法、移動情報算出プログラム、および移動体
JP2014145613A (ja) 移動体角速度推定装置、移動体速度算出装置、移動体、移動体角速度推定方法、移動体速度算出方法、移動体角速度推定プログラム、および移動体速度算出プログラム
Nakamura et al. Fusion of VSLAM/GNSS/INS for Augmented Reality Navigation in Ports
JP4417812B2 (ja) 船舶姿勢角測定装置
CN114258372B (zh) 船舶信息显示系统、船舶信息显示方法、图像生成装置以及程序
US11156721B2 (en) Oscillation observation device, oscillation observation method and oscillation observation program
JP2014145612A (ja) 動揺周期推定装置、移動体角速度算出装置、移動体速度算出装置、移動体、動揺周期推定方法、移動体角速度算出方法、移動体速度算出方法、動揺周期推定プログラム、移動体角速度算出プログラム、および移動体速度算出プログラム
CN117128954A (zh) 一种面向复杂环境的航行器组合定位方法

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: 19761398

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2020502876

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: 2019761398

Country of ref document: EP

Effective date: 20200928