JP6613961B2 - State estimation device, information terminal, information processing system, and program - Google Patents

Description

  The present invention relates to a state estimation device, an information terminal, an information processing system, and a program.

  Conventionally, there has been known a technique for obtaining a posture angle (three axes) and a translational speed (three axes) of a host vehicle by using feature point information corresponding to each other on two or more consecutive camera images. Yes. However, when there is an error in the correspondence between the feature points of the image, a large error may occur in the estimation result.

  On the other hand, in the conventional method described in Patent Document 1, first, the translational velocity is estimated in a state in which the posture angle is already known using a geomagnetic sensor, an acceleration sensor, or the like so as to be hardly affected by the error of the feature point. Next, feature points including erroneous correspondence are detected and excluded by making the translation component known. Then, by recalculating the posture and translational direction velocity of the moving body using the last correct feature point, accurate posture angle and translational direction velocity can be obtained even when there is an error in correspondence of the feature point.

JP 2004-78315 A

  However, in general, when estimating posture and translation speed using multiple images, it is necessary to solve the observation equation using convergence calculation, etc. However, a large estimation error may occur due to the local solution due to the influence of the initial value of the convergence calculation.

  In the prior art, the effect of miscorresponding to feature points is reduced by using an external sensor, but since the posture and translation components are estimated using only the feature point information of the image, the initial values at that time There is a problem that the above problem may occur due to the setting of, and a large erroneous estimation result may be obtained in the end.

  The present invention has been made in view of the above circumstances, and accurately estimates the trajectory of the moving object and the posture of the moving object in consideration of the image information around the moving object and the satellite information acquired by the moving object. An object of the present invention is to provide a state estimation device, an information terminal, an information processing system, and a program.

  In order to achieve the above object, the state estimation apparatus of the present invention includes satellite information at each time received by the receiving means mounted on the moving body and each time imaged by the imaging means mounted on the moving body. An acquisition unit that acquires an image around the moving body as moving body information, and a feature point is extracted from each of the images at each time acquired by the acquisition unit, and each feature point in each of the images at each time A feature point tracking means for tracking the position of each of the feature points tracked by the feature point tracking means in each of the images at each time, a position of each time of the mobile object, and a time of each time of the mobile object Obtained from the posture and the three-dimensional position of each feature point tracked by the feature point tracking means, the difference of each feature point from the position in each of the images at each time, and acquired by the acquisition means The position of each time of the moving body and the posture of each time of the moving body are estimated so as to minimize the cost representing the consistency between the satellite information at each time and the posture of the moving body at each time. And state estimation means.

  In addition, the program of the present invention is a program that includes a computer, satellite information at each time received by the receiving means mounted on the moving body, and the periphery of the moving body at each time imaged by the imaging means mounted on the moving body. Acquisition means for acquiring the image as moving body information, feature points are extracted from each of the images at each time acquired by the acquisition means, and each feature point is tracked in each of the images at each time And the position of each feature point tracked by the feature point tracking means in each of the images at each time, the position of each time of the mobile body, the posture of each time of the mobile body, and the feature points Each feature point obtained from the three-dimensional position of each feature point tracked by the tracking unit, the difference between each feature point and the position of each image at each time, and each time acquired by the acquisition unit State estimation for estimating the position of each time of the mobile body and the posture of each time of the mobile body so as to minimize the cost representing the consistency between the satellite information of the mobile body and the attitude of each time of the mobile body It is a program for functioning as a means.

  According to the present invention, the acquisition means includes satellite information at each time received by the receiving means mounted on the moving body, and an image of the periphery of the moving body at each time imaged by the imaging means mounted on the moving body. Is acquired as moving body information.

  Then, the feature point tracking means extracts feature points from each of the images at each time acquired by the acquisition means, and tracks each feature point in each of the images at each time.

  Then, the state estimation means detects the position of each feature point tracked by the feature point tracking means in each image at each time, the position of each time of the moving object, the posture of each time of the moving object, and the feature point tracking. Each feature point obtained from the three-dimensional position of each feature point tracked by the means, the difference between the position of each feature point in the image at each time, the satellite information at each time obtained by the obtaining means, and each of the moving objects The position of the moving body at each time and the posture of the moving body at each time are estimated so as to minimize the cost representing the consistency with the posture of the time.

  In this way, feature points are extracted from each of the surrounding images of the vehicle at each time, each feature point is tracked in each image at each time, and each image at each time of each tracked feature point is recorded. And the position of each feature point in each image of each time obtained from the position of each time of the own vehicle, the posture of each time of the own vehicle, and the three-dimensional position of each tracked feature point In addition, the position of the host vehicle at each time and the position of the host vehicle at each time are estimated so as to minimize the cost representing the consistency between the acquired satellite information at each time and the attitude of the host vehicle at each time. By doing so, it is possible to accurately estimate the trajectory of the host vehicle and the attitude of the host vehicle in consideration of the image information around the vehicle and the satellite information acquired by the vehicle.

  The present invention also provides clock drift calculation means for calculating a clock drift for each satellite based on the satellite information at each time acquired by the acquisition means, and the satellite at each time acquired by the acquisition means. Obtained from the Doppler shift based on the Doppler shift at each time of each satellite, the clock drift for each satellite calculated by the clock drift calculating means, and the attitude of each time of the mobile body included in the information. Doppler representing the difference between the observed value of the relative speed between the mobile object and each satellite and the estimated value of the relative speed between the mobile object and each satellite obtained from the clock drift and the attitude of each time of the mobile object And a Doppler cost calculation means for calculating a cost, wherein the state estimation means includes the difference and the Doppler. The cost of including the said Doppler cost calculated by the cost calculation means so as to minimize, can be made to estimate the posture of each time the position and the moving body at each time of the moving body.

  The present invention further includes pitch angle calculation means, and the acquisition means is detected by a speed at each time detected by a speed sensor mounted on the moving body and an acceleration sensor mounted on the moving body. Further, the acceleration at each time and the angular velocity at each time detected by the angular velocity sensor mounted on the mobile body are further acquired as the mobile body information, and the pitch angle calculation means is obtained by the acquisition means. Based on the speed of time and the acceleration at each time, the pitch angle at each time of the moving body is calculated, the pitch angle at each time of the moving body calculated by the pitch angle calculating means, and the moving body A pitch change cost calculating means for calculating a pitch change cost that represents a difference from the pitch angle of the postures, and the speed at each time obtained by the obtaining means. A position change cost calculating means for calculating a position change cost representing a difference between a change in the position of the moving body and a change in the position of the moving body, and a movement obtained from the angular velocity at each time obtained by the obtaining means. An azimuth change cost calculating means for calculating an azimuth change cost representing a difference between a change in an azimuth angle of a body and a change in an azimuth angle of the posture of the moving body; and the state estimating means, The Doppler cost calculated by the cost calculation means, the pitch change cost calculated by the pitch change cost calculation means, the position change cost calculated by the position change cost calculation means, and the direction change cost calculation means The position of each moving object and the previous time so as to minimize the cost including the direction change cost calculated by It can be made to estimate the posture of each time mobile.

