JP2009074859A - Motion measuring device and position measuring device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To accurately measure motion of a movable body with robustness secured, in a simple constitution. <P>SOLUTION: Motion estimation is carried out based on a recorded image, and evaluation values of three-axis angular speed and a component indicating a translation direction of motion of own vehicle are calculated (160). The three-axis angular speed detected by a gyro sensor is acquired (162). A correction amount ΔR calculated, based on a difference between an average value of the three-axis angular speed calculated, based on a plurality of images recorded by imaging the exterior of own vehicle and the average value of the three-axis angular speed detected by the gyro sensor is acquired (164). The acquired correction amount ΔR is subtracted from angular speed R<SB>jyro</SB>and a corrected angular speed R'<SB>jyro</SB>is calculated for each of the acquired three-axis angular speeds (166), and the corrected three-axis angular speed and the component indicating the obtained translation direction are output as the estimated values of the motion of own vehicle (168). <P>COPYRIGHT: (C)2009,JPO&INPIT

Description

  The present invention relates to a motion measuring device and a position measuring device, and more particularly to a motion measuring device that measures the motion of a moving body and a position measuring device that measures the position of the moving body based on the motion of the moving body.

2. Description of the Related Art Conventionally, a self (vehicle) motion recognition system that detects an optical flow of an image and estimates a motion and a movement amount of a moving body is known (for example, Patent Document 1). In this system, the motion and amount of movement of a moving object are estimated using a stereo camera. In addition, a remote stereo camera and a near stereo camera are prepared, rotation is obtained using the far stereo camera, and translation is obtained using the near stereo camera. Further, the movement of the moving body is detected based on the optical flow between the two images.
JP2003-247824

  However, since the technique described in Patent Document 1 requires four cameras, there is a problem that the apparatus is expensive and the image processing takes time. Further, as long as a normal camera is used, there is a problem that robustness cannot be obtained because it is easily affected by the surrounding lighting environment.

  The present invention has been made in order to solve the above-described problems, and provides a motion measurement device and a position measurement device capable of accurately measuring the motion of a moving body while ensuring robustness with a simple configuration. The purpose is to provide.

  In order to achieve the above object, the motion measuring apparatus according to the first invention calculates at least one of the three-axis angular velocities of the motion of the mobile body based on a plurality of images obtained by imaging the outside of the mobile body. A calculation means, an angular velocity detection sensor for detecting at least one of the three-axis angular velocities of the movement of the moving body, at least one of the three-axis angular velocities calculated by the image calculation means, and the angular velocity detection sensor. And a motion estimation means for estimating at least one of the three-axis angular velocities of the motion of the moving body based on at least one of the three-axis angular velocities.

  According to the motion measuring apparatus according to the first aspect of the present invention, at least one of the three-axis angular velocities of the motion of the moving body is calculated by the image calculation means based on a plurality of images obtained by imaging the outside of the moving body. Here, the plurality of images obtained by imaging the outside of the moving body are at least two time-series images acquired by a monocular camera.

  Moreover, at least one of the three-axis angular velocities of the movement of the moving body is detected by the angular velocity detection sensor. Then, based on at least one of the three-axis angular velocities calculated by the image calculating unit and at least one of the three-axis angular velocities detected by the angular velocity detection sensor by the motion estimation unit, at least one of the three-axis angular velocities of the motion of the moving body. Estimate one.

  Thus, based on the triaxial angular velocity calculated based on a plurality of images obtained by imaging the outside of the moving body and the triaxial angular velocity detected by the angular velocity detection sensor, the triaxial angular velocity of the movement of the moving body is determined. By estimating, robustness can be ensured with a simple configuration, and the movement of the moving body can be accurately measured.

  The angular velocity detection sensor may be a gyro sensor.

  The motion estimation means according to the first invention is a correction amount for calculating at least one correction amount of the three-axis angular velocity detected by the angular velocity detection sensor based on at least one of the three-axis angular velocities calculated by the image calculation means. A calculating unit that corrects at least one of the three-axis angular velocities detected by the angular velocity detection sensor based on the correction amount calculated by the correction amount calculating unit; It can be estimated as at least one of the three-axis angular velocities of the movement.

  Here, the correction amount can be at least one offset amount of the triaxial angular velocity detected by the angular velocity detection sensor.

  Thereby, the offset amount of the triaxial angular velocity detected by the angular velocity detection sensor can be corrected, and the triaxial angular velocity of the movement of the moving body can be accurately measured.

  The correction amount calculation unit may calculate the correction amount based on a difference between at least one of the three-axis angular velocities calculated by the image calculation unit and at least one of the three-axis angular velocities detected by the angular velocity detection sensor. it can. As a result, a correction amount corresponding to the offset amount of the triaxial angular velocity detected by the angular velocity detection sensor can be calculated.

  The motion measurement apparatus further includes a reliability calculation unit that calculates a reliability indicating how reliable at least one of the three-axis angular velocities is about at least one of the three-axis angular velocities calculated by the image calculation unit. The correction amount calculation unit can calculate the correction amount based on at least one of the three-axis angular velocities whose reliability calculated by the reliability calculation unit is equal to or greater than a predetermined value. Thus, the correction amount can be calculated with high accuracy using a highly reliable triaxial angular velocity.

  The motion estimation means including the correction amount calculation means described above is corrected when a difference between at least one of the corrected triaxial angular velocities and at least one of the three axis angular velocities calculated by the image calculation means is equal to or greater than a predetermined value. At least one of the three-axis angular velocities is estimated as at least one of the three-axis angular velocities of the movement of the moving body, and at least one of the corrected three-axis angular velocities and at least one of the three-axis angular velocities calculated by the image calculating means; When the difference is less than a predetermined value, at least one of the three-axis angular velocities calculated by the image calculation means can be estimated as at least one of the three-axis angular velocities of the movement of the moving body. Accordingly, when the difference between the triaxial angular velocity calculated based on the image obtained by capturing the outside of the moving body and the corrected triaxial angular velocity is small, the triaxial angular velocity calculated based on the external image is If the difference is large, the corrected triaxial angular velocity can be adopted to ensure robustness and to accurately measure the movement of the moving body.

  The motion estimation means including the correction amount calculation means further corrects at least one of the corrected triaxial angular velocities when at least one absolute value of the corrected triaxial angular velocities is equal to or greater than a predetermined value, At least one of the three-axis angular velocities obtained can be estimated as at least one of the three-axis angular velocities of the moving body. As a result, when the corrected absolute value of the triaxial angular velocity is too large, it can be further corrected, so that the movement of the moving body can be accurately measured.

  The above motion measurement device further includes a monocular imaging unit that images the outside of the moving body, and the image calculation unit is based on a plurality of images captured by the imaging unit and at least the three-axis angular velocity of the motion of the moving body. One can be calculated.

  The image calculation means calculates at least one of the three-axis angular velocities of the moving body and the translation direction based on the plurality of images, and the motion estimation means determines at least the three-axis angular velocities calculated by the image calculation means. Based on one and the translation direction and at least one of the three-axis angular velocities detected by the angular velocity detection sensor, at least one of the three-axis angular velocities of the moving body and the translation direction can be estimated.

  According to a second aspect of the present invention, there is provided a position measuring device comprising: the motion measuring device described above; a moving amount estimating means for estimating a moving amount of the moving body; and a triaxial angular velocity of the moving body estimated by the motion measuring device. And a position calculating means for calculating the position of the moving body based on the moving amount of the moving body estimated by the moving amount estimating means.

  According to the position measuring apparatus of the second invention, at least one of the three-axis angular velocities of the movement of the moving body is estimated by the movement measuring apparatus, and the movement amount of the moving body is estimated by the movement amount estimating means. .

  Then, the position calculation means determines the position of the moving body based on at least one of the three-axis angular velocities of the movement of the moving body estimated by the movement measuring device and the movement amount of the moving body estimated by the movement amount estimation means. calculate.

  In this way, by calculating the position of the moving amount based on the movement of the moving body measured with high accuracy while ensuring robustness and the estimated moving amount of the moving body, the robust structure can be achieved with a simple configuration. The position of the moving body can be measured with high accuracy.

  According to a third aspect of the present invention, there is provided a position measuring device for estimating at least one of the three-axis angular velocities and the translation direction of the moving body, and a moving amount estimating means for estimating a moving amount of the moving body. The moving body is based on at least one of the three-axis angular velocities of the movement of the moving body and the translation direction estimated by the movement measuring device, and the movement amount of the moving body estimated by the movement amount estimating means. And a position calculating means for calculating the position.

  According to the position measuring apparatus of the third invention, at least one of the three-axis angular velocities of the moving body and the translation direction are estimated by the motion measuring apparatus, and the moving amount of the moving body is estimated by the moving amount estimating means. Is estimated.

  Then, based on at least one of the three-axis angular velocities of the movement of the moving body estimated by the movement measuring device and the translation direction, and the movement amount of the moving body estimated by the movement amount estimation means, the position calculation means Calculate the position of the body.

  In this way, by calculating the position of the moving amount based on the movement of the moving body measured with high accuracy while ensuring robustness and the estimated moving amount of the moving body, the robust structure can be achieved with a simple configuration. The position of the moving body can be measured with high accuracy.

  As described above, according to the motion measuring apparatus of the present invention, based on the triaxial angular velocity calculated based on a plurality of images obtained by imaging the outside of the moving body and the triaxial angular velocity detected by the angular velocity detection sensor. Thus, by estimating the three-axis angular velocity of the movement of the moving body, it is possible to obtain an effect that the movement of the moving body can be accurately measured with a simple configuration while ensuring robustness.

  According to the position measuring device of the present invention, by calculating the position of the moving amount based on the movement of the moving body that is measured with high accuracy while ensuring robustness, and the estimated moving amount of the moving body, With a simple configuration, it is possible to obtain an effect that the robustness is ensured and the position of the moving body can be accurately measured.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. An example in which the present invention is applied to a position measurement device mounted on a vehicle will be described.

  As shown in FIG. 1, the position measurement device 10 according to the first embodiment includes a movement measurement unit 12 that measures the movement of the host vehicle, a movement amount measurement unit 14 that measures the movement amount of the host vehicle, and a measurement. And a position estimation unit 16 that estimates the position of the host vehicle based on the movement and movement amount of the host vehicle.

  The motion measurement unit 12 includes an image capturing unit 20 configured with a monocular camera that captures an image in front of the host vehicle, and three-axis angular velocities (pitch angular velocity, yaw angular velocity) of the host vehicle as information related to the motion of the host vehicle. , Roll angular velocity) and a motion estimation unit 24 that estimates the motion of the host vehicle based on the captured image and the detection result of the gyro sensor 22.

  The motion estimation unit 24 estimates the three-axis angular velocity of the host vehicle or a component indicating the three-axis angular velocity and the translation direction as the motion of the host vehicle. In the present embodiment, the motion estimation unit 24 does not estimate the translation amount (translation component scalar amount).

  In the present embodiment, a case where a monocular camera is installed in front of the host vehicle and an image in front of the host vehicle is acquired will be described as an example, but an image outside the host vehicle may be captured. A monocular camera may be installed behind the host vehicle to capture the rear of the host vehicle. The monocular camera may be a normal camera with a field angle of about 40 degrees, a wide-angle camera, or an omnidirectional camera. Further, the type of the wavelength of the image to be captured is not limited. As long as the momentum can be estimated appropriately, the installation location, the angle of view, the wavelength, and the number of installation are not limited.

  Further, the case where the triaxial angular velocity is detected by the gyro sensor 22 will be described as an example. However, the present invention is not limited to this, and the gyro sensor 22 detects at least one of the yaw angular velocity, the pitch angular velocity, and the roll angular velocity. You may comprise as follows.

  Here, the offset amount of the detection result by the gyro sensor 22 will be described below. Normally, there is an offset amount in the triaxial angular velocity detected by the gyro sensor, and if position estimation by dead reckoning navigation is performed using the value as it is without correcting the detected triaxial angular velocity, a large error is accumulated. A position error occurs. Also, since the offset amount varies depending on the temperature of the device, it is not appropriate to use a preset correction value. Therefore, the offset amount is calculated based on the GPS positioning result or map information, and the offset amount is corrected. However, when the vehicle is present in a location where the GPS reception state is not good, such as in the city center, If the number of GPS receptions immediately after power-on is limited, the offset amount cannot be calculated correctly, and the offset amount may not be sufficiently corrected.

  In addition, when motion estimation is performed based on a captured image of a camera, motion estimation is possible regardless of the GPS reception state. For example, as shown in FIG. 2, the temporal change in the angular velocity detected by the gyro sensor 22 and the temporal change in the angular velocity estimated based on the captured image of the monocular camera have the same tendency, but the captured image There is an offset amount in the angular velocity detected by the gyro sensor 22 with respect to the angular velocity based on. However, the reliability of the motion estimated based on the plurality of captured images varies depending on the presence or absence of stable feature points in the captured image and the sunshine conditions. For example, as shown in FIG. 3, the angular velocity estimated from the captured image of the monocular camera may not be obtained stably.

  Next, a motion estimation method in the motion estimation unit 24 will be described. The motion estimator 24 compensates for the robustness of the monocular camera, reduces the influence of the offset generated in the gyro sensor 22 and the output error at the time of the curve, and the motion estimated based on the captured image of the monocular camera, the gyro sensor The 22 detection results are mutually monitored, and an appropriate motion is estimated according to the situation.

  First, based on a plurality of captured images captured by the image capturing unit 20, a triaxial angular velocity or a component indicating a triaxial angular velocity and a translation direction is calculated. Further, the triaxial angular velocity detected by the gyro sensor 22 is acquired, and the reliability of the triaxial angular velocity estimated based on the captured image is calculated.

  Then, taking into account variations in the values of the triaxial angular velocities detected by the gyro sensor 22, the angular velocities acquired over a certain period are averaged, and taking into account variations in the values of the triaxial angular velocities estimated from the captured images. Then, the angular velocities over a certain period estimated based on the captured image are averaged. Then, the offset amount of the gyro sensor 22 is corrected using these average values.

  Here, when the reliability of the triaxial angular velocity based on the captured image of the image capturing unit 20 is lower than a predetermined threshold value, it is considered that the estimated value of the triaxial angular velocity includes a large error. The average value of each of the three-axis angular velocities is calculated except for the three-axis angular velocities with low reliability. In addition, when the triaxial angular velocity estimated based on the captured image of the image capturing unit 20 or the triaxial angular velocity estimated by the motion estimating unit 24 one time ago is equal to or greater than a predetermined value, a feature point in the captured image is determined. Since there is a possibility that the error cannot be stably followed and the error has become large, the average value of each of the three-axis angular velocities is calculated except for the three-axis angular velocities above a predetermined value.

  Further, when the triaxial angular velocity detected by the gyro sensor 22 is a triaxial angular velocity detected immediately after the power is turned on or a triaxial angular velocity equal to or greater than a predetermined value, a large error occurs in the detected triaxial angular velocity. Therefore, the average value of each of the three-axis angular velocities is calculated by excluding the three-axis angular speed detected immediately after the power is turned on and the three-axis angular speed greater than a predetermined value.