Further, the state estimation means of the present invention may repeatedly estimate the position of each time of the moving body and the posture of each time of the moving body until a predetermined convergence condition is satisfied. it can.

  Further, the acquisition unit of the present invention acquires the mobile body information for each of the plurality of mobile bodies, and the state estimation unit minimizes the sum of the costs for each of the plurality of mobile bodies. In this way, for each of the plurality of moving bodies, the position of each time of the moving body and the posture of each time of the moving body can be estimated.

  The present invention further includes positioning means, wherein the acquisition means further acquires satellite information at each time point observed at a fixed point observation point and information on the fixed point observation point, and the positioning means includes a plurality of the movements. For each of the bodies, based on the satellite information at each time of the fixed point observation point acquired by the acquisition unit and information on the fixed point observation point, the position of each time of the moving body is estimated, and the state estimation unit Minimizes the sum of the costs including the difference between the position of each time of the moving object estimated by the positioning means and the position of each time of the moving object for each of the plurality of moving objects. As described above, for each of the plurality of moving objects, the position of the moving object at each time and the posture of the moving object can be estimated.

  The information terminal according to the present invention is an information terminal mounted on a mobile body, and includes satellite information at each time received by a receiving unit mounted on the mobile body, and an imaging unit mounted on the mobile body. Images of the surroundings of the moving body at each time taken, speeds at each time detected by a speed sensor mounted on the moving body, accelerations at each time detected by an acceleration sensor mounted on the moving body, And information acquisition means for acquiring at least one angular velocity at each time detected by the angular velocity sensor mounted on the mobile body as the mobile body information, and the mobile body information acquired by the information acquisition means, And communication means for transmitting to the state estimation device of the invention.

  The information processing system of the present invention includes the state estimation device of the present invention and the information terminal of the present invention.

  The storage medium for storing the program of the present invention is not particularly limited, and may be a hard disk or a ROM. Further, it may be a CD-ROM, a DVD disk, a magneto-optical disk or an IC card. Furthermore, the program may be downloaded from a server or the like connected to the network.

  As described above, according to the state estimation device, information terminal, information processing system, and program of the present invention, each feature point extracted from each of the images around the moving body at each time is tracked and tracked. Each feature point is obtained from the position of each feature point in each image at each time, the position of each time point of the moving object, the posture of each time point of the moving object, and the three-dimensional position of each tracked feature point. Each time of the mobile unit so as to minimize the difference between the position of each image at each time and the satellite information obtained at each time and the consistency between the attitude of each time of the mobile unit By estimating the position of the mobile object and the posture of the mobile object at each time, the trajectory of the mobile object and the posture of the mobile object are accurately estimated in consideration of the image information around the mobile object and the satellite information acquired by the mobile object. The effect that it can do is acquired.

It is a block diagram which shows the state estimation apparatus which concerns on the 1st Embodiment of this invention. It is explanatory drawing for demonstrating the flow of the process which concerns on embodiment of this invention. It is explanatory drawing for demonstrating the relationship between the pitch angle of the own vehicle, the azimuth angle of the own vehicle, and a three-dimensional rotation matrix. It is explanatory drawing for demonstrating the relationship between the feature point in an image, and the direction of the own vehicle. It is explanatory drawing for demonstrating the relationship between the feature point in an image, and the direction of the own vehicle. It is explanatory drawing for demonstrating the effect of the 1st Embodiment of this invention. It is a flowchart which shows the content of the vehicle state estimation process routine in the 1st Embodiment of this invention. It is a flowchart which shows the content of the cost calculation processing routine in the 1st Embodiment of this invention. It is explanatory drawing for demonstrating the outline | summary of the 2nd Embodiment of this invention. It is a block diagram which shows the information processing system which concerns on the 2nd Embodiment of this invention. It is a block diagram which shows the vehicle-mounted apparatus which concerns on the 2nd Embodiment of this invention. It is a block diagram which shows the fixed point observation apparatus which concerns on the 2nd Embodiment of this invention. It is a block diagram which shows the state estimation apparatus which concerns on the 2nd Embodiment of this invention. It is explanatory drawing for demonstrating the effect of the 2nd Embodiment of this invention. It is explanatory drawing for demonstrating the effect of the 2nd Embodiment of this invention. It is a flowchart which shows the content of the vehicle state estimation process routine in the 2nd Embodiment of this invention. It is a flowchart which shows the content of the cost calculation processing routine in the 2nd Embodiment of this invention.

  Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings.

<Configuration of State Estimation Device 10 according to First Embodiment of the Present Invention>

  As shown in FIG. 1, the state estimation device 10 according to the first embodiment includes a camera 12 that sequentially captures images around a host vehicle, an acceleration sensor 14 that sequentially detects the acceleration of the host vehicle, and the host vehicle. Speed sensor 16 that sequentially detects the speed of the vehicle, angular velocity sensor 18 that sequentially detects the angular velocity of the host vehicle, a GPS (Global Positioning System) device 19 that sequentially receives satellite information transmitted from each positioning satellite, And a computer 20 for estimating the position and orientation at each time. 1st Embodiment demonstrates the case where the state estimation apparatus 10 is mounted in a vehicle. Further, in the first embodiment, a single vehicle optimizes the information of the images obtained by the GPS device, the internal sensor, and the camera so that the trajectory of the own vehicle and the posture of the own vehicle at each time can be accurately obtained. presume.

  The camera 12 captures an image around the host vehicle at each time. Note that there may be only one camera or a plurality of cameras. Further, the imaging range of the camera 12 may face the traveling direction of the host vehicle, or may be backward or omnidirectional. The camera 12 is an example of an imaging unit.

  The acceleration sensor 14 detects the acceleration of the host vehicle at each time. In this embodiment, the translation acceleration of the host vehicle is acquired. In this embodiment, the minimum requirement is acceleration in the vehicle traveling direction. However, acceleration in the vehicle lateral direction and vertical direction may be acquired and used simultaneously.

  The speed sensor 16 detects the speed of the host vehicle at each time. In the present embodiment, the speed sensor 16 acquires the wheel speed measured by the odometer of the wheel of the host vehicle. Note that vehicle speed estimation means other than the wheel speed (for example, satellite information, ranging information such as laser radar, an acceleration sensor, etc.) may be used.

  The angular velocity sensor 18 detects the angular velocity of the host vehicle at each time. In the present embodiment, the angular velocity sensor 18 detects the yaw rate as an angular velocity. In this embodiment, the angular velocity sensor 18 detects the yaw rate, but may also detect the pitch angular velocity or the roll angular velocity.

  The GPS device 19 receives satellite information from each positioning satellite including pseudorange, Doppler frequency, satellite number, satellite position, satellite velocity, etc. for each time. The GPS device 19 is an example of a receiving unit.

  The computer 20 includes a CPU, a ROM that stores a program for realizing a vehicle state estimation processing routine that will be described later, a RAM that temporarily stores data, and a storage device such as an HDD. When the computer 20 is represented by functional blocks, as shown in FIG. 1, an image acquisition unit 22, an acceleration acquisition unit 24, a vehicle speed acquisition unit 26, an angular velocity acquisition unit 28, a satellite information acquisition unit 30, and data storage. It can be expressed by a configuration including a unit 32, a feature point tracking unit 36, a vehicle pitch angle calculation unit 38, a clock drift calculation unit 40, and a trajectory posture estimation unit 42. The image acquisition unit 22, the acceleration acquisition unit 24, the vehicle speed acquisition unit 26, the angular velocity acquisition unit 28, and the satellite information acquisition unit 30 are examples of acquisition means. The trajectory posture estimation unit 42 is an example of a state estimation unit.

  The image acquisition unit 22 acquires an image around the host vehicle at each time imaged by the camera 12.

  The acceleration acquisition unit 24 acquires the acceleration at each time of the host vehicle detected by the acceleration sensor 14.

  The vehicle speed acquisition unit 26 acquires the speed at each time of the host vehicle detected by the speed sensor 16.

  The angular velocity acquisition unit 28 acquires the angular velocity at each time of the host vehicle detected by the angular velocity sensor 18. The minimum required angular velocity is yaw rate, but pitch rate and roll rate may be acquired and used simultaneously.

  The satellite information acquisition unit 30 acquires the satellite information at each time received by the GPS device 19. The contents of the satellite information include commonly used raw data (eg, pseudorange, Doppler shift, GPS time, SNR, carrier phase), satellite orbit information, troposphere or ionosphere correction factor. In addition to GPS, GLONASS (Global Navigation Satellite System), Beidou (BeiDou Navigation Satellite System), Gallileo, QZSS (Quasi Zenith Satellites System), and the like may be used.

  The data storage unit 32 includes an image at each time acquired by the image acquisition unit 22, an acceleration at each time acquired by the acceleration acquisition unit 24, a speed at each time acquired by the vehicle speed acquisition unit 26, and an angular velocity. The angular velocity at each time acquired by the acquisition unit 28 and the satellite information at each time acquired by the satellite information acquisition unit 30 are accumulated in time series as vehicle data. In this embodiment, data accumulation is performed every time the travel distance increases by 1 m, and when the accumulated data exceeds 100, the oldest data is discarded every time new data is accumulated. However, the accumulation timing may be every arbitrary time step or data reception, and the number of accumulated data may be increased or decreased. Each data stored in the data storage unit 32 is an example of mobile information.

  The feature point tracking unit 36 extracts feature points from each image at each time stored in the data storage unit 32, and tracks each feature point extracted from each image at each time. A feature point is a characteristic pixel pattern extracted from an image.

  Specifically, the feature point tracking unit 36 first extracts N feature points from the oldest image stored in the data storage unit 32. Next, the feature point tracking unit 36 sequentially tracks each feature point in the image at the new time based on the result of extracting the feature points for each image stored in the data storage unit 32.

  Note that the feature point tracking unit 36 does not track any more feature points if tracking of feature points fails during the tracking process. Instead, the feature point tracking unit 36 newly adds another feature in the image at that time. A point is extracted and a tracking process is performed. As a result, the processing by the feature point tracking unit 36 calculates an array x (t, i) that stores the on-image coordinates of the feature point i at time t.

  Various methods have been proposed for selecting feature points, but any method such as SIFT with high robustness, Harris with easy implementation, FAST with low calculation load, etc. may be used. In addition to point features, feature quantities such as lines and surfaces may be used. The feature point tracking method may be a general method such as Lucas-Kanade or another method.

  The vehicle pitch angle calculation unit 38 calculates the pitch angle at each time of the host vehicle based on the speed at each time and the acceleration at each time stored in the data storage unit 32. The following formula (1) shows a formula for calculating the pitch angle.

  In the above equation (1), φ is the pitch angle of the host vehicle, A is the acceleration in the traveling direction of the host vehicle acquired by the acceleration acquiring unit 24, v is the speed acquired by the vehicle speed acquiring unit 26, g is gravitational acceleration, T Indicates the current time, and Δt indicates an arbitrary time step. Here, Δt is 0.5 s, but other values may be used.

  The clock drift calculation unit 40 calculates the clock drift for each satellite based on the satellite information at each time stored in the data storage unit 32. Clock drift represents the rate of change in clock error of the satellite receiver. The following formula (2) shows a general clock drift calculation formula.

Index i of the expression (2) indicates that the observed value of the satellite i, _{D i} is the Doppler shift [Hz], f1 is the frequency of the satellite signal [Hz], c is the speed of light [m / s], V is 3-dimensional velocity vector of the vehicle [m / s], v _i is the three-dimensional velocity vector [m / s] of the satellite i, [rho watch error change rate of the receiver per unit time (clock drift) [s / s ] Is shown. G _i is a line-of-sight direction unit vector from the own vehicle to the satellite i, and is expressed using an approximate three-dimensional position X [m] of the own vehicle and a three-dimensional position x _i [m] of the satellite i.

In the above equation (2), D _i is an observed value obtained from the receiver, f 1 and c are known fixed values, and v _i and x _i are generally known values that can be calculated more accurately using satellite orbit information. , X is a value that can be calculated by a general method using the pseudorange observation value, and therefore, there are four unknowns: a three-dimensional velocity vector V of the own vehicle and a clock drift ρ.

  Therefore, when four or more satellites are observed, the clock drift calculation unit 40 calculates the clock drift by solving the observation equation (2).

  Although the method for calculating the clock drift using only satellite information has been described above, the clock drift calculation method that improves the robustness to the satellite reception environment by using the time series data of satellite information, gyroscope, and wheel speed ( For example, you may use Unexamined-Japanese-Patent No. 2013-113789).

  The trajectory posture estimation unit 42 includes satellite information at each time stored in the data storage unit 32, tracking results of each feature point extracted in each of the images at each time by the feature point tracking unit 36, and vehicle pitch angle calculation. Based on the pitch angle at each time of the host vehicle calculated by the unit 38 and the clock drift calculated by the clock drift calculating unit 40, the trajectory of the host vehicle at each time and the posture of the host vehicle at each time are estimated.

  The trajectory posture estimation unit 42 includes a provisional trajectory posture calculation unit 44, a feature point cost calculation unit 46, a pitch change cost calculation unit 48, a position change cost calculation unit 50, an azimuth change cost calculation unit 52, and a Doppler cost calculation. A unit 54 and an optimum trajectory posture determination unit 56.

  FIG. 2 shows a diagram for explaining the processing of the trajectory posture estimation unit 42. In the trajectory posture estimation unit 42, as shown in FIG. 2, the trajectory of the host vehicle, the posture of the host vehicle, and the features extracted from the images at the respective times by the feature point tracking unit 36 by the provisional trajectory posture calculation unit 44. A provisional value of the three-dimensional position of the point is calculated. Next, the cost for the calculated provisional value is calculated by each cost calculation unit. Then, the optimal trajectory posture determination unit 56 determines whether or not a predetermined convergence condition is satisfied, and outputs the optimal trajectory of the host vehicle and the posture of the host vehicle. Hereinafter, each process will be described.

  When the convergence calculation is the first time, the temporary trajectory posture calculation unit 44 uses the trajectory at each time of the host vehicle, the posture at each time of the host vehicle, and the three-dimensional position of each feature point tracked by the feature point tracking unit 36. An appropriate value is set as an initial value.

  The initial value of the attitude of the host vehicle may be set using the azimuth angle and pitch angle stored in the data storage unit 32 as they are, and the roll angle is set to zero. The initial value of the trajectory of the host vehicle may be simply calculated based on the initial value of the attitude of the host vehicle and the speed of the host vehicle stored in the data storage unit 32. If the initial values of the trajectory of the host vehicle and the attitude of the host vehicle are obtained, the feature point positions in the image are converted into feature point positions in the real world based on the general triangulation principle and used as initial values. be able to.

  When the convergence calculation is performed for the second time or later, the temporary trajectory posture calculation unit 44 includes a feature point cost calculation unit 46, a pitch change cost calculation unit 48, a position change cost calculation unit 50, an orientation change cost calculation unit 52, and a Doppler, which will be described later. Based on each cost calculated by the cost calculation unit 54, the trajectory of each time of the host vehicle and the attitude of each time of the host vehicle are estimated. In the second and subsequent convergence operations, the output value is sequentially corrected to a correct value by feeding back the result of each cost calculation unit. The process when the convergence calculation is performed for the second time or later will be described after the process of each cost calculation unit is described.

  The feature point cost calculation unit 46 includes the position of each feature point tracked by the feature point tracking unit 36, the position of each time of the host vehicle obtained by the provisional trajectory posture calculation unit 44, the posture of each time of the host vehicle, And a feature point cost representing a difference between each feature point and each position of the image at each time, which is obtained from the three-dimensional position of each feature point tracked by the feature point tracking unit 36.

  The feature point cost calculated by the feature point cost calculation unit 46 includes the observed value of the image (the position of the feature point), the trajectory of the host vehicle and the posture of the host vehicle tentatively calculated by the temporary trajectory posture calculation unit 44. The feature point cost is set so as to increase as the consistency decreases. As the feature point cost, for example, the cost can be set as shown in the following formula (3).

In the above formula (3), the index t indicates a time index, and m indicates a feature point index. X _{t, m} ′ is a two-dimensional position in the image of the feature point m at time t, R _t is a provisional value of the attitude of the host vehicle (three-dimensional rotation matrix), I is a three-dimensional unit matrix, and x _t is a time provisional value of the three-dimensional position of the vehicle t, X _m is the provisional value of the three-dimensional position of the feature point m in the real world, K is the calibration matrix of the camera (3 × 3), the feature point M is utilized to estimate , T is the number of time-series data used for estimation, e _cam is the residual of each observation value, and E _cam is the total sum (cost) of the residual. The pitch angle φ _t at time t in the posture of the host vehicle, the azimuth angle θ _t at time t in the posture of the host vehicle, and the three-dimensional rotation matrix R _t representing the posture of the host vehicle are as follows: It is represented by the formula of

  In the above formula, the suffix t is omitted. As shown in FIG. 3, the rotation around the X axis (pitch angle) is represented by φ, the rotation around the Y axis (azimuth angle, yaw angle) is represented by θ, and the rotation around the Z axis (roll angle) is represented by ψ.

  Note that the two-dimensional position coordinates in the image are given in units of pixels with the center point or end point (such as the lower left corner) of the image as the origin. Further, the three-dimensional position coordinates in the real world are plane coordinates with an arbitrary position (such as a position of t = 0) as the origin, and [north-south direction, east-west direction, height direction] are elements of position coordinates.

  The pitch change cost calculation unit 48 calculates the difference between the pitch angle of the host vehicle at each time calculated by the vehicle pitch angle calculation unit 38 and the pitch angle of the host vehicle posture obtained by the provisional trajectory posture calculation unit 44. Is calculated.

  The pitch change cost calculated by the pitch change cost calculation unit 48 is the difference between the vehicle pitch angle calculated by the vehicle pitch angle calculation unit 38 and the pitch angle of the posture provisionally calculated by the temporary trajectory posture calculation unit 44. This is an index for evaluating consistency, and the cost is set so as to increase as the consistency decreases. For example, the cost can be set as shown in the following formula (4).

φ _t ′ is the vehicle pitch angle at time t calculated by the vehicle pitch angle calculation unit 38, φ _t is the provisional value of the pitch angle at time t calculated by the provisional trajectory posture calculation unit 44, and e _Δpitch is each observation value The residual, E _Δpitch, represents the sum of the residuals (cost). The vehicle pitch angle calculated by the vehicle pitch angle calculation unit 38 and the temporary pitch angle calculated by the temporary trajectory posture calculation unit 44 are respectively the pitch angle of the acceleration sensor and the pitch angle of the vehicle body, and always have a certain difference. Therefore, both take the difference with the amount of change from t = 0 as the comparison target.

  The position change cost calculation unit 50 changes the position of the own vehicle obtained from the speed of the own vehicle stored in the data storage unit 32 and the change in the position of the own vehicle obtained by the temporary trajectory posture calculation unit 44. The position change cost representing the difference between is calculated.

  The position change cost calculation unit 50 calculates the position change cost based on the speed of the host vehicle stored in the data storage unit 32 and the track of the host vehicle calculated by the provisional track posture calculation unit 44. The position change cost calculated here is the position change based on the speed of the own vehicle accumulated in the data accumulating unit 32 and the amount of change based on the trajectory of the own vehicle tentatively calculated by the temporary trajectory posture calculating unit 44. This is an index for evaluating consistency, and the cost is set so as to increase as the consistency decreases. For example, the cost can be set as shown in the following formula (5).

V _t is the speed of the host vehicle at time t stored in the data storage unit 32, x _t is a provisional value of the track of the host vehicle at time t calculated by the provisional track attitude calculation unit 44, and Δt is a time step of stored data. , E _Δpos is the residual of each observed value, and E _Δpos is the sum of the residuals (cost).

  The azimuth change cost calculation unit 52 includes a change in the azimuth angle of the host vehicle obtained from the angular velocity at each time stored in the data storage unit 32 and the azimuth among the posture of the host vehicle obtained by the provisional trajectory posture calculation unit 44. An azimuth change cost representing a difference from a change in angle is calculated.

  The azimuth change cost calculation unit 52 calculates the azimuth change cost based on the yaw rate accumulated in the data accumulation unit 32 and the azimuth angle in the posture of the host vehicle calculated by the temporary trajectory posture calculation unit 44. The azimuth change cost calculated here is an index for evaluating the consistency between the yaw rate accumulated in the data accumulation unit 32 and the azimuth angle change amount of the tentatively calculated attitude of the host vehicle. The lower the value, the higher the cost. For example, the cost can be set as shown in the following formula (6).

ω _t is the yaw rate at time t accumulated in the data accumulation unit 32, θ _t is the provisional value of the azimuth angle at time t calculated by the provisional trajectory posture calculation unit 44, Δt is the time step of accumulated data, and e _Δheading is each The residual of the observed value, E _Δheading, is the sum of the residuals (cost).

  4 and 5 are diagrams for explaining the relationship between the feature points in the image and the position of the host vehicle.

  As shown in FIG. 4, when an estimation error occurs in the azimuth angle of the own vehicle, the position of the feature point in the image and the error in the azimuth angle of the own vehicle influence each other, and the estimation error increases synergistically. Go. As a result, the estimation error of the trajectory of the own vehicle also increases.

  Therefore, in the present embodiment, as shown in FIG. 5, satellite information is taken into account by the Doppler cost calculated by the Doppler cost calculation unit 54 described later. By considering the satellite information, the estimation error of the azimuth angle in the attitude of the host vehicle does not increase, so that the estimation error of the position of the feature point is less likely to increase, and a synergistic reduction effect of the estimation error is obtained. It is done. As a result, the estimation error of the trajectory of the own vehicle is hardly increased.

  The Doppler cost calculation unit 54 represents the consistency between the satellite information at each time stored in the data storage unit 32 and the azimuth of the attitude at each time of the host vehicle calculated by the provisional trajectory posture calculation unit 44. Calculate the Doppler cost.

  Specifically, the Doppler cost calculation unit 54 includes the Doppler shift at each time of each satellite included in the satellite information at each time stored in the data storage unit 32 and each satellite calculated by the clock drift calculation unit 40. Of the relative velocity between the vehicle and each satellite determined from the Doppler shift based on the clock drift and the azimuth of the vehicle's posture at each time calculated by the provisional trajectory posture calculation unit 44 And the Doppler cost representing the difference between the clock drift calculated by the clock drift calculation unit 40 and the azimuth angle of the attitude of the host vehicle at each time, and the estimated value of the relative speed between the host vehicle and each satellite. To do.

  The Doppler cost calculation unit 54 calculates the Doppler cost based on the Doppler shift of the satellite information stored in the data storage unit 32 and the azimuth of the vehicle attitude calculated by the provisional trajectory posture calculation unit 44. . The Doppler cost calculated here is an index for evaluating the consistency between the Doppler shift of the satellite information stored in the data storage unit 32 and the azimuth of the posture temporarily calculated by the temporary trajectory posture calculation unit 44. The cost is set to increase as the consistency decreases.

The index i in the above equation (7) indicates that the observation value of the satellite i, D _{t, i} is the Doppler shift of the satellite i at time t, f1 is the frequency of the satellite signal, c is the speed of light, and V _t is the data The speed of the host vehicle accumulated at time t, v _{t, i} is the three-dimensional velocity vector [m / s] of satellite i at time t, and ρ _t is the time t calculated by the clock drift calculator 40. Clock drift, θ _t , indicates a provisional value of the azimuth of the posture of the host vehicle at time t calculated by the provisional trajectory posture calculation unit 44. G _{t, i} is a line-of-sight direction unit vector from the own vehicle to the satellite i at time t, and is expressed using the approximate three-dimensional position X _{t of} the own vehicle and the three-dimensional position x _{t, i} of the satellite i. Further, E _doppler is the total sum (cost) of the residuals.

  When the convergence calculation is performed for the second time or later, the temporary trajectory posture calculation unit 44 performs a feature point cost calculation unit 46, a pitch change cost calculation unit 48, a position change cost calculation unit 50, an orientation change cost calculation unit 52, and a Doppler cost calculation. Based on each cost calculated by the unit 54, the trajectory of each time of the host vehicle and the attitude of each time of the host vehicle are estimated.

  Specifically, the provisional trajectory posture calculation unit 44 calculates the previous time for the position of the vehicle at each time, the posture of the vehicle at each time, and the three-dimensional position of the feature point based on the result of each cost calculation unit. A correction amount δp from the value is calculated according to the following equation (8).

  Then, the temporary trajectory posture calculation unit 44 uses the previous output value p and the correction amount δp calculated above, as shown in the following equation (9), the time position of the host vehicle, and the time of the host vehicle. And the three-dimensional position of the feature point are updated. Thereby, the position of each time of the own vehicle and the attitude of each time of the own vehicle are estimated so as to minimize the cost calculated by each cost calculating unit.

  In the present embodiment, an example of the Newton method, which is a general and simple optimization method, is shown, but the more robust Levenberg-Marquardt method or the like may be used.

The optimum trajectory posture determination unit 56 determines whether or not the result calculated by the provisional trajectory posture calculation unit 44 is an optimal value. If it is determined that the result is an optimal value, the optimal trajectory posture determination unit 56 determines the optimal trajectory and own vehicle trajectory. Output as the attitude of the vehicle. The optimum value is determined when the E _total value, which is the sum of the residuals, is less than a certain value, or when the difference from the previous value is less than a certain value when the E _total value is updated. Further, other conditions may be set.

  FIG. 6 is a diagram for explaining the effect of the present embodiment. As shown in FIG. 6, in this embodiment, the trajectory of the host vehicle and the attitude of the host vehicle are estimated so as to minimize the cost calculated by each cost calculation unit. The trajectory of the subject vehicle and the posture of the subject vehicle are estimated. Further, by using the feature points of the image and the posture of the host vehicle obtained from the satellite information, the trajectory of the host vehicle and the posture of the host vehicle are accurately estimated.

<Operation of the state estimation device 10 according to the first embodiment>

  Next, the operation of the state estimation device 10 according to the first embodiment will be described.

  At each time, the GPS device 19 receives satellite information from each positioning satellite, each sensor detects its own vehicle data, and the vehicle data detected by each sensor by each acquisition unit is stored in the data storage unit 32. 7, the vehicle state estimation processing routine shown in FIG.

  In step S100, each data stored in the data storage unit 32 is acquired.

  In step S102, the feature point tracking unit 36 extracts feature points from each of the images at each time acquired in step S100.

  In step S104, the feature point tracking unit 36 tracks each feature point in each image at each time based on the feature point extracted from each image at each time in step S102.

  In step S106, the vehicle pitch angle calculation unit 38, based on the speed at each time and the acceleration at each time acquired in step S100, the pitch at each time of the own vehicle according to the above equation (1). Calculate the corner.

  In step S108, the clock drift calculation unit 40 calculates the clock drift for each satellite according to the above equation (2) based on the satellite information at each time acquired in step S100.

  In step S110, when the convergence calculation is the first time, the provisional trajectory posture calculation unit 44 determines the trajectory of each time of the own vehicle, the posture of each time of the own vehicle, and the feature points tracked by the feature point tracking unit 36. As a three-dimensional position, an appropriate value is set as an initial value. Alternatively, when the convergence calculation is performed for the second time or later, the temporary trajectory posture calculation unit 44 determines each time of the host vehicle according to the above formulas (8) to (9) based on the costs calculated in the previous step S112. And the posture of the vehicle at each time are estimated.

  In step S112, each cost calculation unit sets the trajectory of each time of the host vehicle obtained in step S110, the posture of each time of the host vehicle, and the three-dimensional position of each feature point tracked by the feature point tracking unit 36. Based on this, each cost is calculated. Step S112 is realized by the cost calculation processing routine shown in FIG.

<Cost calculation processing routine>

  In step S200, the feature point cost calculation unit 46, according to the above equation (3), the position of each tracked feature point obtained in step S104 and the position of each time of the host vehicle obtained in step S110. The feature point cost representing the difference between the position of each feature point in the image at each time, which is obtained from the attitude of the vehicle at each time, and the three-dimensional position of each feature point tracked in step S104 is calculated. To do.

  In step S202, the pitch change cost calculation unit 48 calculates the pitch angle at each time of the host vehicle calculated in step S106 and the attitude of the host vehicle obtained in step S110 according to the above equation (4). A pitch change cost representing a difference from the difference from the pitch angle is calculated.

  In step S204, the position change cost calculation unit 50 obtains the change in the position of the host vehicle obtained from the speed of the host vehicle obtained in step S100 according to the above equation (5) and the step S110. The position change cost representing the difference from the change in the position of the own vehicle is calculated.

  In step S206, the azimuth change cost calculation unit 52 changes the azimuth angle of the host vehicle obtained from the angular velocity at each time obtained in step S100 according to the above equation (6), and the own change obtained in step S110. An azimuth change cost representing a difference from an azimuth change in the posture of the vehicle is calculated.

  In step S208, the Doppler cost calculation unit 54, according to the above equation (7), the satellite information at each time acquired in step S100 and the orientation of the attitude of each time of the host vehicle obtained in step S110. The Doppler cost representing the consistency with the corner is calculated.

  In step S210, each cost calculated in steps S200 to S208 is output as a result, and the cost calculation processing routine is terminated.

  Next, returning to step S114 of the vehicle state estimation processing routine shown in FIG. 7, the optimum trajectory posture determination unit 56 determines whether or not the result calculated in step S110 is an optimum value. If it is determined that the result calculated in step S110 is the optimum value, the process proceeds to step S116. On the other hand, if it is determined that the result calculated in step S110 is not the optimum value, the process returns to step S110.

  In step S116, the optimal trajectory posture determination unit 56 outputs the trajectory of the host vehicle determined to be the optimum value and the posture of the host vehicle at each time as a result, and the vehicle state estimation processing routine is ended.

  As described above, according to the vehicle state estimation device according to the embodiment of the present invention, feature points are extracted from each of the images around the host vehicle at each time, and each feature point in each image at each time is extracted. From the position of each tracked feature point in each image at each time, the position of each time of the host vehicle, the posture of each time of the host vehicle, and the three-dimensional position of each tracked feature point In order to minimize the cost of representing the difference between the obtained position of each feature point in each image at each time and the consistency between the acquired satellite information at each time and the attitude of each vehicle at each time By estimating the position of the host vehicle at each time and the attitude of the host vehicle at each time, the trajectory of the host vehicle and the attitude of the host vehicle are considered in consideration of image information around the vehicle and satellite information acquired by the vehicle. Can be estimated with high accuracy.

  In addition, since the degree of deviation from the observation value of information other than image information (gyro, wheel speed, GPS) is used as the cost function of the maximum likelihood estimation convergence calculation, convergence to an incorrect local solution can be avoided. The estimation accuracy of the trajectory of the vehicle and the posture of the host vehicle can be improved.

<Second Embodiment>
Next, a state estimation apparatus 210 according to the second embodiment of the present invention will be described. In addition, about the structure similar to 1st Embodiment, and the same process, the same code | symbol is attached | subjected and description is abbreviate | omitted.

  The second embodiment is different from the first embodiment in that the state estimation device 210 is configured as a server and receives information acquired by each sensor of the in-vehicle device. In the second embodiment, various information and images obtained by a GPS device, an internal sensor, and a camera included in an in-vehicle device of a plurality of vehicles are transmitted to the state estimation device 210 that is an external data server. And the state estimation apparatus 210 accumulate | stores the vehicle data transmitted from the vehicle-mounted apparatus of a vehicle. Then, as shown in FIG. 9, the state estimation device 210 performs RTK (Real Time Kinematic) positioning for each vehicle based on the satellite information observed at the fixed observation point, and combines the positioning results of each vehicle. By optimizing the data, the trajectory of each vehicle and the posture of each vehicle are accurately estimated.

<Configuration of Information Processing System 100 according to Second Embodiment>
As illustrated in FIG. 10, the information processing system 100 according to the second embodiment includes a plurality of in-vehicle devices 102, a fixed point observation device 110, and a state estimation device 210. The plurality of in-vehicle devices 102, the fixed point observation device 110, and the state estimation device 210 are connected via a network 150 such as the Internet. The communication means may be WiFi (registered trademark), a mobile phone line, or other means.

<In-vehicle device>
As shown in FIG. 11, the in-vehicle device 102 includes a camera 12 that sequentially captures images around the host vehicle, an acceleration sensor 14 that sequentially detects the acceleration of the host vehicle, and a speed sensor 16 that sequentially detects the speed of the host vehicle. An angular velocity sensor 18 that sequentially detects the angular velocity of the host vehicle, a GPS device 19 that sequentially receives satellite information transmitted from each positioning satellite, and a computer 104 that transmits information obtained by each sensor to the state estimation device 210. And. The in-vehicle device 102 is an example of an information terminal.

  The computer 104 includes a CPU, a ROM that stores a program for realizing each processing routine, a RAM that temporarily stores data, and a storage device such as an HDD. When this computer 104 is represented by functional blocks, as shown in FIG. 11, an image acquisition unit 22, an acceleration acquisition unit 24, a vehicle speed acquisition unit 26, an angular velocity acquisition unit 28, a satellite information acquisition unit 30, and vehicle data It can be expressed by a configuration including the transmission unit 106. The image acquisition unit 22, the acceleration acquisition unit 24, the vehicle speed acquisition unit 26, the angular velocity acquisition unit 28, and the satellite information acquisition unit 30 are examples of information acquisition means.

  The vehicle data transmission unit 106 includes an image at each time acquired by the image acquisition unit 22, an acceleration at each time acquired by the acceleration acquisition unit 24, a speed at each time acquired by the vehicle speed acquisition unit 26, and an angular velocity. The angular velocity at each time acquired by the acquisition unit 28 and the satellite information at each time acquired by the satellite information acquisition unit 30 are sequentially transmitted to the state estimation device 210 as vehicle data.

<Fixed point observation device>
As shown in FIG. 12, the fixed point observation device 110 sequentially receives the satellite information transmitted from each positioning satellite, the satellite information received by the GPS device 114, and information on the fixed point observation point. And a computer 112 for transmission to the state estimation device 210. In the present embodiment, fixed point observation apparatus 110 is installed at a reference station. The fixed point observation point is an example of a fixed point observation point.

  The computer 112 includes a CPU, a ROM that stores programs for realizing each processing routine, a RAM that temporarily stores data, and a storage device such as an HDD. When this computer 112 is represented by functional blocks, as shown in FIG. 12, a satellite information acquisition unit 116 that acquires satellite information at each time received by the GPS device 114, and a fixed point acquired by the satellite information acquisition unit 116. It can be expressed by a configuration including a fixed point observation data transmission unit 118 that sequentially transmits satellite information observed at the observation point and information regarding the fixed point observation point to the state estimation device 210.

  The satellite information acquisition unit 116 acquires raw data (for example, pseudorange, Doppler shift, GPS time, SNR (Signal to Noise Ratio), and carrier wave phase) observed at a fixed point observation point such as an electronic reference point. .

  The fixed point observation data transmission unit 118 transmits the satellite information acquired by the satellite information acquisition unit 116 and the information regarding the fixed point observation point to the state estimation device 210 as fixed point observation data. Further, the reference station data may be limited to observation data at fixed point observation points in a certain area such as a city, or observation data at a wider range of fixed point observation points such as nationwide or the whole world may be transmitted.

<State estimation device>
As shown in FIG. 13, the state estimation device 210 according to the second embodiment includes a CPU, a ROM that stores a program for realizing each processing routine described later, a RAM that temporarily stores data, a storage It is composed of a server including a memory, a network interface, etc. as means, and functionally, a vehicle data receiving unit 230, a fixed point observation data receiving unit 231, a data storage unit 232, and a feature point tracking unit 36 The vehicle pitch angle calculation unit 38, the clock drift calculation unit 40, the high-precision positioning unit 241, and the trajectory posture estimation unit 242 can be represented. In the second embodiment, the vehicle data receiving unit 230 and the fixed point observation data receiving unit 231 are examples of acquisition means. The trajectory posture estimation unit 242 is an example of vehicle state estimation means.

  The vehicle data receiving unit 230 receives vehicle data transmitted by the plurality of in-vehicle devices 102.

  The fixed point observation data receiving unit 231 receives the fixed point observation data transmitted from the fixed point observation device 110.

  The data storage unit 232 stores the vehicle data for each of the plurality of vehicles received by the vehicle data reception unit 230 and the fixed point observation data received by the fixed point observation data reception unit 231 in time series. The

  The feature point tracking unit 236 extracts, for each of the plurality of vehicle data stored in the data storage unit 232, feature points from each of the images at each time of the vehicle data, and the feature points are extracted from each of the images at each time. Track each feature point.

  The vehicle pitch angle calculation unit 238, for each of the plurality of vehicle data stored in the data storage unit 232, according to the above formula (1) based on the speed of the vehicle at each time and the acceleration at each time of the vehicle data. The pitch angle of each time of the vehicle is calculated.

  For each of the plurality of vehicle data stored in the data storage unit 232, the clock drift calculation unit 240 calculates the clock drift for each satellite based on the satellite information at each time of the vehicle data.

  The high-accuracy positioning unit 241 estimates the positions of a plurality of vehicles at each time with high accuracy based on the satellite information of the fixed-point observation points at each time and the information related to the fixed-point observation points stored in the data storage unit 232. . In the present embodiment, the position of each time of a plurality of vehicles is estimated with high accuracy using a general RTK (Realtime Kinematic) positioning method. The high-precision positioning unit 241 is an example of a positioning unit.

  The trajectory posture estimation unit 242 includes a temporary trajectory posture calculation unit 244, a feature point cost calculation unit 246, a pitch change cost calculation unit 248, a position change cost calculation unit 250, an orientation change cost calculation unit 252, and a Doppler cost calculation. A unit 254, a position cost calculation unit 255, and an optimum trajectory posture determination unit 56.

  When the convergence calculation is the first time, the temporary trajectory posture calculation unit 244 uses the trajectory at each time of the host vehicle, the posture at each time of the host vehicle, and the three-dimensional position of each feature point tracked by the feature point tracking unit 236. An appropriate value is set as an initial value.

  When the convergence calculation is performed for the second time or later, the temporary trajectory posture calculation unit 244 has a feature point cost calculation unit 246, a pitch change cost calculation unit 248, a position change cost calculation unit 250, an orientation change cost calculation unit 252, and a Doppler cost calculation unit. Based on each cost calculated by H.254 and a position cost calculation unit 255 described later, the trajectory of each vehicle at each time and the attitude of each vehicle at each time are estimated. The process when the convergence calculation is performed for the second time or later will be described after the process of each cost calculation unit is described.

The feature point cost calculation unit 246 associates feature points in different vehicles with each feature point tracked by the feature point tracking unit 236 for each of the plurality of vehicles.
Then, the feature point cost calculation unit 246, for each of the plurality of vehicles, the position of each feature point tracked by the feature point tracking unit 236 and the position of each vehicle obtained by the provisional trajectory posture calculation unit 244 at each time. The feature point cost representing the difference between the position of each feature point in each image at each time, obtained from the posture of each vehicle at each time, and the three-dimensional position of each feature point tracked by the feature point tracking unit 236 Is calculated.

  For each of the plurality of vehicles, the pitch change cost calculation unit 248 calculates the pitch angle of each vehicle calculated by the vehicle pitch angle calculation unit 238 and the posture of each vehicle obtained by the provisional trajectory posture calculation unit 244. A pitch change cost representing a difference from the difference between the pitch angles is calculated.

  The position change cost calculation unit 250 is obtained by the provisional trajectory posture calculation unit 244 and the change in the position of each vehicle obtained from the speed at each time of each vehicle stored in the data storage unit 232 for each of the plurality of vehicles. The position change cost representing the difference from the change in the position of each vehicle is calculated.

  The azimuth change cost calculation unit 252 changes the azimuth angle of each vehicle obtained from the angular velocities at each time stored in the data storage unit 232 for each of the plurality of vehicles, and each obtained by the temporary trajectory posture calculation unit 244. An azimuth change cost representing a difference from an azimuth change in the posture of the vehicle is calculated.

  For each of the plurality of vehicles, the Doppler cost calculation unit 254 includes the satellite information at each time stored in the data storage unit 232 and the azimuth among the postures at each time of each vehicle calculated by the provisional trajectory posture calculation unit 244. The Doppler cost representing the consistency with the corner is calculated.

  The position cost calculation unit 255, for each of the vehicles corresponding to the plurality of vehicle data stored in the data storage unit 232, the vehicle trajectory obtained from the position of the vehicle at each time estimated by the high-precision positioning unit 241; A position cost representing a difference from the vehicle trajectory calculated by the provisional trajectory posture calculation unit 244 is calculated.

  The position cost calculated by the position cost calculation unit 255 is tentatively calculated for each of a plurality of vehicles by the position of the vehicle at each time calculated by the high-precision positioning unit 241 and the temporary trajectory posture calculation unit 244. It is an index for evaluating the consistency with the trajectory of the vehicle, and is set so that the cost increases as the consistency decreases. For example, the cost can be set as shown in the following formula (10).

x _t ′ is the vehicle position at time t calculated by the high-precision positioning unit 241, x _t is the provisional value of the vehicle trajectory at time t calculated by the provisional trajectory posture calculation unit 244, and e _pos is the value of each observation value The residual, E _pos, is the sum of the residuals (cost).

  When the convergence calculation is performed for the second time or later, the temporary trajectory posture calculation unit 244 is a feature point cost calculation unit 46, a pitch change cost calculation unit 48, a position change cost calculation unit 50, an orientation change cost calculation unit 52, and a Doppler cost calculation unit. 54 and the cost calculated by the position cost calculation unit 255, the time trajectory of each vehicle and the attitude of each vehicle at each time are estimated.

  Specifically, the provisional trajectory posture calculation unit 244 determines the previous time position of each vehicle, the posture of each vehicle at each time, and the three-dimensional position of the feature point based on the result of each cost calculation unit. The correction amount δp from the value is calculated according to the following equation (11).

  In the above equation (11), n is a vehicle index, m is a feature point index, and t is a time index.

  The provisional trajectory posture calculation unit 44 estimates the position of each vehicle at each time and the posture of each vehicle at each time so as to minimize the cost calculated by each cost calculation unit according to the above equation (11).

  Then, the temporary trajectory posture calculation unit 44 uses the correction amount δp calculated above as the previous output value p, as shown in the following equation (12), at each time position of each vehicle and each time of each vehicle. The posture and the three-dimensional position of the feature point are updated. Thereby, the position of each time of the vehicle and the attitude of each time of the vehicle are estimated for each of the plurality of vehicles so as to minimize the cost calculated by each cost calculation unit.

  In the present embodiment, an example of the Newton method, which is a general and simple optimization method, is shown, but the more robust Levenberg-Marquardt method or the like may be used.

  14 and 15 are diagrams for explaining the effects of the second embodiment. As shown in FIG. 14, when estimating the trajectory of a single vehicle, the trajectory errors accumulate little by little as the vehicle travels. In this case, an error also occurs in the position of the feature point, and it is difficult to correct the locus.

  On the other hand, as shown in FIG. 15, in the second embodiment, since the information of a plurality of vehicles is collected, the error of the position of the feature point in the image is averaged by the number of vehicles, Compared with the estimation of the locus, the error of the position of the feature point in the image is reduced. Furthermore, as in the first embodiment, since the trajectory of each vehicle is optimized so that it matches the position of the feature point in the image, the estimation error of the trajectory of each vehicle is also reduced.

<Operation of State Estimation Device 210 according to Second Embodiment>

  Next, the operation of the state estimation device 210 according to the second embodiment will be described.

  At each time, each in-vehicle device 102 receives satellite information from each positioning satellite, each sensor detects vehicle data, and transmits the acquired vehicle data to the state estimation device 210. At each time, the fixed point observation device 110 receives satellite information from each positioning satellite and transmits the acquired fixed point observation data to the state estimation device 210. At each time, the state estimation device 210 receives the vehicle data transmitted from each in-vehicle device 102 and the fixed point observation data transmitted from the fixed point observation device 110, and the vehicle data and the fixed point observation data are received. When the data is stored in the data storage unit 232, the computer 220 executes a vehicle state estimation processing routine shown in FIG.

  In step S300, the vehicle data and fixed point observation data of each vehicle stored in the data storage unit 232 are acquired.

  In step S302, the feature point tracking unit 236 extracts feature points from each of the images at each time included in the vehicle data acquired in step S300 for each of the plurality of vehicles.

  In step S304, the feature point tracking unit 236 tracks each feature point in each of the images at each time based on the feature points extracted from each of the images at each time in step S302 for each of the plurality of vehicles. To do.

  In step S306, the vehicle pitch angle calculation unit 238, for each of the plurality of vehicles, based on the speed at each time and the acceleration at each time included in the vehicle data acquired in step S300, the above formula (1 ) To calculate the pitch angle at each time of the vehicle.

  In step S308, for each of the plurality of vehicles, the clock drift calculation unit 240 performs the calculation for each satellite according to the above equation (2) based on the satellite information at each time included in the vehicle data acquired in step S300. Calculate clock drift.

  In step S309, the high-accuracy positioning unit 241 estimates the position of each time of a plurality of vehicles with high accuracy based on the fixed point observation data acquired in step S300.

  In step S310, when the convergence calculation is the first time, the provisional trajectory posture calculation unit 44 is tracked by the trajectory at each time of the vehicle, the posture at each time of the vehicle, and the feature point tracking unit 36 for each of the plurality of vehicles. An appropriate value is set as an initial value as the three-dimensional position of each feature point. Alternatively, when the convergence calculation is performed for the second time or later, the temporary trajectory posture calculation unit 44 calculates the above formulas (8) to (9) for each of the plurality of vehicles based on the respective costs calculated in the previous step S312. Thus, the trajectory of each time of the vehicle and the attitude of each time of the vehicle are estimated.

  In step S312, each cost calculation unit, for each of the plurality of vehicles, trajectory of each time of the vehicle obtained in step S310, posture of each time of the vehicle, and each feature point tracked by the feature point tracking unit 36 Each cost is calculated based on the three-dimensional position. Step S312 is realized by the cost calculation processing routine shown in FIG.

<Cost calculation processing routine>

  In step S400, the feature point cost calculation unit 246 associates feature points in different vehicles with each feature point obtained in step S304 for each of the plurality of vehicles. Then, the feature point cost calculation unit 246, for each of the plurality of vehicles, according to the above equation (3), the position of each tracked feature point obtained in step S304 and the vehicle obtained in step S310. A feature point representing a difference between each feature point and a position in each time image obtained from the position at each time, the posture of each vehicle at each time, and the three-dimensional position of each feature point tracked in step S304. Calculate the cost.

  In step S402, the pitch change cost calculation unit 248, for each of a plurality of vehicles, according to the above equation (4), the pitch angle at each time of the vehicle calculated in step S306 and the vehicle obtained in step S310. A pitch change cost representing a difference from the difference between the pitch angle and the pitch angle is calculated.

  In step S404, the position change cost calculation unit 250, for each of the plurality of vehicles, according to the above equation (5), the change in the position of the vehicle obtained from the speed at each time of the vehicle acquired in step S300, and the above A position change cost representing a difference from the change in the position of the vehicle obtained in step S310 is calculated.

  In step S406, the azimuth change cost calculation unit 252 changes the azimuth angle of the vehicle obtained from the angular velocity at each time obtained in step S300 according to the above equation (6) for each of the plurality of vehicles, and the above step. An azimuth change cost representing a difference from a change in azimuth angle of the vehicle attitude obtained in S310 is calculated.

  In step S408, the Doppler cost calculation unit 254, for each of the plurality of vehicles, according to the equation (7), the satellite information of each time acquired in step S300 and each time of the vehicle obtained in step S310. The Doppler cost representing the consistency with the azimuth angle in the posture is calculated.

  In step S409, the position cost calculation unit 255, for each of the plurality of vehicles, according to the above equation (10), the trajectory of each time of the vehicle obtained from the position of each time of the vehicle estimated in the above step S309, and the above A position cost representing a difference from the vehicle trajectory obtained in step S310 is calculated.

  In step S410, each cost calculated in steps S400 to S409 is output as a result, and the cost calculation processing routine is terminated.

  Next, returning to step S114 of the vehicle state estimation processing routine shown in FIG. 16, the optimum trajectory posture determination unit 56 determines whether or not the result calculated in step S310 is an optimum value. If it is determined that the result calculated in step S310 is the optimum value, the process proceeds to step S316. On the other hand, if it is determined that the result calculated in step S310 is not the optimum value, the process returns to step S310.

  In step S316, the optimal trajectory posture determination unit 56 outputs the trajectory of the vehicle determined to be the optimum value and the posture of each time of the vehicle as a result, and the vehicle state estimation processing routine ends.

  As described above, according to the state estimation device according to the second embodiment, the vehicle is determined for each of the plurality of vehicles based on the satellite information at each time of the fixed point observation point and the information on the fixed point observation point. The position cost for each of a plurality of vehicles is calculated by calculating the position cost representing the difference between the trajectory of each vehicle time obtained from the position of each vehicle time and the trajectory of the vehicle. By estimating the trajectory of the vehicle and the attitude of the vehicle so as to minimize the cost including the vehicle trajectory, the trajectory of the vehicle and the attitude of the vehicle can be accurately estimated for each of the plurality of vehicles.

  Sensor information (gyro, wheel speed, GPS) measured by a plurality of vehicles is stored in a state estimation device as a server, and the trajectory of each vehicle and the posture and feature point position of each vehicle are optimized. Thus, since the averaging effect of each vehicle sensor error is obtained, the vehicle trajectory and the vehicle posture can be accurately estimated for each of the plurality of vehicles.

  In the first embodiment, the feature point cost calculator 46, the pitch change cost calculator 48, the position change cost calculator 50, the azimuth change cost calculator 52, and the Doppler cost calculator 54 are calculated. The case where the trajectory of the vehicle and the posture of the vehicle are estimated based on the cost will be described as an example. In the second embodiment, the feature point cost calculation unit 246, the pitch change cost calculation unit 248, the position change cost calculation unit 250, the case where the vehicle trajectory and the vehicle posture are estimated based on the respective costs calculated by the azimuth change cost calculation unit 252, the Doppler cost calculation unit 254, and the position cost calculation unit 255 has been described. It is not limited to. The vehicle trajectory and the vehicle attitude may be estimated so as to minimize at least one of the costs. For example, the trajectory of the vehicle and the posture of the vehicle may be estimated so as to minimize the feature point cost calculated by the feature point cost calculation unit and the Doppler cost calculated by the Doppler cost calculation unit.

  In the first embodiment, the state estimation device is mounted on a vehicle. However, the present invention is not limited to this, and the state estimation device is not limited to this. As a server, the trajectory and posture of the vehicle may be estimated based on vehicle data transmitted from the in-vehicle device of the vehicle.

  Moreover, although the said embodiment demonstrated the state estimation apparatus mounted in a vehicle, the mobile body in which the state estimation apparatus of this invention is mounted is not limited to a vehicle. For example, the state estimation device may be mounted on a robot, or the state estimation device may be configured as a portable terminal so that a pedestrian can carry it.

DESCRIPTION OF SYMBOLS 10,210 State estimation apparatus 12 Camera 14 Acceleration sensor 16 Speed sensor 18 Angular speed sensor 18 Speed sensor 19, 114 GPS apparatus 20, 104, 112, 220 Computer 22 Image acquisition part 24 Acceleration acquisition part 26 Vehicle speed acquisition part 28 Angular speed acquisition part 30 Satellite information acquisition unit 32, 232 Data storage unit 36, 236 Feature point tracking unit 38, 238 Vehicle pitch angle calculation unit 40, 240 Clock drift calculation unit 42, 242 Trajectory posture estimation unit 44, 244 Temporary trajectory posture calculation unit 46, 246 Feature point cost calculation unit 48, 248 Pitch change cost calculation unit 50, 250 Position change cost calculation unit 52, 252 Direction change cost calculation unit 54, 254 Doppler cost calculation unit 56 Optimal trajectory posture determination unit 100 Information processing system 102 In-vehicle device 106 Vehicle data transmission unit 11 0 fixed point observation device 116 satellite information acquisition unit 118 fixed point observation data transmission unit 150 network 230 vehicle data reception unit 231 fixed point observation data reception unit 241 high accuracy positioning unit 255 position cost calculation unit

Claims (9)

Acquisition means for acquiring satellite information at each time received by the receiving means mounted on the moving body and images of the surroundings of the moving body at each time imaged by the imaging means mounted on the moving body as moving body information When,
Feature point tracking means for extracting feature points from each of the images at each time acquired by the acquisition means and tracking each feature point in each of the images at each time;
The position of each feature point tracked by the feature point tracking means in each image at each time, the position of each time of the moving body, the posture of each time of the moving body, and the feature point tracking means The difference between each feature point and the position of each feature point in each of the images obtained from the tracked three-dimensional position of each feature point, the satellite information at each time obtained by the obtaining means, and the moving body State estimation means for estimating the position of each time of the moving body and the posture of each time of the moving body so as to minimize the cost representing the consistency with the posture of each time of
A state estimation apparatus including: A clock drift calculating means for calculating a clock drift for each satellite based on the satellite information at each time acquired by the acquiring means;
The Doppler shift at each time of each satellite included in the satellite information at each time acquired by the acquisition means, the clock drift for each satellite calculated by the clock drift calculation means, and each time of the mobile body Based on the attitude of the Doppler shift, the observed relative velocity between the moving object and each satellite, and the moving object and each satellite determined from the clock drift and the attitude of the moving object at each time. And a Doppler cost calculating means for calculating a Doppler cost representing a difference from the estimated value of the relative speed of
The state estimating means includes
The position of each time of the moving body and the posture of each time of the moving body are estimated so as to minimize the cost including the difference and the Doppler cost calculated by the Doppler cost calculating means. The state estimation apparatus according to 1. A pitch angle calculating unit;
The acquisition means includes a speed at each time detected by a speed sensor mounted on the mobile body, an acceleration at each time detected by an acceleration sensor mounted on the mobile body, and the mobile body mounted on the mobile body. Further acquiring the angular velocity at each time detected by the angular velocity sensor as the moving body information,
The pitch angle calculation means calculates the pitch angle at each time of the mobile body based on the speed at each time acquired by the acquisition means and the acceleration at each time,
A pitch change cost calculating means for calculating a pitch change cost representing a difference between a pitch angle at each time of the moving body calculated by the pitch angle calculating means and a pitch angle of the posture of the moving body;
A position change cost calculating means for calculating a position change cost representing a difference between a change in the position of the moving object obtained from the speed at each time acquired by the acquiring means and a change in the position of the moving object;
The azimuth change cost for calculating the azimuth change cost representing the difference between the change in the azimuth angle of the moving object obtained from the angular velocity at each time acquired by the acquisition means and the change in the azimuth angle of the posture of the moving object. Computing means;
The state estimating means includes
The difference, the Doppler cost calculated by the Doppler cost calculating means, the pitch change cost calculated by the pitch change cost calculating means, and the position change cost calculated by the position change cost calculating means, The position of each time of the said mobile body and the attitude | position of each time of the said mobile body are estimated so that the said cost including the said direction change cost calculated by the said direction change cost calculating means may be minimized. The state estimation apparatus described. The state estimating means includes
The state according to any one of claims 1 to 3, wherein estimation of a position of each time of the moving body and a posture of each time of the moving body is repeated until a predetermined convergence condition is satisfied. Estimating device. The acquisition means acquires the moving body information for each of the plurality of moving bodies,
The state estimation means, for each of the plurality of moving bodies, for each of the plurality of moving bodies, each time position of the moving body and each time of the moving body so as to minimize the sum of the costs for each of the plurality of moving bodies. The state estimation apparatus according to any one of claims 1 to 4, wherein the posture is estimated. Further including positioning means,
The acquisition means further acquires satellite information at each time point observed at a fixed point observation point and information on the fixed point observation point,
The positioning means, for each of a plurality of the mobile body, based on the satellite information of each time of the fixed point observation point acquired by the acquisition means and information on the fixed point observation point of each time of the mobile body Estimate the position,
The state estimating means includes
For each of the plurality of moving bodies, the total cost including the difference between the position of each time of the moving body estimated by the positioning means and the position of each time of the moving body is minimized. The state estimation device according to claim 5, wherein for each of the plurality of moving bodies, the position of the moving body at each time and the posture of the moving body are estimated. An information terminal mounted on a mobile object,
Satellite information at each time received by the receiving means mounted on the mobile body, images of the periphery of the mobile body at each time imaged by the imaging means mounted on the mobile body, mounted on the mobile body At least one of the speed at each time detected by the speed sensor, the acceleration at each time detected by the acceleration sensor mounted on the mobile body, and the angular speed at each time detected by the angular speed sensor mounted on the mobile body Information acquisition means for acquiring one as the mobile body information;
Communication means for transmitting the mobile object information acquired by the information acquisition means to the state estimation device according to any one of claims 1 to 6,
Information terminal including. The state estimation device according to any one of claims 1 to 6,
An information processing system including the information terminal according to claim 7. Computer
Acquisition means for acquiring satellite information at each time received by the receiving means mounted on the moving body and images of the surroundings of the moving body at each time imaged by the imaging means mounted on the moving body as moving body information ,
Feature points are extracted from each of the images at each time acquired by the acquisition means, and feature points tracking means for tracking each feature point in each of the images at each time, and tracked by the feature point tracking means The position of each feature point in each of the images at each time, the position of each time of the moving body, the posture of each time of the moving body, and the three-dimensional of each feature point tracked by the feature point tracking means The difference between each feature point obtained from the position and the position in each of the images at each time, and the consistency between the satellite information at each time acquired by the acquisition unit and the attitude of the mobile body at each time A program for functioning as state estimating means for estimating the position of each time of the moving body and the posture of each time of the moving body so as to minimize the cost representing

Images