Calculated as described above, the difference between the average value _ave R _jyro a certain time of the angular velocity by a gyro sensor 22 T [sec] min, and the average value _ave R _image for a predetermined time of angular velocity according to the captured image T [sec] min ΔR (= _ave R _jiro - _ave R _image ) is calculated, and the calculated difference ΔR is set as an offset amount generated in the angular velocity detected by the gyro sensor 22. Then, the offset amount ΔR is subtracted from the angular velocity R _jyro detected by the gyro sensor 22, and the subtracted value R ′ _jyro (= R _jyro −ΔR) is used as the correction value of the angular velocity detected by the gyro sensor 22. Further, the offset amount as the correction amount is constantly updated, and an appropriate angular velocity is calculated in response to thermal fluctuations.

The movement amount measurement unit 14 includes a vehicle speed sensor 26 that detects information related to the vehicle speed of the host vehicle, and a movement amount estimation unit 28 that estimates the movement amount from the detection result detected by the vehicle speed sensor 26. The vehicle speed sensor 26 is composed of, for example, a vehicle speed pulse sensor that outputs a vehicle speed pulse signal according to the vehicle speed, and outputs a vehicle speed pulse signal according to the ground speed of the host vehicle. The movement amount estimation unit 28 estimates the translation component scalar quantity or the vehicle speed as the movement amount of the host vehicle from the detection result of the vehicle speed sensor. For example, the vehicle speed is estimated based on the number of pulses of the vehicle speed pulse signal in the measurement unit period T _ms .

  The position estimation unit 16 calculates a change amount of the relative position of the own vehicle based on the measured movement and movement amount of the own vehicle, and based on the past absolute position and the change amount of the relative position of the own vehicle. Estimate absolute position. Hereinafter, a method for calculating the amount of change in the relative position will be described with reference to FIG.

First, in the coordinate system as shown in FIG. 4, the initial position is (X (0), Y (0), Z (0)), and the movement amount (speed V (t)) and motion (3 axis angular (yaw rate phi (t), the pitch angular velocity theta (t), the roll angular velocity [psi (t)), the component indicating a translational direction _{(a x, a y, a} z) If) is obtained, the following (1 ), The relative position (X (t + Δt), Y (t + Δt), Z (t + Δt)) at time t + Δt can be updated.

ただし、ａ_ｘ／｜ａ｜、ａ_ｙ／｜ａ｜、及びａ_ｚ／｜ａ｜は、正規化された並進方向を示す成分の各々を表わす。

_{However, a x / | a |,} a y / | a |, and _a z / | _a | represents the respective components indicating the normalized translation direction.

  If absolute position information represented by longitude and latitude has been acquired in the past from a GPS (not shown) mounted on the host vehicle, the absolute position indicated by the absolute position information and the calculated relative position Based on the above, the current absolute position of the host vehicle is measured. In addition, it is not always necessary to acquire the absolute position information from the GPS positioning position, for example, the absolute position information of the moving body may be acquired intermittently by communication or the like from equipment whose roadside position is known, You may acquire the position of the own vehicle from the map etc. which landmark information whose position is known beforehand was registered. Moreover, it is not necessary to acquire at the same frequency as GPS, it is sufficient to acquire at intervals that can be complemented by the amount of movement and exercise, and the acquisition means is not limited.

  Next, the operation of the position measurement apparatus 10 according to the first embodiment will be described. In addition, the case where the position of the own vehicle is measured when the own vehicle carrying the position measuring device 10 is traveling will be described as an example.

  First, in the position measurement apparatus 10, a correction amount calculation processing routine shown in FIG. 5 is executed. In step 100, motion estimation processing based on the captured image is performed. Step 100 is realized by a motion estimation processing routine based on the captured image shown in FIG. The motion estimation processing routine based on the captured image will be described below.

  First, in step 140, a first image and a second image obtained by imaging the front of the host vehicle at different timings are acquired. In step 142, a plurality of first images acquired in step 140 are obtained. Divide into areas. In this step 142, for example, given first constants y1 and y2 as boundary lines (intercept coordinates of the y-axis) of each region, the first image is divided into an upper region in the upper part of the image, an intermediate region in the middle part, and a lower part. It is divided into three regions with the lower region.

  In the next step 144, a predetermined number of feature points (points expected to be fixed points) are extracted from each of the divided images obtained in step 142. Here, the feature points are points that can be distinguished from surrounding points and can be easily tracked. In this step 144, for example, feature points are automatically extracted by using a general well-known method (for example, Harris operator or the like) that detects pixels in which the gradient value of the gradation change is two-dimensionally large.

  In step 146, each feature point in the first image extracted in step 144 is tracked in the second image acquired in step 140, and each corresponding feature point is tracked from the second image. Detect each. At this time, since the two feature points corresponding between the two times are expected to be the same fixed point, the corresponding points in the first image and the second image and the surrounding points Based on this assumption, the feature points are tracked based on the assumption that the brightness of the light hardly changes. When performing such a tracking process, for example, a well-known Lucas-Kanade method may be used. Further, after the tracking process of each feature point, the coordinates in each image of each set of feature points corresponding to each other between the first image and the second image are arranged in one appropriate format.

  In step 148, the relative positional relationship of the image capturing unit 20 between the two image capturing times, that is, the image image capturing unit 20 is determined from the pair of feature points corresponding to each other between the two times obtained in step 146. Information related to the movement of the mounted vehicle between the two times is estimated.

Hereinafter, the relationship between the extracted feature points (fixed points) and the motion of the moving body will be described with reference to FIG. As shown in FIG. 7, the motion of the moving body is composed of a rotation matrix R and a translation vector t from the first image to the second image. For example, when the feature points on the second image corresponding to the feature point x ₁ of the first image and x _2, these feature points, a point showing the same fixed point.

  Here, the relationship between two corresponding feature points is expressed by the following equations (2) to (4) using the calibration matrix K and the basic matrix F that correct image distortion due to the imaging characteristics of the camera. Can be represented.

  As in the above formulas (2) to (4), if a set of eight or more corresponding feature points (that is, the same fixed point) between the two times is obtained, based on that, The above basic matrix F can be calculated. If the camera calibration matrix K is known, as can be seen from the above equation, the movement of the moving body between the two times, that is, the rotation matrix R of the host vehicle and the translation vector t between the two times. Can be requested.

  In step 150, based on the rotation matrix R of the host vehicle obtained in step 148 and the translation vector t, the three-axis angular velocity and the component indicating the translation direction of the host vehicle are calculated. The component indicating the translation direction is output as an estimated value of the motion of the host vehicle, and the motion estimation processing routine based on the captured image is terminated.

  Then, in step 102 of FIG. 5, a reliability indicating how reliable the triaxial angular velocity estimated in step 100 is calculated. In step 102, for example, in the motion estimation process based on the captured image in step 100, the higher the number of feature points associated between the first image and the second image, the higher the reliability is calculated. Is done. That is, the degree representing how much feature points can be collated is calculated as the reliability.

In step 104, the triaxial angular velocity detected by the gyro sensor 22 is acquired. In the following description of Step 106 to Step 124, any one of the three-axis angular velocities R _jyro will be described.

In step 106, the angular velocity _{R Jyro} acquired in step 104 is equal to or less than the threshold value _{R th} predetermined. If it is determined in step 106 that the angular velocity R _jyro is less than the threshold value R _th , the angular velocity R _jyro acquired in step 104 is used as the angular velocity of the average calculation target section T _{ave_jyro} in step 108. . On the other hand, if it is determined in step 106 that the angular velocity R _jyro is _equal to or greater than the threshold value R _th , in step 110, the angular velocity R _jyro acquired in step 104 is set as the angular velocity of the average calculation target section T _{ave_jyro} . Without adopting, the angular velocity of the average calculation target section T _{ave_jyro} obtained last time is held.

In step 112, the average velocity _ave R _jiro is calculated by averaging the angular velocities in the average calculation target section T _{ave_jyro} .

In the next step 114, the reliability calculated in step 102, determines whether the larger predetermined threshold P _th, reliability, if the threshold P _th greater than, at step 116, It is determined whether the angular velocity R _image estimated in step 100 is less than a predetermined threshold value R _th . If it is determined in step 116 that the angular velocity R _image is less than the threshold value R _th , the angular velocity R _image acquired in step 100 is used as the angular velocity of the average calculation target section T _{ave_image} in step 118. Then, the process proceeds to step 122. On the other hand, if it is determined in step 116 that the angular velocity R _image is equal to or greater than the threshold value R _th, it is determined that the obtained angular velocity is large, and the process proceeds to step 120. If the calculated reliability in step 114 is less than the threshold value P _th, it is determined that the reliability of the obtained angular velocity is low, and the process proceeds to step 120.

In step 120, the angular velocity _{R image} acquired in step 100, without employing the angular velocity of the average calculation target section _{T Ave_image,} maintains the angular velocity of the average calculation target section _{T Ave_image} previously obtained, it proceeds to step 122 To do.

Then, in step 122, the average velocity _ave R _image is calculated by averaging the angular velocities of the average calculation target section T _{ave_image} , and in step 124, from the angular velocity average _ave R _jiro calculated in step 112 above, The average value _ave R _image of the angular velocity calculated in the above step 122 is subtracted, and the average value difference ΔR (= _ave R _jiro - _ave R _image ) is calculated as a correction amount and stored in a memory (not shown). Then, the correction amount calculation processing routine ends.

  Note that the processing from step 106 to step 124 is executed for each of the three-axis angular velocities output from the gyro sensor 22, and a correction amount is calculated for each of the three-axis angular velocities and stored in a memory (not shown). The correction amount calculation processing routine described above is periodically executed, and the correction amount of the triaxial angular velocity is periodically updated.

  The threshold necessary for each determination may be a constant determined empirically or a constant determined by numerical analysis from a data group. In addition, a threshold value may be determined by using a mechanism that dynamically changes according to data and surrounding conditions.

  Further, in the position measurement apparatus 10, a motion estimation processing routine shown in FIG. 8 is executed. First, in step 160, similarly to step 100 described above, a motion estimation process based on the captured image is performed, and an estimated value of a component indicating the three-axis angular velocity and the translation direction of the motion of the host vehicle is calculated. In step 162, the triaxial angular velocity detected by the gyro sensor 22 is acquired, and in step 164, the correction amount ΔR of each of the three axial angular velocities calculated by the correction amount calculation processing routine and stored in the memory is acquired.

In the next step 166, for each of the three-axis angular velocities acquired in step 162, the corrected angular velocity R ′ _jyro is calculated by subtracting the correction amount ΔR acquired in step 164 from the angular velocity R _jyro. , The triaxial angular velocity corrected in step 166 and the component indicating the translation direction obtained in step 160 are output as the estimated value of the motion of the host vehicle, and the motion estimation processing routine is terminated.

Further, in the position measurement apparatus 10, a movement amount estimation processing routine shown in FIG. 9 is executed. First, in step 170, the number of vehicle speed pulses output from the vehicle speed sensor 26 in the measurement unit period T _ms is counted. In step 172, the vehicle speed of the host vehicle is estimated based on the number of pulses counted in step 170. In step 174, the vehicle speed of the host vehicle estimated in step 172 is calculated as the amount of movement of the host vehicle. It outputs as an estimated value and complete | finishes a movement amount estimation processing routine.

  Further, in the position measurement device 10, a position measurement processing routine shown in FIG. 10 is executed. First, in step 180, the position of the host vehicle is measured by GPS, and the measured position is set as an initial position.

  In step 182, the above-described motion estimation processing routine is executed to obtain a three-axis angular velocity of the motion of the host vehicle, or an estimated value of a component indicating the three-axis angular velocity and the translation direction. In step 184, the above movement is performed. An amount estimation processing routine is executed to obtain an estimated value of the vehicle speed as the amount of movement of the host vehicle.

  In step 186, based on the three-axis angular velocity of the movement of the own vehicle acquired in step 182 or the component indicating the three-axis angular velocity and the translation direction, and the vehicle speed of the own vehicle acquired in step 184, The amount of change in the relative position of is calculated. In the next step 188, the absolute position of the host vehicle is calculated based on the initial position set in step 180 and the amount of change in the relative position calculated in step 186.

Then, in step 192, after measuring the absolute position by step 188 from step 182, the measurement unit period _{T ms} is determined whether the elapsed measuring unit period _{T ms} has elapsed, the process returns to step 182, The absolute position of the host vehicle is measured again, and the absolute position of the host vehicle is updated.

  Next, the measurement result of the position measurement apparatus 10 according to the present embodiment will be described. Using the true value as the initial position, the offset amount was corrected for the triaxial angular velocity detected by the gyro sensor 22, and the relative position was measured. Further, as a comparison target, the relative position was measured without correcting the offset amount for the triaxial angular velocity detected by the gyro sensor 22 using the true value as the initial position.

  The detection result of a general gyro sensor includes an offset error and an amplitude error during rotation due to its characteristics. Therefore, as shown in FIG. 11, the measurement result when the offset is corrected and the result when the correction is not performed are shown. Comparing with the measurement result, when the offset error is not corrected, the trajectory is extremely widened or reduced depending on the turning direction. On the other hand, it has been found that when a relative position is measured by correcting an offset error as needed based on a captured image, a trajectory relatively close to the true trajectory shape is obtained.

  As described above, according to the position measuring apparatus according to the first embodiment, the average value of the triaxial angular velocities calculated based on a plurality of images obtained by imaging the outside of the host vehicle and the gyro sensor are detected. The difference between the average value of the three-axis angular velocities is calculated as a correction amount, the offset amount of the three-axis angular velocity detected by the gyro sensor is corrected by the calculated correction amount, and the three-axis angular velocity of the motion of the host vehicle is calculated. By estimating, robustness can be ensured with a simple configuration, and the triaxial angular velocity of the motion of the host vehicle can be accurately measured.

  In addition, since the correction amount corresponding to the offset amount of the gyro sensor is calculated using the average value of the three-axis angular velocities that are calculated based on a plurality of captured images and have high reliability, the correction amount is accurately calculated. be able to.

  In addition, by measuring the position of the host vehicle based on the components indicating the three-axis angular velocity and the translation direction of the host vehicle's motion, which are accurately measured while ensuring robustness, and the estimated vehicle speed of the host vehicle With a simple configuration, robustness can be ensured and the position of the host vehicle can be accurately measured.

  Further, by using a monocular camera and a gyro sensor mounted on the vehicle, it is possible to realize position estimation with higher accuracy than using a gyro used in a normal car navigation system.

  In addition, since the captured image outside the vehicle is easily affected by the lighting environment, the motion of the host vehicle may not be calculated based on the captured image. However, by using the gyro sensor together, the motion of the host vehicle can be calculated. Can always be measured.

  Conventionally, when a gyro sensor is used in combination with GPS, the amount of change in the angular direction obtained by GPS and the amount of change obtained by the gyro sensor are monitored to correct the offset amount of the gyro sensor. Since the positioning interval of 1 is usually as long as 1 Hz and the positioning accuracy is not high, the fluctuation in the angular direction is large. In addition, there is a problem in that offset correction cannot be performed stably because the frequency of positioning is reduced in central areas. On the other hand, in the present embodiment, by estimating the triaxial angular velocity using the captured image of the monocular camera, a highly accurate triaxial angular velocity can be obtained at the imaging rate of the camera, so the offset amount of the gyro sensor is effectively reduced. It can be corrected.

  Next, a second embodiment will be described. In addition, since the position measuring device according to the second embodiment has the same configuration as that of the first embodiment, the same reference numerals are given and description thereof is omitted.

  In the second embodiment, when the reliability of the triaxial angular velocity estimated based on the captured image is high, the estimated value of the triaxial angular velocity based on the captured image is used as the estimated value of the motion of the host vehicle. This is different from the first embodiment.

  In the motion estimation unit 24 of the position measurement apparatus 10 according to the second embodiment, the reliability for the triaxial angular velocity based on the captured image of the image capturing unit 20 is calculated, and the calculated reliability is greater than or equal to a predetermined value. Does not use the triaxial angular velocity detected by the gyro sensor 22, but outputs the triaxial angular velocity estimated based on the captured image of the image capturing unit 20 as an estimated value of the motion of the host vehicle.

  Next, a motion estimation processing routine according to the second embodiment will be described with reference to FIG. In addition, about the process similar to 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

First, in step 160, a motion estimation process based on the captured image is performed to calculate an estimated value of a component indicating the triaxial angular velocity and translation direction of the motion of the host vehicle. Then, in step 200, similarly to step 102, the reliability for the triaxial angular velocity estimated in step 100 is calculated. In step 202, the reliability calculated in step 200 is set to a predetermined threshold value. determines whether P _th greater.

If it is determined in step 202 that the reliability is greater than the threshold value P _th, it is determined that the reliability of the estimated value estimated based on the captured image is high, and the calculation in step 160 is performed in step 204. The component indicating the three-axis angular velocity and the translation direction is output as an estimated value of the motion of the host vehicle, and the motion estimation processing routine is terminated.

On the other hand, if it is determined in step 202 that the reliability is equal to or less than the threshold value P _th, it is determined that the reliability of the estimated value estimated based on the captured image is low, and in step 162 the gyro sensor is determined. The triaxial angular velocity detected at 22 is acquired, and in step 164, the correction amount ΔR of each of the triaxial angular velocities calculated by the correction amount calculation processing routine described above and stored in the memory is acquired.

In the next step 166, for each of the three-axis angular velocities acquired in step 162, the corrected angular velocity R ′ _jyro is calculated by subtracting the correction amount ΔR acquired in step 164 from the angular velocity R _jyro. , The triaxial angular velocity corrected in step 166 and the component indicating the translation direction obtained in step 160 are output as the estimated value of the motion of the host vehicle, and the motion estimation processing routine is terminated.

  Other processes are the same as those in the first embodiment, and a description thereof will be omitted.

  Thus, when the reliability with respect to the triaxial angular velocity estimated based on the captured image is high, the estimated value of the triaxial angular velocity based on the captured image is used as the estimated value of the motion of the host vehicle, and is estimated based on the captured image. When the reliability of the three-axis angular velocity is low, the correction value of each angular velocity detected from the gyro sensor is used as the estimated value of the motion of the host vehicle, thereby ensuring robustness with a simple configuration, It is possible to accurately estimate the movement of the host vehicle.

  If not all of the three-axis angular velocities are output from the gyro sensor, the correction value of the angular velocity detected from the gyro sensor and other angular velocities estimated based on the captured image are estimated values of the motion of the host vehicle. As output.

  Next, a third embodiment will be described. In addition, since the position measuring device according to the third embodiment has the same configuration as that of the first embodiment, the same reference numerals are given and description thereof is omitted.

  In the third embodiment, when the difference between the triaxial angular velocity from the gyro sensor corrected with the calculated correction amount and the triaxial angular velocity estimated based on the captured image is small, the captured image is displayed. If the estimated value of the three-axis angular velocity based on the estimated value of the motion of the host vehicle and the absolute value of the three-axis angular velocity from the corrected gyro sensor are equal to or greater than a predetermined value, the corrected three-axis The main difference from the first embodiment is that the angular velocity is further corrected, and the corrected triaxial angular velocity is used as an estimated value of the motion of the host vehicle.

In the motion estimation unit 24 of the position measurement apparatus 10 according to the third embodiment, the difference between the triaxial angular velocity based on the captured image of the image capturing unit 20 and the correction value of the triaxial angular velocity detected from the gyro sensor 22 is calculated. , may become more than a certain amount D _th it is particularly determines that there is an error in the estimate based on the captured image of the monocular camera, the correction value of the three-axis angular velocity detected by the gyro sensor 22, the estimation of the motion of the vehicle Output as a value.

In addition, when the absolute amount of the correction value of the triaxial angular velocity detected from the gyro sensor 22 is equal to or greater than the predetermined amount _Rth , the motion estimation unit 24 may have a larger absolute value than the true value. A high value is determined, and a value obtained by multiplying the correction value of the triaxial angular velocity by a predetermined correction coefficient is output as an estimated value of the motion of the host vehicle.

  Next, a motion estimation processing routine according to the third embodiment will be described with reference to FIG. In addition, about the process similar to 1st Embodiment and 2nd Embodiment, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  First, in step 160, a motion estimation process based on the captured image is performed to calculate an estimated value of a component indicating the triaxial angular velocity and translation direction of the motion of the host vehicle. In step 162, the triaxial angular velocity detected by the gyro sensor 22 is acquired, and in step 164, each correction amount ΔR of the triaxial angular velocity calculated by the correction amount calculation processing routine described above is acquired.

In the next step 166, for each of the three-axis angular velocities obtained in step 162, the corrected angular velocity R ′ _jyro is calculated by subtracting the correction amount ΔR obtained in step 164 from the angular velocity R _jyro . In step 300, for each of the three-axis angular velocities, the absolute value (= | R ′ _jyro −R _image |) of the difference between the corrected angular velocity R ′ _jyro and the angular velocity R _image obtained in step 160 is It is determined whether or not it is less than a predetermined threshold value _Dth . If it is determined in step 300 that the absolute value of the difference between the angular velocity R ′ _jyro and the angular velocity R _image is less than the threshold value D _th , the triaxial angular velocity and translation direction calculated in step 160 are determined in step 204. Is output as an estimated value of the motion of the host vehicle, and the motion estimation processing routine is terminated.

On the other hand, if it is determined in step 300 that the absolute value of the difference between the angular velocity R ′ _jyro and the angular velocity R _image is equal to or greater than the threshold value D _th , the angular velocity corrected in step 166 for each of the three-axis angular velocities. It is determined whether or not the absolute value of R ′ _jyro is less than a predetermined threshold value R _th . If it is determined in step 302 that the absolute value of the corrected angular velocity R ′ _jyro is less than the threshold value R _th , the triaxial angular velocity corrected in step 166 is obtained in step 168, and the obtained in step 160 is obtained. The obtained component indicating the translation direction is output as the estimated value of the motion of the host vehicle, and the motion estimation processing routine is terminated.

On the other hand, in step 302, the absolute value of the corrected angular velocity _R'Jyro is, when it is judged that the threshold value _{R th} or more, at step 304, for each of the 3-axis angular velocity, the correction angular velocity _R'Jyro The angular velocity R ′ _jyro is further corrected by multiplying by a predetermined correction coefficient corr (corr <1) so as to decrease. In the next step 306, the triaxial angular velocity (= R ′ _jyro × corr) corrected in step 304 and the component indicating the translation direction obtained in step 160 are used as the estimated value of the motion of the _host vehicle. And the motion estimation processing routine ends.

  Other processes are the same as those in the first embodiment, and a description thereof will be omitted.

  As described above, when the difference between the three-axis angular velocity calculated based on a plurality of captured images obtained by capturing the outside of the host vehicle and the corrected three-axis angular velocity is small, 3 calculated based on the captured image. Adopting the axial angular velocity as the estimated value of the motion of the host vehicle, and if the difference is large, adopting the corrected three-axis angular velocity as the estimated value of the motion of the own vehicle to ensure the robustness. Can be estimated with high accuracy.

  In addition, if the absolute value of the corrected triaxial angular velocity is too large, it is further corrected and the corrected triaxial angular velocity is used as an estimated value of the motion of the host vehicle to ensure robustness and The motion of the vehicle can be estimated with high accuracy.

  Next, a fourth embodiment will be described. In addition, since the position measuring device according to the fourth embodiment has the same configuration as that of the first embodiment, the same reference numerals are given and description thereof is omitted.

  The fourth embodiment is different from the first embodiment in that the triaxial angular velocity of the motion of the host vehicle is estimated by an optimum processing method using a Kalman filter.

  The motion estimation unit 24 of the position measurement apparatus 10 according to the fourth embodiment observes the three-axis angular velocity detected by the gyro sensor 22 and the three-axis angular velocity estimated based on the captured image of the image capturing unit 20. The process is performed using the Kalman filter optimal processing method, with the estimated values of the three-axis angular velocities of the motion of the host vehicle, the angular acceleration of each of the three-axis angular velocities, and the offset amount of the detected value of the gyro sensor 22 .

In the optimum processing method using the Kalman filter, when the observation value y _image based on the captured image of the monocular camera of the image capturing unit 20 is obtained, the observation noise w _image is set in consideration of the variation of the observation value y _image , and the observation is performed. For the matrix H _image , an observation matrix H _image is set from the relational expression between the observed value y _image and the estimated value x.

Also, if the observed value _{y Jyro} were obtained from the gyro sensor 22, in consideration of the variation of the observed values _{y Jyro,} it sets the observation noise _{w Jyro,} the observation matrix _{H Jyro,} observed value _{y Jyro} the estimated value x The observation matrix H _jyro is set from the relational expression.

  Based on the set observation matrix, observation value, and observation noise, an estimated value x and an estimated error covariance matrix are calculated and output by the Kalman filter. When the observed value cannot be obtained, the predicted value of the estimated value and the predicted value of the estimated error covariance matrix are calculated and output.

  Next, the operation of the position measurement apparatus 10 according to the fourth embodiment will be described. In addition, about the part similar to 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  In the position measurement apparatus 10 according to the fourth embodiment, the above-described motion estimation processing routine based on the captured image is repeatedly executed, and components indicating the estimated triaxial angular velocity and translation direction are repeatedly output. Further, the triaxial angular velocity is repeatedly detected from the gyro sensor 22.

  Further, in the position measurement apparatus 10, a motion estimation processing routine shown in FIG. 14 is executed. First, in step 400, a component indicating the triaxial angular velocity and the translation direction estimated by the motion estimation processing routine based on the captured image is acquired, and in step 402, the triaxial angular velocity detected from the gyro sensor 22 is acquired.

In step 404, based on the three-axis angular velocity acquired in step 400 and the three-axis angular velocity acquired in step 402, each of the three-axis angular velocity, the three-axis angular velocity, and the three-axis angular velocity. An initial value is set for the estimated correction amount x. In the next step 406, Δt is added to time t _i−1 to update to time t _i . In step 408, based on the estimated value x one time before, a predicted value of the estimated value x is calculated, and a predicted value of estimated error covariance is calculated.

  In the next step 410, it is determined whether the triaxial angular velocity estimated by the motion estimation processing routine based on the captured image or the triaxial angular velocity detected from the gyro sensor 22 is input as an observation value at the current timing. If the observation value is not input at the current timing, in step 412, the three-axis angular velocity of the predicted value of the estimated value x calculated in step 408 and the component indicating the translation direction acquired in step 400 are obtained. , Output as an estimated value of the movement of the host vehicle, and return to step 406.

  If it is determined in step 410 that the observation value is input at the current timing, it is determined in step 414 whether or not the triaxial angular velocity detected from the gyro sensor 22 is input as the observation value.

When it is determined in step 414 that the triaxial angular velocity detected from the gyro sensor 22 is input as an observation value, in step 416, the observation matrix H _jyro , observation is performed using the input triaxial angular velocity as an observation value. A value y _jyro and an observation noise w _jiro are set.

On the other hand, if it is determined in step 414 that the triaxial angular velocity estimated by the motion estimation processing routine based on the captured image is input as the observed value, the input triaxial angular velocity is measured in step 418 as the observed value. As such, an observation matrix H _image , an observation value y _image , and an observation noise w _image are set.

  In step 420, the Kalman gain K, the estimated value x, and the estimated error covariance matrix are calculated by the Kalman filter based on the observation matrix, the observation value, and the observation noise set in step 416 or step 418. In the next step 422, the three-axis angular velocity of the estimated value x calculated in step 420 and the component indicating the translation direction acquired in step 400 are output as estimated values of the movement of the host vehicle, and the process returns to step 406. .

  Further, similarly to the first embodiment, a movement amount estimation processing routine is executed, and a vehicle speed as an estimated value of the movement amount of the host vehicle is calculated. Further, using the estimated value of the motion of the host vehicle output from the motion estimation processing routine and the estimated value of the vehicle speed of the host vehicle output from the movement amount estimation processing routine, the same as in the first embodiment. Then, the position measurement processing routine is executed, and the absolute position of the host vehicle is measured.

  Thus, based on the three-axis angular velocity and translation direction calculated based on a plurality of captured images obtained by imaging the outside of the host vehicle by the optimum processing method using the Kalman filter, and the three-axis angular velocity detected by the gyro sensor. Thus, by calculating the estimated value of the motion of the host vehicle, it is possible to ensure the robustness with a simple configuration and accurately measure the motion of the host vehicle.

  In addition, robustness is ensured with a simple configuration by measuring the position of the host vehicle based on the movement of the host vehicle measured with high accuracy while ensuring robustness and the estimated vehicle speed of the host vehicle. Thus, the position of the host vehicle can be accurately measured.

  In the above-described embodiment, an example in which estimation is performed using the Kalman filter has been described. However, other optimal estimation methods such as a well-known particle filter may be used. This is not the case.

  Further, in the above embodiment, the error covariance may be used as a reliability for the estimated value of the estimated motion of the host vehicle. In this case, for example, when it is determined that the error of the estimated value is large due to the error covariance calculated together with the estimated value, the estimated value is not used as the estimated value of the motion of the host vehicle. be able to.

  Next, a fifth embodiment will be described. In addition, since the position measuring device according to the fifth embodiment has the same configuration as that of the first embodiment, the same reference numerals are given and description thereof is omitted.

  The fifth embodiment is different from the first embodiment in that the position of the host vehicle is measured by an optimum processing method using a Kalman filter.

  In the position estimation unit 16 of the position measurement apparatus 10 according to the fifth embodiment, the movement estimated by the movement estimation unit 24 (the component indicating the triaxial angular velocity and the translation direction), the movement estimated by the movement amount estimation unit 28. Optimum using the Kalman filter with the absolute position (Xg, Yg, Zg) measured by GPS (vehicle speed) and GPS as the observed value and the absolute position, azimuth, vehicle speed, and triaxial angular velocity of the vehicle as estimates A position measurement process is performed by a processing method.

In the optimal processing method using the Kalman filter, when the observation value y _{move of the} motion estimated by the motion estimation unit 24 is obtained, the observation noise w _move is set in consideration of the variation of the observation value y _move and the observation is performed. For the matrix H _move , an observation matrix H _move is set from the relational expression between the observed value y _move and the estimated value x.

In addition, when the observation value y _trav of the movement amount estimated by the movement amount estimation unit 28 is obtained, the observation noise w _trav is set in consideration of the variation of the observation value y _trav , and the observation matrix H _trav is An observation matrix H _trav is set from a relational expression between the observation value y _trav and the estimated value x.

Also, if the observed value _{y gps} positioning absolute position is obtained by GPS, by considering the variation of the observed values _{y gps,} it sets the observation noise _{w gps,} the observation matrix _{H gps,} observed value _{y gps} And an observation matrix H _gps are set from the relational expression between and the estimated value x.

  Based on the set observation matrix, observation value, and observation noise, an estimated value x and an estimated error covariance matrix are calculated and output by the Kalman filter. When the observed value cannot be obtained, the predicted value of the estimated value and the predicted value of the estimated error covariance matrix are calculated and output.

  Next, the operation of the position measurement apparatus 10 according to the fifth embodiment will be described. In addition, about the part similar to 1st Embodiment, the same code | symbol is attached | subjected and detailed description is abbreviate | omitted.

  In the position measurement apparatus 10 according to the fifth embodiment, the motion estimation processing routine described in the first embodiment is repeatedly executed, and the movement amount estimation processing routine described above is repeatedly executed. In addition, the absolute position of the host vehicle is measured every predetermined time (for example, every second) by GPS.

  Further, in the position measurement device 10, a position measurement processing routine shown in FIG. 15 is executed. First, in step 500, a component indicating the triaxial angular velocity and the translation direction estimated by the motion estimation processing routine is acquired, and in step 502, the vehicle speed estimated by the movement amount estimation processing routine is acquired. In the next step 504, the absolute position of the host vehicle measured by GPS is acquired.

In step 506, based on the component indicating the triaxial angular velocity and translation direction acquired in step 500, the vehicle speed acquired in step 502, and the absolute position acquired in step 504, the absolute position and azimuth angle are determined. The initial value is set for the estimated value x of the vehicle speed and the triaxial angular velocity. In the next step 508, Δt is added to the time t _i−1 to update to the time t _i . In step 510, based on the estimated value x one time ago, a predicted value of the estimated value x is calculated, and a predicted value of estimated error covariance is calculated.

  In the next step 512, whether or not the three-axis angular velocity estimated by the motion estimation processing routine, the vehicle speed estimated by the movement estimation processing routine, or the absolute position measured by GPS is input as an observation value at the current timing In step 514, the absolute position of the predicted value of the estimated value x calculated in step 510 is output as an estimated value of the absolute position of the host vehicle. Then, the process returns to step 508.

  If it is determined in step 512 that an observation value is input at the current timing, it is determined in step 516 which observation value is input in step 512.

If it is determined in step 516 that a component indicating the triaxial angular velocity and translation direction estimated in the motion estimation processing routine is input as the observation value, in step 518, the input triaxial angular velocity and translation direction are input. An observation matrix H _move , an observation value y _move , and an observation noise w _move are set with the component indicating as an observation value.

On the other hand, if it is determined in step 516 that the vehicle speed estimated by the movement amount estimation processing routine has been input as the observation value, in step 520, the input vehicle speed is used as the observation value, the observation matrix H _trav , An observation value y _trav and an observation noise w _trav are set.

If it is determined in step 516 that an absolute position measured by GPS is input as an observation value, in step 522, the input matrix is used as an observation value, the observation matrix H _gps , the observation value. Set y _gps and observation noise w _gps .

  In step 524, based on the observation matrix, observation value, and observation noise set in step 518, step 520, or step 522, the Kalman gain K, the estimated value x, and the estimated error covariance matrix are calculated using the Kalman filter. Is calculated. In the next step 526, the absolute position of the estimated value x calculated in step 524 is output as an estimated value of the absolute position of the host vehicle, and the process returns to step 508.

  Thus, based on the component indicating the three-axis angular velocity and the translation direction of the estimated movement of the own vehicle, the estimated vehicle speed of the own vehicle, and the position measured by the GPS by the optimum processing method using the Kalman filter. Thus, by calculating the estimated value of the absolute position of the host vehicle, the absolute position of the host vehicle can be accurately measured with a simple configuration while ensuring robustness.

  In the above-described embodiment, in the position measurement process using the Kalman filter, the components indicating the estimated triaxial angular velocity and translational direction of the movement of the host vehicle, the estimated vehicle speed of the host vehicle, and the GPS are used. Although the case where the absolute position is input as an observation value has been described as an example, the present invention is not limited to this. For example, the amount of change in the relative position of the host vehicle estimated based on the component indicating the three-axis angular velocity and translation direction of the estimated host vehicle motion and the estimated vehicle speed is the observed value. The position may be input to position measurement processing using a Kalman filter. In this case, the estimated amount of change in the relative position of the host vehicle and the absolute position measured by GPS are taken as observation values, and the absolute position, azimuth angle, vehicle speed, and triaxial angular velocity of the host vehicle are taken as estimated values, What is necessary is just to perform the position measurement process by the optimal process method using a Kalman filter.

  Further, not only the Kalman filter but also an optimal estimation method such as a particle filter may be used.

  Moreover, in said 1st Embodiment-5th Embodiment, although the case where the vehicle speed as a movement amount was estimated using the vehicle speed sensor was demonstrated to the example, it is not limited to this, The movement amount of the host vehicle may be estimated by other known methods. For example, the amount of movement of the host vehicle may be estimated from a signal from GPS, or the amount of movement may be estimated from a captured image of a monocular camera. When the movement amount is estimated from the captured image of the monocular camera, the movement amount may be measured by assuming the road surface of the three-dimensional feature point space restored from the captured image. Moreover, although the case where the vehicle speed is estimated as the movement amount has been described as an example, the movement distance may be estimated, or the movement vector may be estimated.

  Further, the case where the position measurement processing is sequentially performed online has been described as an example. However, the present invention is not limited to this, and offline using the result of obtaining the estimated value of the movement of the host vehicle and the estimated value of the moving amount. Then, position measurement processing may be performed as post-processing.

It is a block diagram which shows the position measuring device which concerns on the 1st Embodiment of this invention. It is a graph which shows the angular velocity estimated based on a captured image, and the angular velocity detected by a gyro sensor. It is a graph which shows the angular velocity estimated based on a captured image, and the angular velocity detected by a gyro sensor. It is an image figure which shows the coordinate system for demonstrating the calculation method of the variation | change_quantity of a relative position. It is a flowchart which shows the content of the correction amount calculation processing routine in the position measuring device which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the motion estimation process routine based on the captured image in the position measuring device which concerns on the 1st Embodiment of this invention. It is a figure for demonstrating the relationship between the extracted feature point and a motion of a moving body. It is a flowchart which shows the content of the motion estimation process routine in the position measuring device which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the movement amount estimation processing routine in the position measuring device which concerns on the 1st Embodiment of this invention. It is a flowchart which shows the content of the position measurement process routine in the position measuring device which concerns on the 1st Embodiment of this invention. It is a figure which shows the measurement result of the relative position at the time of performing offset correction by the position measurement apparatus which concerns on the 1st Embodiment of this invention, and the measurement result of the relative position when not performing offset correction. It is a flowchart which shows the content of the motion estimation process routine in the position measuring device which concerns on the 2nd Embodiment of this invention. It is a flowchart which shows the content of the motion estimation process routine in the position measuring device which concerns on the 3rd Embodiment of this invention. It is a flowchart which shows the content of the motion estimation process routine in the position measuring device which concerns on the 4th Embodiment of this invention. It is a flowchart which shows the content of the position measurement process routine in the position measurement apparatus which concerns on the 5th Embodiment of this invention.

Explanation of symbols

DESCRIPTION OF SYMBOLS 10 Position measuring device 12 Motion measurement part 14 Movement amount measurement part 16 Position estimation part 20 Image imaging part 22 Gyro sensor 24 Motion estimation part 26 Vehicle speed sensor 28 Movement amount estimation part

Claims (12)

Image calculating means for calculating at least one of the three-axis angular velocities of the movement of the moving body based on a plurality of images obtained by imaging the outside of the moving body;
An angular velocity detection sensor for detecting at least one of the three-axis angular velocities of the movement of the moving body;
Based on at least one of the three-axis angular velocities calculated by the image calculating means and at least one of the three-axis angular velocities detected by the angular velocity detection sensor, at least one of the three-axis angular velocities of the movement of the moving body. Motion estimation means for estimating
Motion measuring device including   The motion estimation unit calculates a correction amount for calculating at least one correction amount of the triaxial angular velocity detected by the angular velocity detection sensor based on at least one of the triaxial angular velocities calculated by the image calculation unit. Means for correcting at least one of the three-axis angular velocities detected by the angular velocity detection sensor based on the correction amount calculated by the correction amount calculation means, and at least one of the corrected three-axis angular velocities. The motion measurement device according to claim 1, wherein the motion is estimated as at least one of three-axis angular velocities of motion of the moving body.   The correction amount calculation unit calculates the correction amount based on a difference between at least one of the three-axis angular velocities calculated by the image calculation unit and at least one of the three-axis angular velocities detected by the angular velocity detection sensor. The motion measuring device according to claim 2 to calculate. A reliability calculation means for calculating a reliability indicating how reliable at least one of the three-axis angular velocities is about at least one of the three-axis angular velocities calculated by the image calculation means;
4. The correction amount calculation unit according to claim 2, wherein the correction amount calculation unit calculates the correction amount based on at least one of the three-axis angular velocities in which the reliability calculated by the reliability calculation unit is a predetermined value or more. Motion measurement device.   When the difference between at least one of the corrected three-axis angular velocities and at least one of the three-axis angular velocities calculated by the image calculation unit is equal to or greater than a predetermined value, the motion estimation unit At least one of the axial angular velocities is estimated as at least one of the three axial velocities of the movement of the moving body, and at least one of the corrected three axial angular velocities and the three axial angular velocities calculated by the image calculating unit. The at least one of the three-axis angular velocities calculated by the image calculation unit is estimated as at least one of the three-axis angular velocities of the movement of the moving body when a difference from at least one is less than a predetermined value. The motion measuring device according to claim 4.   The motion estimation means further corrects at least one of the corrected three-axis angular velocities when at least one absolute value of the corrected three-axis angular velocities is equal to or greater than a predetermined value, and the corrected 3 The motion measuring apparatus according to claim 2, wherein at least one of the axial angular velocities is estimated as at least one of three axial velocities of the motion of the moving body.   The motion measurement device according to claim 2, wherein the correction amount is at least one offset amount of the three-axis angular velocity detected by the angular velocity detection sensor.   The motion measuring apparatus according to claim 1, wherein the angular velocity detection sensor is a gyro sensor. Further comprising monocular imaging means for imaging the outside of the mobile body,
The said image calculation means calculates at least 1 of the triaxial angular velocity of the motion of the said mobile body based on the several image imaged by the said imaging means, The any one of Claims 1-8. Motion measurement device. The image calculation means calculates at least one of the three-axis angular velocities of the movement of the moving body and the translation direction based on the plurality of images,
The motion estimation means is based on at least one of the three-axis angular velocities calculated by the image calculation means and the translation direction and at least one of the three-axis angular velocities detected by the angular speed detection sensor. The motion measuring device according to claim 1, wherein at least one of the three-axis angular velocities of the motion and the translation direction are estimated. The motion measuring device according to any one of claims 1 to 9,
A moving amount estimating means for estimating a moving amount of the moving body;
The position of the moving body is calculated based on at least one of the three-axis angular velocities of the movement of the moving body estimated by the movement measuring device and the movement amount of the moving body estimated by the movement amount estimating means. Position calculating means;
Position measuring device including The motion measuring device according to claim 10;
A moving amount estimating means for estimating a moving amount of the moving body;
Based on at least one of the three-axis angular velocities of the movement of the moving body estimated by the movement measuring device and the translation direction, and the movement amount of the moving body estimated by the movement amount estimating means, Position calculating means for calculating a position;
Position measuring device including

Images