WO2017168654A1 - 測定装置、測定方法、及び、プログラム - Google Patents

測定装置、測定方法、及び、プログラム Download PDF

Info

Publication number
WO2017168654A1
WO2017168654A1 PCT/JP2016/060509 JP2016060509W WO2017168654A1 WO 2017168654 A1 WO2017168654 A1 WO 2017168654A1 JP 2016060509 W JP2016060509 W JP 2016060509W WO 2017168654 A1 WO2017168654 A1 WO 2017168654A1
Authority
WO
WIPO (PCT)
Prior art keywords
moving body
feature
time
distance
viewed
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2016/060509
Other languages
English (en)
French (fr)
Inventor
加藤　正浩
諒子 新原
岩井　智昭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Pioneer Corp
Original Assignee
Pioneer Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Pioneer Corp filed Critical Pioneer Corp
Priority to PCT/JP2016/060509 priority Critical patent/WO2017168654A1/ja
Priority to JP2018507956A priority patent/JP6707627B2/ja
Publication of WO2017168654A1 publication Critical patent/WO2017168654A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/105—Speed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/114—Yaw movement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Definitions

  • the present invention relates to a technique for detecting a change in the traveling direction of a moving object.
  • Patent Document 1 describes a method of recognizing a fixed object existing in front of a moving body and correcting a deviation amount of a yaw rate sensor based on a movement trajectory in which the fixed object moves relative to the host vehicle. ing.
  • the invention according to claim 1 is a measuring apparatus, wherein the distance from the moving body to at least two positions on one feature at each of the first time and the second time and the distance from the moving body.
  • a first acquisition unit that acquires an angle formed by the direction of the feature and the traveling direction of the moving body, and the moving body from the first time to the second time based on the acquisition result of the first acquisition unit.
  • a calculating unit that calculates at least one of the moving distance and the amount of change in the traveling direction of the moving body.
  • the invention according to claim 10 is a measuring method executed by the measuring device, wherein each distance from the moving body to at least two positions on one feature at each of the first time and the second time and Based on the first acquisition step of acquiring the angles formed by the direction of the feature viewed from the moving body and the traveling direction of the moving body, respectively, and the second time from the first time based on the acquisition result of the first acquisition unit. And a calculating step of calculating at least one of a moving distance of the moving body up to a time and an amount of change in the traveling direction of the moving body.
  • the invention according to claim 11 is a program executed by a measurement apparatus including a computer, and each of the first time and the second time from the moving body to at least two positions on one feature.
  • a first acquisition unit that acquires a distance and an angle formed by the direction of the feature viewed from the moving body and the traveling direction of the moving body, respectively, and the first time from the first time based on the acquisition result of the first acquisition unit
  • the computer is caused to function as a calculation unit that calculates at least one of a moving distance of the moving body and a moving direction change amount of the moving body up to two hours.
  • a method of calculating the movement distance and the yaw angle change amount using one feature will be described.
  • It is a block diagram which shows the structure of the coefficient update apparatus by an Example. It is a flowchart of a coefficient update process. It is a figure explaining the calculation method of the beam angle by 1st Example. It is another figure explaining the calculation method of the beam angle by 1st Example. It is another figure explaining the calculation method of the beam angle by 1st Example. It is another figure explaining the calculation method of the beam angle by 1st Example. It is another figure explaining the calculation method of the beam angle by 1st Example. It is a flowchart of the calculation process of the beam angle by 1st Example. It is a figure explaining the calculation method of the beam angle by 2nd Example. It is a flowchart of the calculation process of the beam angle by 2nd Example.
  • the measuring device includes a distance from the moving body to at least two positions on one feature at each of the first time and the second time, and the feature viewed from the moving body.
  • a first acquisition unit that acquires an angle formed by the direction of the moving body and the traveling direction of the mobile body, and the movement of the mobile body from the first time to the second time based on the acquisition result of the first acquisition unit
  • a calculating unit that calculates at least one of the distance and the amount of change in the traveling direction of the moving body.
  • the measuring apparatus includes the distance from the moving body to at least two positions on one feature at each of the first time and the second time, the direction of the feature viewed from the moving body, and the position of the moving body.
  • Each angle formed by the traveling direction is acquired, and at least one of the moving distance of the moving body from the first time to the second time and the amount of change in the moving direction of the moving body is calculated based on the obtained result.
  • at least one of the moving distance of the moving body and the amount of change in the traveling direction of the moving body can be calculated using any feature that can be measured from the moving body.
  • One aspect of the measurement apparatus further includes a second acquisition unit that acquires the coordinates of the at least two positions, and the calculation unit includes: (i) from the moving body at the first time and the second time. A normal to the surface of the feature and a direction of one location on the feature as seen from the moving body, calculated based on the respective distances to the at least two locations and the coordinates of the at least two locations; And (ii) at least one of the moving distance and the amount of change is calculated based on the angle formed by (ii) the direction of the one position viewed from the moving body and the traveling direction of the moving body.
  • the first acquisition unit may be configured from a distance from the moving body to at least three positions of the one feature and a direction of the at least three positions viewed from the moving body.
  • a first calculator that calculates the coordinates of the at least three positions, and calculates the coordinates of the center of gravity of the surface of the feature based on the coordinates of the at least three positions;
  • a first direction determining unit for determining a direction, a formula of a plane defining the surface of the feature based on the coordinates of the at least three positions, and a method for the surface of the feature based on the formula of the plane
  • a second direction determining unit that determines a direction of the line; and a second calculating unit that calculates an angle formed by the direction of the center of gravity and the direction of the normal line, and the calculating unit includes the first time and The movement at each second time From the distance from the center of gravity to the center of gravity, the angle formed by the direction of the center of gravity viewed from the moving body and the traveling direction of the moving body, and
  • the first acquisition unit is configured to determine a distance from the moving body to both end points of the first feature, and angles formed by directions of the both end points as viewed from the moving body.
  • the calculation unit includes a distance from the moving body to the midpoint at each of the first time and the second time, an angle formed by a direction of the midpoint viewed from the mobile body and a traveling direction of the mobile body, and Based on the angle formed by the direction of the midpoint and the direction of the normal Calculating at least one of the moving distance and the amount of change.
  • the first acquisition unit includes a distance from the moving body to both end points of the one feature, and angles formed by directions of the both end points as viewed from the moving body.
  • a first calculation unit for determining the coordinates of the midpoint of the first feature based on a distance to the two end points and an angle formed by the directions of the two end points as viewed from the moving body.
  • the moving body position which is the position of the moving body when one of a plurality of positions on the feature is measured
  • a coordinate conversion unit that converts coordinates of a position measured at the moving body position other than the reference position into coordinates in a coordinate system of the reference position is provided.
  • the amount of change in the traveling direction of the moving body can be calculated with high accuracy by correcting the position coordinates in consideration of the movement of the moving body within the time when the feature is being measured.
  • the calculation unit calculates the movement distance
  • the measurement apparatus is mounted on the moving body based on the movement distance per unit time calculated based on the movement distance.
  • the vehicle speed sensor further includes a first correction unit that corrects the first information for calculating the speed of the moving body from the output result at the second time.
  • the vehicle speed sensor of the moving body can be appropriately corrected based on the calculated moving distance.
  • the calculation unit calculates the movement distance
  • the measurement apparatus is mounted on the moving body based on the movement distance per unit time calculated based on the movement distance.
  • a second correction unit that corrects second information for calculating the speed of the moving body from the output result of the acceleration sensor at the second time.
  • the acceleration sensor of the moving body can be appropriately corrected based on the calculated moving distance.
  • the calculation unit calculates the amount of change, and the measuring device is calculated based on the amount of change in the traveling direction per unit time, which is calculated based on the amount of change.
  • the apparatus further includes a third correction unit that corrects third information for calculating a yaw rate of the moving body from an output result of the angular velocity sensor mounted on the moving body at the second time.
  • the angular velocity sensor of the moving body can be appropriately corrected based on the calculated amount of change in the traveling direction.
  • Another preferred embodiment of the present invention is a measurement method executed by a measurement apparatus, each of a first time and a second time from a moving object to at least two positions on one feature.
  • the first acquisition step of acquiring the distance and the angle formed by the direction of the feature viewed from the moving body and the traveling direction of the moving body, and the acquisition result of the first acquisition unit the first time from the first time
  • a program executed by a measuring device including a computer is each from a moving object to at least two positions on one feature at each of a first time and a second time.
  • a first acquisition unit that acquires a distance and an angle formed by the direction of the feature viewed from the moving body and the traveling direction of the moving body, respectively, and the first time from the first time based on the acquisition result of the first acquisition unit
  • the computer is caused to function as a calculation unit that calculates at least one of a moving distance of the moving body up to two hours and a change amount of the moving direction of the moving body. Accordingly, at least one of the moving distance of the moving body and the amount of change in the traveling direction of the moving body can be calculated using any feature that can be measured from the moving body.
  • This program can be stored in a storage medium and used.
  • the vehicle body speed of the vehicle is obtained by multiplying the wheel rotation speed based on the axle rotation pulse output from the vehicle speed sensor by the conversion coefficient.
  • the vehicle body speed can be calculated based on the detected acceleration output from the acceleration sensor, the sensitivity coefficient of the acceleration sensor, and the offset coefficient.
  • the yaw angle of the vehicle can be calculated based on the angular velocity output from the gyro sensor and the sensitivity coefficient and offset coefficient of the gyro sensor.
  • the conversion coefficient of the vehicle speed sensor, the sensitivity and offset of the acceleration sensor, and the sensitivity and offset of the gyro sensor have characteristics that vary depending on temperature, vibration, and the like. Therefore, if the vehicle speed and yaw angle are calculated in a state including these errors, the errors accumulate, and the calculated vehicle speed and yaw angle deviate greatly from the actual values. Therefore, in order to estimate the vehicle body speed and the yaw angle with high accuracy, it is necessary to continuously correct the conversion coefficient of the vehicle speed sensor, the sensitivity and offset of the acceleration sensor, and the sensitivity and offset of the gyro sensor.
  • a conversion coefficient for obtaining a vehicle body speed from a vehicle speed sensor output using a Doppler speed based on GPS information, and sensitivity and offset correction of an acceleration sensor are corrected, and a gyroscope is used using a direction change amount of GPS information.
  • Sensor sensitivity and offset are corrected. For this reason, the accuracy of correction deteriorates in an environment such as an urban area where the GPS reception state is poor. Moreover, it cannot correct
  • the coefficient updating apparatus calculates the moving distance and the yaw angle change amount of the vehicle based on the measurement result of one feature by the external sensor without using the GPS information, and obtains the vehicle body speed from the vehicle speed sensor. Conversion coefficient, acceleration sensor sensitivity and offset, and gyro sensor sensitivity and offset are corrected. Further, the coefficient updating device can calculate the vehicle body speed V by dividing the moving distance by a predetermined time.
  • An example of the external sensor is LiDAR (Light Detection And Ranging).
  • the travel distance ⁇ L of the vehicle is obtained using the feature measurement value obtained by the external sensor, and the vehicle body speed is calculated by dividing it by the time interval ⁇ t.
  • the obtained measured vehicle speed is the true vehicle speed V (t)
  • the measured vehicle speed V (t) and the wheel rotational speed ⁇ p (t) obtained from the vehicle speed sensor are used to calculate
  • the conversion coefficient K can be estimated.
  • the conversion coefficient K corresponds to the first information of the present invention.
  • the travel distance ⁇ L of the vehicle is obtained using the feature measurement value by the external sensor, and the measured vehicle body speed V (t) is calculated by dividing the travel distance ⁇ t by the time interval ⁇ t.
  • the measured vehicle body acceleration is calculated using the vehicle body speed V (t ⁇ 1) and the time interval ⁇ t.
  • the obtained measured vehicle body acceleration is regarded as the true vehicle body acceleration V_dot (t) .
  • the recursive least square method it is possible to obtain the sensitivity coefficient A and offset coefficients B. That is, if the moving distance ⁇ L of the vehicle can be obtained using an external sensor, the sensitivity and offset of the acceleration sensor can be estimated.
  • the sensitivity coefficient A and the offset coefficient B correspond to the second information of the present invention.
  • yaw angle change amount the change amount of the yaw angle of the vehicle (hereinafter referred to as “yaw angle change amount”) ⁇ L can be obtained from the feature measurement value by the external sensor, the following is obtained by dividing the change amount by the time interval ⁇ t.
  • the angular velocity is calculated as follows.
  • the angular velocity (also referred to as “measured yaw rate”) obtained from the feature measurement value by the external sensor is regarded as the true angular velocity, it can be expressed as follows.
  • the recursive least square method it is possible to determine the sensitivity coefficients C and the offset coefficient D.
  • the sensitivity coefficient C and the offset coefficient D correspond to the third information of the present invention.
  • FIG. 1 is a diagram for explaining a method of calculating a movement distance and a yaw angle change amount using one feature.
  • a road sign having a flat plate shape is used as a feature.
  • LiDAR is used as an external sensor.
  • the vehicle travel distance ⁇ L and the yaw angle change amount ⁇ are calculated by the following procedure. (1) at a point P 1 of the current vehicle, it is measured by LiDAR the distance and direction to a predetermined feature A.
  • the moving distance ⁇ L is obtained by using the cosine theorem for triangle composed of feature A and the vehicle position P 1 and the vehicle position P 2. Further, the yaw angle change amount ⁇ is obtained by subtracting ⁇ from ⁇ A2 , and ⁇ is obtained by adding ⁇ A1 and ⁇ . Therefore, using the distances r A1 , r A2 , angles ⁇ A1 , ⁇ A2 , and ⁇ , the movement distance ⁇ L and the yaw angle change amount ⁇ are calculated by the following equations.
  • the yaw angle ⁇ can be calculated by integrating the yaw angle change amount ⁇
  • the yaw rate ⁇ _dot (t) can be calculated by dividing the yaw angle change quantity ⁇ by the predetermined time ⁇ t.
  • the vehicle body speed V can be calculated by dividing the moving distance ⁇ L by the predetermined time ⁇ t
  • the moving distance L can also be calculated by integrating the moving distance ⁇ L.
  • FIG. 2 is a block diagram illustrating a configuration of the coefficient updating apparatus 10 according to the embodiment.
  • the coefficient update device 10 includes a vehicle speed sensor 7, an acceleration sensor 8, a gyro sensor 9, an external sensor (LiDAR) 12, a feature measurement unit 13, a beam angle calculation unit 14, and a coefficient calculation.
  • the feature measurement unit 13, the beam angle calculation unit 14, the coefficient calculation unit 15, and the movement distance / yaw angle change amount calculation unit 16 are realized by a computer such as a CPU executing a program prepared in advance. be able to.
  • the vehicle speed sensor 7 supplies the wheel rotation speed ⁇ p (t) measured based on the axle rotation pulse to the coefficient calculation unit 15.
  • the acceleration sensor 8 detects the acceleration ⁇ ac (t) of the vehicle and supplies it to the coefficient calculation unit 15. Further, the gyro sensor 9 supplies the detected angular velocity ⁇ g (t) to the coefficient calculation unit 15.
  • the feature measurement unit 13 measures the distance r to the feature and the angle ⁇ with the feature based on the output of the external sensor 12 and supplies the measured distance to the movement distance / yaw angle change calculation unit 16. In addition, the feature measurement unit 13 converts the distance r and the angle ⁇ into orthogonal coordinates (x, y, z) in which the traveling direction is x, the horizontal direction is y, and the height direction is z. The coordinates are supplied to the beam angle calculation unit 14.
  • the beam angle calculation unit 14 is also referred to as an angle ⁇ (hereinafter referred to as “beam angle”) formed by the beam from the external sensor 12 and the normal vector of the planar plate-like feature based on the output of the external sensor 12. ) And is supplied to the movement distance / yaw angle change amount calculation unit 16.
  • the movement distance / yaw angle change amount calculation unit 16 uses the distance r to the feature at different times, the angle ⁇ with the feature, and the beam angle ⁇ according to the above formulas (11) and (12).
  • the movement distance ⁇ L and the yaw angle change amount ⁇ are calculated and supplied to the coefficient calculation unit 15.
  • the coefficient calculation unit 15 first calculates the vehicle body speed V (t) by dividing the movement distance ⁇ L by the predetermined time ⁇ t. Next, the coefficient calculation unit 15 calculates the conversion coefficient K of the vehicle speed sensor 7 according to the equation (1) based on the vehicle body speed V (t) and the wheel rotation speed ⁇ p (t) output from the vehicle speed sensor 7. ,Update. Further, the coefficient calculation unit 15 is based on the vehicle body speed V (t) , the vehicle body speed V (t ⁇ 1) one time before, the time interval ⁇ t, and the acceleration ⁇ ac (t) output from the acceleration sensor 8. The sensitivity coefficient A and the offset B of the acceleration sensor 8 are calculated and updated by the equations (2) to (3).
  • the coefficient calculation unit 15 uses the equation (7) based on the angular velocity ⁇ g (t) supplied from the gyro sensor 9 and the yaw angle change amount ⁇ supplied from the movement distance / yaw angle change amount calculation unit 16.
  • the sensitivity coefficient C and the offset coefficient D of the gyro sensor are calculated and updated by (8) to (8).
  • the feature measurement unit 13 is an example of the first acquisition unit of the present invention
  • the beam angle calculation unit 14 includes the second acquisition unit, the first to second direction determination units, and the first to first of the present invention.
  • 6 is an example of the first and second midpoint determination units
  • the coefficient calculation unit 15 is an example of the first to third correction units of the present invention
  • the movement distance / yaw angle change amount calculation unit 16 is the main calculation unit. It is an example of the calculation part of invention.
  • the yaw angle change amount corresponds to the change amount in the traveling direction in the present invention.
  • FIG. 3 is a flowchart of the coefficient update process.
  • the coefficient updating apparatus 10 measures one feature A by the external sensor 12, and acquires measurement data including the distance r to the feature and the angle ⁇ with the feature (step S10).
  • the coefficient update device 10 calculates the beam angle ⁇ using the measurement data (step S11). The method for calculating the beam angle ⁇ will be described in detail later.
  • the coefficient updating apparatus 10 determines whether or not there is measurement data of the feature A one hour before (step S12). When there is no measurement data one hour before (step S12: NO), the coefficient update device 10 increments the time (step S13), and the process returns to step S10.
  • step S12 when there is measurement data one hour ago (step S12: YES), the coefficient updating apparatus 10 determines the distance r A1 , angle ⁇ A1 , beam angle ⁇ 1 of the feature A one hour ago, and the ground at the current time. Using the object distance r A2 , the angle ⁇ A2 , and the beam angle ⁇ 2 , the yaw angle change amount ⁇ and the movement distance ⁇ L are calculated from the equations (11) and (12) (step S14).
  • the coefficient updating apparatus 10 calculates the measured vehicle body speed V (t) from the movement distance ⁇ L and the time interval ⁇ t, and calculates the true angular velocity (measurement) using the equation (7) from the yaw angle change amount ⁇ and the time interval ⁇ t. Yaw rate) ⁇ _dot (t) is calculated (step S15).
  • the coefficient updating device 10 calculates and updates the conversion coefficient K from the obtained measured vehicle body speed V (t) and the output ⁇ p (t) of the vehicle speed sensor according to the equation (1), and the obtained measured vehicle body. From the speed V (t) , the measured vehicle body speed V (t-1) one time ago, the time interval ⁇ t, and the output ⁇ ac (t ) of the acceleration sensor, the sensitivity coefficient A and the offset coefficient B of the acceleration sensor are obtained by the equation The sensitivity coefficient C and the offset coefficient D are calculated from the measured yaw rate ⁇ _dot (t) and the output ⁇ g (t) of the gyro sensor ⁇ g (t) and updated (step S16). .
  • the first embodiment is a method for accurately obtaining the beam angle ⁇ .
  • FIG. 4B shows a state where a circular road sign is measured by LiDAR as an example of the target feature.
  • a plurality of measurement points are formed on a road sign by a plurality of LiDAR beams.
  • the beam angle calculation unit 14 acquires measurement values of a plurality of beams that hit the target feature.
  • the beam angle calculation unit 14 determines that the measurement point having the reflectance equal to or higher than the predetermined threshold is the measurement point on the road sign. To do. Thereby, the beam angle calculation unit 14 acquires a plurality of measurement points existing on the road sign.
  • the white circles in FIG. 4B image a beam that is not detected below the threshold.
  • the beam angle calculation unit 14 coordinates (x 1 , y 1 , z 1 ), (x 2 , y 2 , z 2 ), (x 2 , y 2 , z 2 ),.
  • the coordinates G (x g , y g , z g ) of the barycentric point are obtained from x n , y n , z n ).
  • the beam angle calculation unit 14 uses the coordinates G (x g , y g , z g ) of the centroid point to move the centroid point (x g , y g , Find direction vectors up to z g ).
  • the beam angle calculation unit 14 calculates the coordinates (x 1 , y 1 , z 1 ), (x 2 , y 2 , z 2 ), ... (x n , y n , z n ) of each measurement point, By using the method of least squares, a plane expression that defines the surface of the road sign is generated, and a normal vector of the plane is obtained. Then, the beam angle calculation unit 14 obtains a beam angle ⁇ , which is an angle formed by the two vectors, from the plane normal vector and the xy component of the direction vector from the vehicle to the center of gravity.
  • the beam angle calculation unit 14 calculates a plane equation from a plurality of measurement points (x k , y k , z k ) existing on the road sign.
  • Equation (16) ( ⁇ , ⁇ , ⁇ ) that minimizes Equation (16) is obtained by the method of least squares.
  • the ( ⁇ , ⁇ , ⁇ ) that minimizes the equation (16) becomes 0 when the equation (16) is partially differentiated.
  • the normal vector N V or less (see FIG. 6 (A)).
  • the direction vector DV is as follows (see FIG. 6B).
  • the direction vector DV is as follows (see FIG. 6C).
  • the beam angle ⁇ is obtained from the following equation.
  • the first embodiment obtains a plane equation by the least square method, as shown in FIG. 7B, the normal vector is obtained even if the feature is somewhat curved. be able to. Further, as shown in FIG. 7C, even if there is some unevenness on the surface of the feature, a plane equation by the least square method can be created, and a normal vector can be obtained.
  • FIG. 8 is a flowchart of beam angle calculation processing according to the first embodiment. This process corresponds to step S11 of the coefficient update process shown in FIG.
  • the beam angle calculation unit 14 obtains the center of gravity G (x g , y g , z g ) from n pieces of data (x k , y k , z k ) obtained by measuring the feature A with LiDAR.
  • the direction vector D V (x g , y g , z g ) from the vehicle to the center of gravity G is calculated.
  • the beam angle calculation unit 14 obtains the distance r A [t] and the angle ⁇ A [t] from the vehicle to the center of gravity G (step S21).
  • the second embodiment is a method for simply obtaining the beam angle ⁇ .
  • FIG. 9 shows a calculation method of the beam angle ⁇ according to the second embodiment.
  • the position of the vehicle is the origin O
  • both ends of the planar plate-like feature are points C and E.
  • the beam angle calculation unit 14 extracts the distance to the end points C and E, that is, the OC length and the OE length, and the angle ⁇ between them from the measurement data of the feature.
  • the beam angle calculation unit 14 obtains the CE length by the cosine theorem for the triangle ⁇ OCE as follows.
  • the beam angle calculation unit 14 obtains the angle of ⁇ OCE using the cosine theorem using the obtained CE length as follows.
  • the beam angle calculation unit 14 obtains the angle of the CDO using the obtained OD length.
  • the beam angle calculation unit 14 subtracts the angle of the CDO from 90 degrees to obtain the angle ⁇ .
  • FIG. 10 is a flowchart of beam angle calculation processing according to the second embodiment. This process corresponds to step S11 of the coefficient update process shown in FIG.
  • the beam angle calculation unit 14 selects the two beams that are farthest in the horizontal direction from among the plurality of beams that have detected the feature A, obtains both end points C and E of the feature, and both end points C and E Distances OC, OE and an angle ⁇ therebetween are acquired (step S31).
  • the beam angle calculation unit 14 obtains the midpoint D of the end points C and E, and obtains the distance (OD length) from the vehicle to the midpoint D (step S32).
  • the beam angle calculation unit 14 obtains an angle ( ⁇ CDO) formed by the straight line OD connecting the vehicle and the midpoint D and the plane of the feature (step S33).
  • the beam angle calculation unit 14 subtracts the obtained angle ( ⁇ CDO) from 90 degrees to calculate the angle ⁇ (step S34).
  • the calculation process of the beam angle ⁇ ends.
  • the third embodiment is a method of approximately obtaining the beam angle ⁇ .
  • FIG. 11 shows a calculation method of the beam angle ⁇ according to the third embodiment.
  • the position of the vehicle is the origin O
  • both ends of the planar plate-like feature are points C and E.
  • the beam angle calculation unit 14 first extracts the distance to the end points C and E, that is, the OC length and the OE length, and the angle ⁇ between them from the measurement data of the feature.
  • triangle ⁇ OCF is an isosceles triangle
  • the straight line OC and the straight line OE can be regarded as substantially parallel. Therefore, when the triangle ⁇ CFE is regarded as a right triangle whose ⁇ CFE is a right angle, the angle ⁇ is obtained as follows.
  • FIG. 12 is a flowchart of a beam angle calculation process according to the third embodiment. This process corresponds to step S11 of the coefficient update process shown in FIG.
  • the beam angle calculation unit 14 selects the two beams that are farthest in the horizontal direction from the plurality of beams that have detected the feature A, obtains the end points C and E of the feature, and reaches the end points C and E. Distances OC and OE and an angle ⁇ therebetween are acquired (step S41).
  • the beam angle calculation unit 14 provides the point F on the straight line OE from the vehicle to the end point E, and calculates the FE length from the point F to the end point E by the above-described equation (29) (step S42).
  • the beam angle calculation unit 14 calculates the CF length by the equation (30) using the OC length and the angle ⁇ (step S43). And the beam angle calculation part 14 calculates angle (phi) by Formula (31) using FE length and CF length (step S44). Thus, the calculation process of the beam angle ⁇ ends.
  • FIG. 13A shows how the vehicle moves while measuring one feature with LiDAR.
  • the vehicle When measuring the left edge of the feature is the vehicle is in the position P 1, when measuring the right edge of a feature is, the vehicle is moving to the position P n.
  • FIG. 13B is a diagram in which the coordinates (r, ⁇ ) of points on the feature measured by LiDAR in the example of FIG. 13A are converted into (x, y) coordinates with the vehicle position as the origin. Indicates. As the vehicle moves, the distance and direction to the feature changes, and the plane of the feature becomes distorted. As a result, the center (or centroid) coordinates of the feature and the normal vector are shifted.
  • the scan period of the horizontal scan type LiDAR is T [s]
  • the speed at which the beam moves is r ⁇ [m / s]. If there is a road sign with a width W [m], the time ⁇ T for scanning the road sign is as follows.
  • the road sign measurement time estimated in the above consideration is about several ms, it can be considered that there is almost no change in vehicle speed or yaw rate within that time. Therefore, using the vehicle speed and yaw rate values calculated at the previous time, calculate the direction change and position change of the vehicle during the current feature measurement, and use the changes to measure each measurement point. What is necessary is just to obtain
  • the feature coordinates measured by P k points converted into values using a coordinate system P 1 point.
  • coordinates are rotated to a coordinate axis indicated by a broken line at a point P k , and then a difference ( ⁇ x k , ⁇ y k ) between positions in the x direction and the y direction is added.
  • ⁇ x k and ⁇ y k can be expressed as follows.
  • FIG. 15 is a flowchart of the coordinate calculation processing of the measurement points of the feature in consideration of the movement of the vehicle within the feature measurement time. This process is performed by the above-described feature measuring unit 13.
  • the feature measuring unit 13 reads the vehicle speed V (t-1) and yaw rate ⁇ _dot (t-1) measured (or estimated) at the previous time (step S50), and has a planar road sign.
  • the measurement data of n points for the feature A as a target is acquired (step S51).
  • feature measuring unit 13 sets the data P 1 at the left end of the measurement data of the feature in the position and orientation of the reference (step S52).
  • the feature measuring unit 13 sets “1” to the coefficient k (step S53), and determines whether the coefficient k is equal to or less than the number of measurement data n (step S54). If the coefficient k is less than or equal to the measurement data number n (step S54: YES), feature measuring unit 13 obtains the time difference Delta] t k from point P 1 to the point P k (step S55).
  • the feature measuring unit 13 obtains an angle change ⁇ k from the point P 1 to the point P k using the above-described equation (33) (step S56), and further calculates equations (34) and (35). Using this, the difference ( ⁇ x k , ⁇ y k ) between the points P 1 and P k is obtained (step S57). Then, the feature measuring unit 13 calculates the coordinates of the feature at the point P k in the coordinate system with reference to the point P 1 using the equation (36) (step S58).
  • the feature measuring unit 13 increments the coefficient k (step S59) and returns to step S54. Then, when the coefficient k exceeds the number of measurement data n (step S54: NO), the process ends.
  • the yaw angle ⁇ can be calculated by integrating the yaw angle change amount ⁇
  • the yaw rate ⁇ _dot (t) can be calculated by dividing the yaw angle change quantity ⁇ by the predetermined time ⁇ t.
  • the vehicle body speed V can be calculated by dividing the moving distance ⁇ L by the predetermined time ⁇ t
  • the moving distance L can also be calculated by integrating the moving distance ⁇ L.
  • the present invention can be used for an apparatus mounted on a moving body.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Measurement Of Distances Traversed On The Ground (AREA)
  • Navigation (AREA)

Abstract

測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得し、その取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離と進行方向の変化量を算出する。

Description

測定装置、測定方法、及び、プログラム
　本発明は、移動体の進行方向の変化を検出する技術に関する。
　移動体の前方に存在する物体を認識して、移動体のヨーレートを補正する手法が知られている。特許文献１は、移動体の前方に存在する固定物を認識し、その固定物が自車両に対して相対的に移動する移動軌跡に基づいて、ヨーレートセンサのずれ量を補正する手法を記載している。
特開２０１２－６６７７７号公報
　特許文献１に記載の手法では、上記の移動軌跡の他に、車速、旋回半径からのずれ量、固定物が横方向に相対的に移動する横移動量など、推定ヨーレートの算出に用いるパラメータが多く、これらのパラメータを取得するセンサのずれ量も考慮しなくてはならない。
　本発明が解決しようとする課題としては、上記のものが例として挙げられる。本発明は、１つのセンサで移動体の移動量とヨー角を算出し、移動距離と進行方向の変化量を得ることが可能な測定装置を提供することを目的とする。
　請求項１に記載の発明は、測定装置であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、を備えることを特徴とする。
　請求項１０に記載の発明は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、を備えることを特徴とする。
　請求項１１に記載の発明は、コンピュータを備える測定装置によって実行されるプログラムであって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、として前記コンピュータを機能させることを特徴とする。
１つの地物を用いて移動距離とヨー角変化量を算出する方法を示す。 実施例による係数更新装置の構成を示すブロック図である。 係数更新処理のフローチャートである。 第１実施例によるビーム角度の算出方法を説明する図である。 第１実施例によるビーム角度の算出方法を説明する他の図である。 第１実施例によるビーム角度の算出方法を説明する他の図である。 第１実施例によるビーム角度の算出方法を説明する他の図である。 第１実施例によるビーム角度の算出処理のフローチャートである。 第２実施例によるビーム角度の算出方法を説明する図である。 第２実施例によるビーム角度の算出処理のフローチャートである。 第３実施例によるビーム角度の算出方法を説明する図である。 第３実施例によるビーム角度の算出処理のフローチャートである。 地物の計測中における車両の移動による影響を示す。 車両の移動を考慮した地物の計測点の座標計算方法を示す。 地物の計測点の座標計算のフローチャートである。
　本発明の好適な実施形態では、測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、を備える。
　上記の測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得し、その取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。
　上記の測定装置の一態様は、前記少なくとも２つの位置の座標を取得する第２取得部を更に備え、前記算出部は、前記第１時刻及び前記第２時刻における、（ｉ）前記移動体から前記少なくとも２つの位置までのそれぞれの距離及び前記少なくとも２つの位置の座標に基づいて算出される、前記地物の表面に対する法線と前記移動体からみた前記地物上の１つの位置の方向とがなす角度、並びに（ｉｉ）前記移動体からみた前記１つの位置の方向と前記移動体の進行方向とがなす角度、に基づき前記移動距離及び前記変化量のうち少なくとも一方を算出する。
　上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の少なくとも３つの位置までの距離及び前記移動体から見た前記少なくとも３つの位置の方向から、前記少なくとも３つの位置の座標を計算する第１計算部と、前記少なくとも３つの位置の座標に基づいて前記地物の表面の重心点の座標を計算し、前記移動体からみた前記重心点の方向を決定する第１方向決定部と、前記少なくとも３つの位置の座標に基づいて前記地物の表面を規定する平面の式を計算し、前記平面の式に基づいて前記地物の表面に対する法線の方向を決定する第２方向決定部と、前記重心点の方向と前記法線の方向とのなす角度を計算する第２計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記重心点までの距離、前記移動体からみた前記重心点の方向と前記移動体の進行方向とがなす角度、及び、前記重心点の方向と前記法線の方向とのなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。
　上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角度を計算する第３計算部と、前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第１中点決定部と、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第４計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。
　上記の測定装置の他の一態様では、前記第１取得部は、前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向のなす角度を計算する第５計算部と、前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第２中点決定部と、前記移動体からみた前記両端点それぞれの方向が平行であるとみなして、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第６計算部と、を備え、前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出する。
　上記の測定装置の他の一態様は、前記地物上の複数の位置のうちの１つを計測したときの前記移動体の位置である移動体位置を基準位置とし、前記複数の位置のうち前記基準位置以外の前記移動体位置で計測した位置の座標を、前記基準位置の座標系における座標に変換する座標変換部を備える。この態様では、地物を計測している時間内における移動体の移動を考慮して位置座標を補正することにより、移動体の進行方向の変化量を高精度で算出することができる。
　上記の測定装置の他の一態様では、前記算出部は前記移動距離を算出し、前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される車速センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第１情報を補正する第１補正部を更に備える。この態様では、算出される移動距離に基づいて、移動体の車速センサを適切に補正することができる。
　上記の測定装置の他の一態様では、前記算出部は前記移動距離を算出し、前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される加速度センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第２情報を補正する第２補正部を更に備える。この態様では、算出される移動距離に基づいて、移動体の加速度センサを適切に補正することができる。
　上記の測定装置の他の一態様では、前記算出部は前記変化量を算出し、前記測定装置は、前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第３情報を補正する第３補正部を更に備える。この態様では、算出される進行方向の変化量に基づいて、移動体の角速度センサを適切に補正することができる。
　本発明の他の好適な実施形態は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、を備える。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。
　本発明の他の好適な実施形態では、コンピュータを備える測定装置によって実行されるプログラムは、第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、として前記コンピュータを機能させる。これにより、移動体から計測できる任意の地物を利用して、移動体の移動距離及び移動体の進行方向の変化量のうち少なくとも一方を算出することができる。このプログラムは、記憶媒体に記憶して利用することができる。
　以下、図面を参照して本発明の好適な実施例について説明する。
　［１］背景
　従来のカーナビゲーション装置に搭載されている自車位置推定システムでは、車速センサから算出される速度や加速度センサの出力から算出される速度と、ジャイロセンサの出力から算出されるヨー角とを用いて車両の移動状態を測定し、ＧＰＳ情報と統合することにより現在位置を推定する複合航法システムが用いられている。複合航法システムによる自車位置推定精度向上のためには、車体速度とヨー角を正確に把握する必要がある。
　車両の車体速度は、車速センサから出力される車軸回転パルスに基づく車輪回転速度に変換係数を乗算することにより得られる。また、車体速度は、加速度センサから出力される検出加速度と、加速度センサの感度係数及びオフセット係数とに基づいて算出することができる。さらに、車両のヨー角は、ジャイロセンサから出力される角速度と、ジャイロセンサの感度係数及びオフセット係数とに基づいて算出することができる。
　一般的に、車速センサの変換係数、加速度センサの感度とオフセット、及び、ジャイロセンサの感度とオフセットは、温度や振動などによって変化する特性がある。そのため、それらの誤差を含んだ状態で車体速度やヨー角を算出すると、誤差が蓄積し、計算される車体速度やヨー角は実際の値から大きくずれてしまう。従って、車体速度やヨー角を高精度に推定するためには、車速センサの変換係数、加速度センサの感度とオフセット、ジャイロセンサの感度とオフセットを継続的に補正することが必要である。
　従来のカーナビゲーション装置では、ＧＰＳ情報に基づくドップラー速度を用いて車速センサ出力から車体速度を求める変換係数や加速度センサの感度とオフセットの補正を行っており、ＧＰＳ情報の方位変化量を用いてジャイロセンサの感度とオフセットの補正を行っている。そのため、都市部のようなＧＰＳの受信状態の悪い環境では補正の精度が劣化する。また、トンネルや地下駐車場のようなＧＰＳ測位ができない環境では補正ができない。よって、ＧＰＳの受信状態に関わらず、継続的に変換係数や感度とオフセットの補正を行うことが要求される。
　本実施例による係数更新装置は、ＧＰＳ情報を用いずに、外界センサによる１つの地物の計測結果に基づいて車両の移動距離とヨー角変化量を算出し、車速センサから車体速度を求めるための変換係数、加速度センサの感度とオフセット、及び、ジャイロセンサの感度とオフセットを補正する。また、係数更新装置は移動距離を所定時間で除算することにより車体速度Ｖを算出することができる。なお、外界センサとしては、ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　Ａｎｄ　Ｒａｎｇｉｎｇ）が挙げられる。
　［２］係数の推定方法
　［２．１］車速センサから車体速度を求めるための変換係数の推定方法
　真の車体速度Ｖ（ｔ）と、車速センサから出力される車軸回転パルスに基づく車輪回転速度ωｐ（ｔ）との関係は、変換係数Ｋを用いて次のように表すことができる。
Figure JPOXMLDOC01-appb-M000001
 よって、まず、外界センサによる地物計測値を用いて車両の移動距離ΔＬを求め、それを時間間隔Δｔで除算して計測車体速度を算出する。得られた計測車体速度を真の車体速度Ｖ（ｔ）とみなすと、計測車体速度Ｖ（ｔ）と車速センサから得た車輪回転速度ωｐ（ｔ）とを用いて、式（１）により変換係数Ｋを推定することができる。なお、変換係数Ｋは本発明の第１情報に相当する。
　［２．２］加速度センサの感度係数とオフセット係数の推定方法
　真の車体加速度Ｖ＿ｄｏｔ（ｔ）と、加速度センサで検出する加速度αａｃ（ｔ）との関係は、感度係数Ａとオフセット係数Ｂを用いて次のように表すことができる。
Figure JPOXMLDOC01-appb-M000002
 　よって外界センサによる地物計測値を用いて車両の移動距離ΔＬを求め、それを時間間隔Δｔで除算して計測車体速度Ｖ（ｔ）を算出し、１時刻前に同様な方法で求めた計測車体速度Ｖ（ｔ－１）と時間間隔Δｔを用いて、計測車体加速度を算出する。
Figure JPOXMLDOC01-appb-M000003
 得られた計測車体加速度を真の車体加速度Ｖ＿ｄｏｔ（ｔ）とみなし、次のように置くと、
Figure JPOXMLDOC01-appb-M000004
 以下の１次式で表すことができる。
Figure JPOXMLDOC01-appb-M000005
 　従って、ｘｔとｙｔの複数のデータがあれば、逐次最小二乗法等により、感度係数Ａとオフセット係数Ｂを求めることができる。即ち、外界センサを用いて車両の移動距離ΔＬを取得できれば、加速度センサの感度とオフセットを推定することができる。なお、感度係数Ａとオフセット係数Ｂは本発明の第２情報に相当する。
　［２．３］ジャイロセンサの感度係数とオフセット係数の推定方法
　真の角速度Ψ＿ｄｏｔ（ｔ）と、ジャイロセンサで検出する角速度ωｇ（ｔ）との関係は、感度係数Ｃとオフセット係数Ｄを用いて次のように表すことができる。
Figure JPOXMLDOC01-appb-M000006
 　外界センサによる地物計測値から、車両のヨー角の変化量（以下、「ヨー角変化量」と呼ぶ。）ΔΨＬを取得できた場合、これをその時間間隔Δｔで除することにより、次のように角速度を算出する。
Figure JPOXMLDOC01-appb-M000007
 　外界センサによる地物計測値から求めた角速度（「計測ヨーレート」とも呼ぶ。）を真の角速度とみなすと、次のように表すことができる。
Figure JPOXMLDOC01-appb-M000008
 　よって、次のように置くと、
Figure JPOXMLDOC01-appb-M000009
 以下の１次式で表すことができる。
Figure JPOXMLDOC01-appb-M000010
 　従って、ｘｔとｙｔの複数のデータがあれば、逐次最小二乗法等により、感度係数Ｃとオフセット係数Ｄを求めることができる。即ち、外界センサを用いて車両のヨー角変化量ΔΨを取得できれば、ジャイロセンサの感度とオフセットを推定することができる。なお、感度係数Ｃとオフセット係数Ｄは本発明の第３情報に相当する。
　［３］移動距離とヨー角変化量の算出方法
　以下、外界センサを用いて車両の移動距離ΔＬとヨー角変化量ΔΨを求める方法を説明する。図１は、１つの地物を用いて移動距離とヨー角変化量を算出する方法を説明する図である。なお、前提として、平面板状である道路標識を地物として利用するものとする。また、外界センサとしてＬｉＤＡＲを用いるものとする。以下の手順により、車両の移動距離ΔＬとヨー角変化量ΔΨを算出する。
（１）現在の車両の地点Ｐ１において、所定の地物Ａまでの距離と方向をＬｉＤＡＲにより計測する。
（２）地物を検出した複数のビームから、地物の重心点又は中点の座標を求め、車両から地物までの距離ｒＡ１と角度θＡ１を求める。
（３）地物が平面板状であることを前提に、地物の法線ベクトルと、ＬｉＤＡＲのビームとの角度φ１を算出する。
（４）所定時間Δｔ経過後、地点Ｐ２において上記（１）～（３）の処理を行い、距離ｒＡ２、角度θＡ２、角度φ２を算出する。
（５）下記の式により角度φ２とφ１の差分Δφを求める。
Figure JPOXMLDOC01-appb-M000011
 （６）図１（Ａ）からわかるように、地物Ａと車両位置Ｐ１と車両位置Ｐ２で構成される三角形に対する余弦定理を用いることで移動距離ΔＬが求まる。また、ヨー角変化量ΔΨはθＡ２から∠αを引いたものであり、その∠αはθＡ１とΔφを足したものである。よって、距離ｒＡ１、ｒＡ２、角度θＡ１、θＡ２、Δφを用いて、下記の式により移動距離ΔＬとヨー角変化量ΔΨを算出する。
Figure JPOXMLDOC01-appb-M000012
 　このように、ＬｉＤＡＲで１つの地物を検出することにより、車両の移動距離ΔＬとヨー角変化量ΔΨを算出することができる。ヨー角変化量ΔΨを積分することにより、ヨー角Ψを算出することができ、ヨー角変化量ΔΨを所定時間Δｔで除算することによりヨーレートΨ＿ｄｏｔ（ｔ）を算出することができる。また、移動距離ΔＬを所定時間Δｔで除算することにより車体速度Ｖを算出することができ、移動距離ΔＬを積分することにより移動距離Ｌを算出することもできる。
　［４］係数更新装置
　次に、上記のようにして算出された移動距離ΔＬ及びヨー角変化量ΔΨを用いて、車速センサの変換係数Ｋ、加速度センサの感度係数Ａ及びオフセットＢ、ジャイロセンサの感度係数Ｃ及びオフセット係数Ｄを更新する係数更新装置について説明する。図２は、実施例による係数更新装置１０の構成を示すブロック図である。図示のように、係数更新装置１０は、車速センサ７と、加速度センサ８と、ジャイロセンサ９と、外界センサ（ＬｉＤＡＲ）１２と、地物計測部１３と、ビーム角度算出部１４と、係数計算部１５と、移動距離・ヨー角変化量計算部１６とを備える。なお、地物計測部１３、ビーム角度算出部１４、係数計算部１５、及び、移動距離・ヨー角変化量計算部１６は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。
　車速センサ７は、車軸回転パルスに基づき計測される車輪回転速度ωｐ（ｔ）を係数計算部１５へ供給する。加速度センサ８は、車両の加速度αａｃ（ｔ）を検出し、係数計算部１５へ供給する。また、ジャイロセンサ９は、検出した角速度ωｇ（ｔ）を係数計算部１５へ供給する。
　地物計測部１３は、外界センサ１２の出力に基づいて地物までの距離ｒ及び地物との角度θを計測して移動距離・ヨー角変化量計算部１６へ供給する。また、地物計測部１３は、距離ｒと角度θを、進行方向をｘ、横方向をｙ、高さ方向をｚとする直交座標（ｘ，ｙ，ｚ）に変換し、各計測点の座標としてビーム角度算出部１４へ供給する。
　ビーム角度算出部１４は、外界センサ１２の出力に基づいて、外界センサ１２からのビームと、平面板状である地物の法線ベクトルとのなす角度φ（以下、「ビーム角度」とも呼ぶ。）を算出し、移動距離・ヨー角変化量計算部１６へ供給する。移動距離・ヨー角変化量計算部１６は、異なる時刻における地物までの距離ｒと、地物との角度θと、ビーム角度φとを用いて、前述の式（１１）、（１２）により、移動距離ΔＬ及びヨー角変化量ΔΨを算出し、係数計算部１５へ供給する。
　係数計算部１５は、まず、移動距離ΔＬを所定時間Δｔで除算して車体速度Ｖ（ｔ）を算出する。次に、係数計算部１５は、車体速度Ｖ（ｔ）と車速センサ７から出力された車輪回転速度ωｐ（ｔ）に基づいて、式（１）により車速センサ７の変換係数Ｋを算出し、更新する。また、係数計算部１５は、車体速度Ｖ（ｔ）と１時刻前の車体速度Ｖ（ｔ－１）と時間間隔Δｔと、加速度センサ８から出力された加速度αａｃ（ｔ）に基づいて、式（２）～（３）により加速度センサ８の感度係数Ａ及びオフセットＢを算出し、更新する。さらに、係数計算部１５は、ジャイロセンサ９から供給された角速度ωｇ（ｔ）と、移動距離・ヨー角変化量計算部１６から供給されたヨー角変化量ΔΨに基づいて、式（７）～（８）によりジャイロセンサの感度係数Ｃ及びオフセット係数Ｄを算出し、更新する。
　上記の構成において、地物計測部１３は本発明の第１取得部の一例であり、ビーム角度算出部１４は本発明の第２取得部、第１～第２方向決定部、第１～第６計算部、第１～第２中点決定部の一例であり、係数計算部１５は本発明の第１～第３補正部の一例であり、移動距離・ヨー角変化量計算部１６は本発明の算出部の一例である。また、ヨー角変化量は本発明における進行方向の変化量に相当する。
　次に、上記の係数更新装置１０による係数更新処理について説明する。図３は、係数更新処理のフローチャートである。まず、係数更新装置１０は、外界センサ１２により１つの地物Ａを計測し、地物との距離ｒ及び地物との角度θを含む計測データを取得する（ステップＳ１０）。次に、係数更新装置１０は、それら計測データを用いて、ビーム角度φを算出する（ステップＳ１１）。なお、ビーム角度φの算出方法については後に詳しく説明する。
　次に、係数更新装置１０は、地物Ａの１時刻前の計測データがあるか否かを判定する（ステップＳ１２）。１時刻前の計測データが無い場合（ステップＳ１２：ＮＯ）、係数更新装置１０は時刻をインクリメントし（ステップＳ１３）、処理はステップＳ１０へ戻る。
　一方、１時刻前の計測データがある場合（ステップＳ１２：ＹＥＳ）、係数更新装置１０は、１時刻前の地物Ａの距離ｒＡ１、角度θＡ１、ビーム角度φ１と、現在時刻の地物の距離ｒＡ２、角度θＡ２、ビーム角度φ２を用いて、式（１１）、（１２）によりヨー角変化量ΔΨ及び移動距離ΔＬを算出する（ステップＳ１４）。そして、係数更新装置１０は、移動距離ΔＬと時間間隔Δｔから計測車体速度Ｖ（ｔ）を算出し、ヨー角変化量ΔΨと時間間隔Δｔから、式（７）を用いて真の角速度（計測ヨーレート）Ψ＿ｄｏｔ（ｔ）を算出する（ステップＳ１５）。
　次に、係数更新装置１０は、得られた計測車体速度Ｖ（ｔ）と車速センサの出力ωｐ（ｔ）から式（１）により変換係数Ｋを算出して更新し、得られた計測車体速度Ｖ（ｔ）と１時刻前の計測車体速度Ｖ（ｔ－１）と時間間隔Δｔと加速度センサの出力αａｃ（ｔ）から式（２）により加速度センサの感度係数Ａとオフセット係数Ｂを算出して更新し、さらに得られた計測ヨーレートΨ＿ｄｏｔ（ｔ）とジャイロセンサの出力ωｇ（ｔ）から式（８）により感度係数Ｃとオフセット係数Ｄを算出して更新する（ステップＳ１６）。
　［５］ビーム角度の算出方法
　次に、上記のビーム角度φの算出方法について詳しく説明する。
　［５．１］第１実施例
　まず、ビーム角度φの算出方法の第１実施例を説明する。第１実施例は、ビーム角度φを正確に求める方法である。
　［５．１．１］概要説明
　ＬｉＤＡＲによる計測によって、車両位置から地物までの距離ｒと、水平角度θと、垂直角度とが得られる。係数更新装置１０のビーム角度算出部１４は、図４（Ａ）に示すように、ＬｉＤＡＲにより得られる距離と角度データを、車両の進行方向をｘ、横方向をｙ、高さ方向をｚとする直交座標（ｘ，ｙ，ｚ）に変換する。
　図４（Ｂ）は、対象となる地物の例として、円形の道路標識をＬｉＤＡＲにより計測した様子を示す。ＬｉＤＡＲの複数のビームにより、道路標識上に複数の計測点が形成される。ビーム角度算出部１４は、対象とする地物に当たった複数のビームの計測値を取得する。このとき、地物の縁にかかるビームは反射率が低くなるため検出されないので、ビーム角度算出部１４は、所定の閾値以上の反射率を有する計測点を道路標識上の計測点であると判定する。これにより、ビーム角度算出部１４は、道路標識上に存在する複数の計測点を取得する。図４（Ｂ）の白抜きの丸は、閾値以下で検出されなかったビームをイメージしている。
　次に、ビーム角度算出部１４は、図４（Ｂ）に示すように、各計測点の座標（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、・・（ｘｎ，ｙｎ，ｚｎ）から、重心点の座標Ｇ（ｘｇ，ｙｇ，ｚｇ）を求める。次に、図５に示すように、ビーム角度算出部１４は、重心点の座標Ｇ（ｘｇ，ｙｇ，ｚｇ）を用いて、車両（原点）から重心点（ｘｇ，ｙｇ，ｚｇ）までの方向ベクトルを求める。また、ビーム角度算出部１４は、各計測点の座標（ｘ１，ｙ１，ｚ１）、（ｘ２，ｙ２，ｚ２）、・・（ｘｎ，ｙｎ，ｚｎ）から、最小２乗法により、道路標識の表面を規定する平面の式を生成し、その平面の法線ベクトルを求める。そして、ビーム角度算出部１４は、平面の法線ベクトル及び車両から重心点までの方向ベクトルのｘｙ成分から、２つのベクトルのなす角度であるビーム角度φを求める。
　［５．１．２］詳細説明
　次に、上記の算出方法を詳しく説明する。まず、円形の道路標識を含む平面の方程式の求め方を説明する。具体的に、ビーム角度算出部１４は、道路標識上に存在する複数の計測点（ｘｋ，ｙｋ，ｚｋ）から、平面の方程式
Figure JPOXMLDOC01-appb-M000013
 の係数ａ、ｂ、ｃ、ｄを求める。いま、法線ベクトルの長さを１として、
Figure JPOXMLDOC01-appb-M000014
 とする。式（１３）を変形して、
Figure JPOXMLDOC01-appb-M000015
 を得る。
　式（１５）に（ｘｋ，ｙｋ）を代入して計算する右辺の値と、ｚｋの値の誤差の総和ｅは以下の式となる。
Figure JPOXMLDOC01-appb-M000016
 　最小二乗法により、式（１６）を最小にする（α，β，γ）を求める。式（１６）を最小にする（α，β，γ）は、それぞれで式（１６）を偏微分すると０になる。
Figure JPOXMLDOC01-appb-M000017
 　式（１７）、（１８）、（１９）をまとめると以下のようになる。
Figure JPOXMLDOC01-appb-M000018
 　式（２０）により、逆行列を用いた演算により、以下のように（α，β，γ）を求めることができる。
Figure JPOXMLDOC01-appb-M000019
 　式（１５）より、
Figure JPOXMLDOC01-appb-M000020
 であるから、
Figure JPOXMLDOC01-appb-M000021
 となる。
　式（２２）を式（１４）に代入すると、
Figure JPOXMLDOC01-appb-M000022
 となり、よって
Figure JPOXMLDOC01-appb-M000023
 となる。
　これを式（２２）に代入するとａ，ｂ，ｄが求まり、最終的に以下の結果を得る。
Figure JPOXMLDOC01-appb-M000024
 　次に、平面の法線ベクトルと直線の方向ベクトルについて説明する。まず、一般形で示される平面の方程式
Figure JPOXMLDOC01-appb-M000025
 の法線ベクトルＮＶは以下である（図６（Ａ）参照）。
Figure JPOXMLDOC01-appb-M000026
 　一方、一般形で示される直線の方程式
Figure JPOXMLDOC01-appb-M000027
 の方向ベクトルＤＶは以下である（図６（Ｂ）参照）。
Figure JPOXMLDOC01-appb-M000028
 　なお、（ｘｇ，ｙｇ，ｚｇ）と（ｘ０，ｙ０，ｚ０）の２点を通る直線の方程式は
Figure JPOXMLDOC01-appb-M000029
 で与えられるため、（ｘ０，ｙ０，ｚ０）が原点（０，０，０）である場合、
Figure JPOXMLDOC01-appb-M000030
 となるので、方向ベクトルＤＶは以下となる（図６（Ｃ）参照）。
Figure JPOXMLDOC01-appb-M000031
 　次に、法線ベクトルと方向ベクトルのなす角、即ちビーム角φの算出について説明する。本実施例では、車両の高さ方向の成分を考慮していないので、ｚ軸成分を無視すればよい。したがって、図７（Ａ）に示すように、法線ベクトルと方向ベクトルをｘｙ平面に投影したベクトルをそれぞれＮＶ’、ＤＶ’とすると、法線ベクトルＮＶ’を角度φ回転させて｜ＤＶ’｜／｜ＮＶ’｜倍すると方向ベクトルＤＶ’になるため、以下の式で表せる。
Figure JPOXMLDOC01-appb-M000032
 式（２５）を計算すると、以下の２式が得られる。
Figure JPOXMLDOC01-appb-M000033
 　式（２６）、（２７）に対し、“－式（２６）×ｂ＋式（２７）×ａ”によりｃｏｓφを削除すると、
Figure JPOXMLDOC01-appb-M000034
 となるため、以下の式からビーム角度φが求められる。
Figure JPOXMLDOC01-appb-M000035
 　なお、本実施例では基本的に道路標識などの平面板状の地物をおもて面から計測することを前提としており、ビーム角度φの範囲は－９０［ｄｅｇ］～９０［ｄｅｇ］となるので、式（２８）のようにビーム角度φをｓｉｎφで算出することが好ましい。
　上記のように、第１実施例は、最小２乗法で平面の方程式を求めているため、図７（Ｂ）に示すように、地物が多少曲面形状であったとしても法線ベクトルを求めることができる。さらに、図７（Ｃ）に示すように、地物の面に多少の凹凸があったとしても、最小２乗法による平面の方程式を作成することができ、法線ベクトルを求めることができる。
　［５．１．３］算出処理
　次に、ビーム角度φの算出処理について説明する。図８は、第１実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。
　まず、ビーム角度算出部１４は、ＬｉＤＡＲにより地物Ａを計測して得たｎ個のデータ（ｘｋ，ｙｋ，ｚｋ）から、重心点Ｇ（ｘｇ，ｙｇ，ｚｇ）を算出し、車両から重心点Ｇへの方向ベクトルＤＶ（ｘｇ，ｙｇ，ｚｇ）とする。また、ビーム角度算出部１４は、車両から重心点Ｇまでの距離ｒＡ［ｔ］と角度θＡ［ｔ］を求める（ステップＳ２１）。
　次に、ビーム角度算出部１４は、ｎ個のデータ（ｘｋ，ｙｋ，ｚｋ）から、最小２乗法を用いて、平面の方程式ａｘ＋ｂｙ＋ｃｚ＋ｄ＝０を作成し、平面の法線ベクトルＮＶ（ａ，ｂ，ｃ）を求める（ステップＳ２２）。そして、ビーム角度算出部１４は、法線ベクトルＮＶと方向ベクトルＤＶのｘｙ成分から、２つのベクトルがなす角度φを算出する（ステップＳ２３）。以上でビーム角度φの算出処理は終了する。
　［５．２］第２実施例
　次に、ビーム角度φの算出方法の第２実施例を説明する。第２実施例は、ビーム角度φを簡易的に求める方法である。
　［５．２．１］算出方法
　図９は、第２実施例によるビーム角度φの算出方法を示す。図９において、車両の位置を原点Ｏとし、平面板状の地物の両端が点Ｃ、Ｅであるとする。ビーム角度算出部１４は、まず、地物の計測データから、その両端点Ｃ、Ｅまでの距離、即ち、ＯＣ長、ＯＥ長と、その間の角度Δθを抽出する。
　次に、ビーム角度算出部１４は、三角形△ＯＣＥに対する余弦定理により、下記のようにＣＥ長を求める。
Figure JPOXMLDOC01-appb-M000036
 　次に、ビーム角度算出部１４は、求めたＣＥ長を利用して余弦定理を用いて、下記のように∠ＯＣＥの角度を求める。
Figure JPOXMLDOC01-appb-M000037
 　次に、ビーム角度算出部１４は、辺ＣＥの中点を点Ｄ（ＣＤ長＝ＤＥ長）とし、三角形△ＯＣＤに対する余弦定理によりＯＤ長を求める。
Figure JPOXMLDOC01-appb-M000038
 　次に、ビーム角度算出部１４は、求めたＯＤ長を利用して、∠ＣＤＯの角度を求める。
Figure JPOXMLDOC01-appb-M000039
 そして、ビーム角度算出部１４は、９０度から∠ＣＤＯの角度を引いて角度φを求める。
Figure JPOXMLDOC01-appb-M000040
 こうして、ビーム角度φを簡易的に求めることができる。この方法の利点は、第１実施例よりも計算量が少なくて済むことである。
［５．２．２］算出処理
　次に、ビーム角度φの算出処理について説明する。図１０は、第２実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。
　まず、ビーム角度算出部１４は、地物Ａを検出した複数のビームのうち、横方向に最も離れた２つのビームを選択して地物の両端点Ｃ、Ｅを求め、両端点Ｃ、Ｅまでの距離ＯＣ、ＯＥ及びその間の角度Δθを取得する（ステップＳ３１）。次に、ビーム角度算出部１４は、両端点Ｃ、Ｅの中点Ｄを求め、車両からその中点Ｄまでの距離（ＯＤ長）求める（ステップＳ３２）。次に、ビーム角度算出部１４は、車両と中点Ｄとを結んだ直線ＯＤと地物の平面とがなす角度（∠ＣＤＯ）を求める（ステップＳ３３）。そして、ビーム角度算出部１４は、求めた角度（∠ＣＤＯ）を９０度から減算して角度φを算出する（ステップＳ３４）。以上でビーム角度φの算出処理は終了する。
　［５．３］第３実施例
　次に、ビーム角度φの算出方法の第３実施例を説明する。第３実施例は、ビーム角度φを近似的に求める方法である。
　［５．３．１］算出方法
　図１１は、第３実施例によるビーム角度φの算出方法を示す。図１１において、車両の位置を原点Ｏとし、平面板状の地物の両端が点Ｃ、Ｅであるとする。ビーム角度算出部１４は、まず地物の計測データから、その両端点Ｃ、Ｅまでの距離、即ち、ＯＣ長、ＯＥ長と、その間の角度Δθを抽出する。
　次に、直線ＯＥ上に、ＯＣ長と同じ長さとなる点Ｆを設けると、
Figure JPOXMLDOC01-appb-M000041
 となる。
　また、三角形△ＯＣＦは二等辺三角形であるので、
Figure JPOXMLDOC01-appb-M000042
 となる。
　ここで、車両から地物までの距離が数メートル以上であれば、直線ＯＣと直線ＯＥはほぼ平行とみなすことができる。よって、三角形△ＣＦＥを、∠ＣＦＥが直角である直角三角形とみなすと、角度φは以下のように得られる。
Figure JPOXMLDOC01-appb-M000043
 この方法の利点は、第２実施例よりもさらに計算量が少なくて済むことである。
［５．３．２］算出処理
　次に、ビーム角度φの算出処理について説明する。図１２は、第３実施例によるビーム角度の算出処理のフローチャートである。なお、この処理は、図３に示す係数更新処理のステップＳ１１に相当する。
　まず、ビーム角度算出部１４は、地物Ａを検出した複数のビームのうち横方向に最も離れた２つのビームを選択して地物の両端点Ｃ、Ｅを求め、両端点Ｃ、Ｅまでの距離ＯＣ、ＯＥ及びその間の角度Δθを取得する（ステップＳ４１）。次に、ビーム角度算出部１４は、車両から端点Ｅまでの直線ＯＥ上に点Ｆを設け、前述の式（２９）により点Ｆから端点ＥまでのＦＥ長を算出する（ステップＳ４２）。
　次に、ビーム角度算出部１４は、ＯＣ長と角度Δθを用いて、式（３０）によりＣＦ長を算出する（ステップＳ４３）。そして、ビーム角度算出部１４は、ＦＥ長とＣＦ長を用いて、式（３１）により角度φを算出する（ステップＳ４４）。以上でビーム角度φの算出処理は終了する。
　［６］地物計測中の車両の移動を考慮した地物の計測点の座標計算
　上述したヨー角変化量ΔΨの算出は、地物の法線ベクトルと車両から地物までの方向ベクトルとがなす角度φを求めることにより行われる。しかし、水平方向にスキャンするタイプのＬｉＤＡＲの場合、対象とする地物の左端と右端を計測する時刻が異なるので、その時間内に車両自身が移動すると、計測の基準位置と基準方位が異なることになる。よって、計測の基準位置と基準方位を合わせないままでは、角度φの算出に含まれる誤差が大きくなってしまう。
　図１３（Ａ）は、ＬｉＤＡＲにより１つの地物を計測する間に車両が移動する様子を示す。地物の左端を計測するときには車両は位置Ｐ１にあり、地物の右端を計測するときには車両は位置Ｐｎに移動している。
　図１３（Ｂ）は、図１３（Ａ）の例においてＬｉＤＡＲで計測した地物上の点の座標（ｒ、θ）を、車両の位置を原点とした（ｘ，ｙ）座標に変換したものを示す。車両の移動に伴って、地物までの距離と方向が変化するため、地物の平面が歪んだものとなってしまう。その結果、地物の中心（又は重心）座標と法線ベクトルとがずれてしまう。
　図１４（Ａ）に示すように、水平スキャン型のＬｉＤＡＲのスキャン周期がＴ［ｓ］の場合、スキャン角周波数ω［ｒａｄ／ｓ］は、ω＝２π／Ｔとなり、ｒ［ｍ］先でのビームが移動する速度はｒω［ｍ／ｓ］となる。そこに幅Ｗ［ｍ］の道路標識があった場合、その道路標識をスキャンする時間ΔＴは以下となる。
Figure JPOXMLDOC01-appb-M000044
 　つまり、図１４（Ａ）のようにＬｉＤＡＲのスキャンが時計回りであるとすると、道路標識の左端と右端の計測にΔＴの時間差があることになる。仮に、ＬｉＤＡＲのスキャン周期が１００［ｍｓ］の場合、１０［ｍ］先の６０［ｃｍ］幅の道路標識をビームが計測する時間は、式（３２）にＴ＝０．１、ｒ＝１０、Ｗ＝０．６を代入して、ΔＴ＝０．９５５［ｍｓ］と求められる。車両の速度が１００［ｋｍ／ｈ］の時を想定すると、ΔＴ＝０．９５５［ｍｓ］の間に、車両は２．６５［ｃｍ］進むことになる。また、仮に道路標識がもっと近く、２［ｍ］先の位置にあるとすると、ｒ＝２を代入して、ΔＴ＝４．７７５［ｍｓ］となり、車両は１３．２６［ｃｍ］進むことになる。
　従って、車両の速度が速いほど、また対象の地物が近いほど、地物の計測時間内における車両の移動距離が長くなる。その移動距離を考慮せずに計算を行うと、前述のように誤差が大きくなってしまう。よって、車体速度とヨー角変化量の高精度な算出のためには、地物の計測時間内における車両の移動を考慮する必要がある。
　上記の考察で見積もりした道路標識の計測時間は数ｍｓ程度であるため、その時間内では車両の速度やヨーレートの変化はほぼ無いとみなすことができる。よって、前の時刻で計算された自車の速度とヨーレートの値を用いて、現在の地物計測中における車両の方向変化分と位置変化分を算出し、その変化分を用いて各計測点の座標を求めればよい。
　例えば、図１４（Ｂ）において、Ｐｋ点で計測した地物の座標を、Ｐ１点の座標系を用いた値に変換する。順番としては、まずＰｋ点において破線で示す座標軸へと座標を回転させ、その後、ｘ方向とｙ方向の位置の差分（Δｘｋ，Δｙｋ）を加算する。
　まず、Ｐｋ点で計測した距離ｒｋと角度θｋから、以下のようにＰｋ点を原点とするｘｙ座標系の値（ｘｋ，ｙｋ）が得られているものとする。
Figure JPOXMLDOC01-appb-M000045
 ヨーレートが一定という前提があるため、Ｐ１点からＰｋ点までのΔｔｋ間の方向の変化量Ψｋは次式となる。
Figure JPOXMLDOC01-appb-M000046
 　また、仮想的な中心点Ｏからの回転半径Ｒを想定すると、
Figure JPOXMLDOC01-appb-M000047
 となるため、ΔｘｋとΔｙｋは以下のように表すことができる。
Figure JPOXMLDOC01-appb-M000048
 よって、Ｐｋ点で計測する地物の座標は、Ｐ１点を基準位置にすると、以下で計算される。
Figure JPOXMLDOC01-appb-M000049
 　よって、地物のｎ個の計測点に対して上記の計算を行い、算出した複数の座標点を用いて地物の重心点又は中点、及び、法線ベクトルを求めて角度φを算出すればよい。
　図１５は、地物計測時間内の車両の移動を考慮した、地物の計測点の座標計算処理のフローチャートである。この処理は、前述の地物計測部１３により行われる。まず、地物計測部１３は、前の時刻で計測（あるいは推定）した自車の速度Ｖ（ｔ-1）とヨーレートΨ＿ｄｏｔ（ｔ－１）を読み出し（ステップＳ５０）、平面形状の道路標識である地物Ａを対象としたｎ点の計測データを取得する（ステップＳ５１）。次に、地物計測部１３は、地物の計測データの左端のデータＰ１を基準の位置及び方位に設定する（ステップＳ５２）。
　次に、地物計測部１３は、係数ｋに「１」をセットし（ステップＳ５３）、係数ｋが計測データ数ｎ以下か否かを判定する（ステップＳ５４）。係数ｋが計測データ数ｎ以下である場合（ステップＳ５４：ＹＥＳ）、地物計測部１３は点Ｐ１から点Ｐｋまでの時間差Δｔｋを求める（ステップＳ５５）。
　次に、地物計測部１３は、前述の式（３３）を用いて、Ｐ１点からＰｋ点までの角度変化Ψｋを求め（ステップＳ５６）、さらに式（３４）、（３５）を用いてＰ１点からＰｋ点までの位置の差（Δｘｋ，Δｙｋ）を求める（ステップＳ５７）。そして、地物計測部１３は、式（３６）を用いて、Ｐ１点を基準とした座標系におけるＰｋ点での地物の座標を算出する（ステップＳ５８）。
　次に、地物計測部１３は、係数ｋをインクリメントし（ステップＳ５９）、ステップＳ５４へ戻る。そして、係数ｋが計測データ数ｎを超えると（ステップＳ５４：ＮＯ）、処理は終了する。こうして得られた計測データを用いてビーム角度φを算出することにより、地物の計測時間内の車両の移動分を補正し、移動距離ΔＬ及びヨー角変化量を高精度で算出することが可能となる。また、ヨー角変化量ΔΨを積分することにより、ヨー角Ψを算出することができ、ヨー角変化量ΔΨを所定時間Δｔで除算することによりヨーレートΨ＿ｄｏｔ（ｔ）を算出することができる。さらに、移動距離ΔＬを所定時間Δｔで除算することにより車体速度Ｖを算出することができ、移動距離ΔＬを積分することにより移動距離Ｌを算出することもできる。
　［７］変形例
　地物の平面に対してＬｉＤＡＲのビームが小さな角度で入射すると、距離計測値の誤差が大きくなるため、その状態で算出する車体速度とヨー角変化量にも誤差が大きくなる。従って、地物の法線ベクトルと車両から地物までの方向ベクトルとのなす角度φが大きいときは、誤差が大きいと判断し、車速センサの変換係数や、加速度センサとジャイロセンサの感度係数とオフセット係数を算出する際、重み付け値を小さくして平均化処理を行うことが好ましい。これにより、誤差の含まれる算出値の影響を小さくして、感度係数やオフセット係数を算出することができる。
　本発明は、移動体に搭載する装置に利用することができる。
　７　車速センサ
　８　加速度センサ
　９　ジャイロセンサ
　１２　外界センサ
　１３　地物計測部
　１４　ビーム角度算出部
　１５　係数計算部
　１６　ヨー角変化量計算部

Claims (12)

  1. 　第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部と、
    　前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部と、
    　を備えることを特徴とする測定装置。
  2. 　前記少なくとも２つの位置の座標を取得する第２取得部を更に備え、
    　前記算出部は、前記第１時刻及び前記第２時刻における、（ｉ）前記移動体から前記少なくとも２つの位置までのそれぞれの距離及び前記少なくとも２つの位置の座標に基づいて算出される、前記地物の表面に対する法線と前記移動体からみた前記地物上の１つの位置の方向とがなす角度、並びに（ｉｉ）前記移動体からみた前記１つの位置の方向と前記移動体の進行方向とがなす角度、に基づき前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
  3. 　前記第１取得部は、
    　前記移動体から前記１の地物の少なくとも３つの位置までの距離及び前記移動体から見た前記少なくとも３つの位置の方向から、前記少なくとも３つの位置の座標を計算する第１計算部と、
    　前記少なくとも３つの位置の座標に基づいて前記地物の表面の重心点の座標を計算し、前記移動体からみた前記重心点の方向を決定する第１方向決定部と、
    　前記少なくとも３つの位置の座標に基づいて前記地物の表面を規定する平面の式を計算し、前記平面の式に基づいて前記地物の表面に対する法線の方向を決定する第２方向決定部と、
    　前記重心点の方向と前記法線の方向とのなす角度を計算する第２計算部と、を備え、
    　前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記重心点までの距離、前記移動体からみた前記重心点の方向と前記移動体の進行方向とがなす角度、及び、前記重心点の方向と前記法線の方向とのなす角度に基づき、前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
  4. 　前記第１取得部は、
    　前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角度を計算する第３計算部と、
    　前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づいて、前記１の地物の中点の座標を決定する第１中点決定部と、
    　前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第４計算部と、を備え、
    　前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づき、前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
  5. 　前記第１取得部は、
    　前記移動体から前記１の地物の両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向のなす角度を計算する第５計算部と、
    　前記両端点までの距離、及び、前記移動体からみた前記両端点それぞれの方向がなす角に基づき前記１の地物の中点の座標を決定する第２中点決定部と、
    　前記移動体からみた前記両端点それぞれの方向が平行であるとみなして、前記移動体からみた前記中点の方向と、前記中点における前記地物の表面に対する法線の方向とがなす角度を計算する第６計算部と、を備え、
    　前記算出部は、前記第１時刻及び第２時刻それぞれにおける、前記移動体から前記中点までの距離、前記移動体からみた前記中点の方向と前記移動体の進行方向とがなす角度、及び、前記中点の方向と前記法線の方向とがなす角度に基づいて、前記移動距離及び前記変化量のうち少なくとも一方を算出することを特徴とする請求項１に記載の測定装置。
  6. 　前記地物上の複数の位置のうちの１つを計測したときの前記移動体の位置である移動体位置を基準位置とし、前記複数の位置のうち前記基準位置以外の前記移動体位置で計測した位置の座標を、前記基準位置の座標系における座標に変換する座標変換部を備えることを特徴とする請求項１乃至５のいずれか一項に記載の測定装置。
  7. 　前記算出部は前記移動距離を算出し、
    　前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される車速センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第１情報を補正する第１補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
  8. 　前記算出部は前記移動距離を算出し、
    　前記測定装置は、前記移動距離に基づき算出される、単位時間あたりの移動距離に基づき、前記移動体に搭載される加速度センサの前記第２時刻における出力結果から前記移動体の速度を算出するための第２情報を補正する第２補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
  9. 　前記算出部は前記変化量を算出し、
    　前記測定装置は、前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第３情報を補正する第３補正部を更に備えることを特徴とする請求項１乃至６のいずれか一項に記載の測定装置。
  10. 　測定装置により実行される測定方法であって、
    　第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得工程と、
    　前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出工程と、
    　を備えることを特徴とする測定方法。
  11. 　コンピュータを備える測定装置によって実行されるプログラムであって、
    　第１時刻及び第２時刻それぞれにおける、移動体から１の地物上の少なくとも２つの位置までのそれぞれの距離及び前記移動体からみた前記地物の方向と前記移動体の進行方向とがなす角度をそれぞれ取得する第１取得部、
    　前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の移動距離及び前記移動体の進行方向の変化量のうち少なくとも一方を算出する算出部、
    　として前記コンピュータを機能させることを特徴とするプログラム。
  12. 　請求項１１に記載のプログラムを記憶した記憶媒体。
PCT/JP2016/060509 2016-03-30 2016-03-30 測定装置、測定方法、及び、プログラム Ceased WO2017168654A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
PCT/JP2016/060509 WO2017168654A1 (ja) 2016-03-30 2016-03-30 測定装置、測定方法、及び、プログラム
JP2018507956A JP6707627B2 (ja) 2016-03-30 2016-03-30 測定装置、測定方法、及び、プログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2016/060509 WO2017168654A1 (ja) 2016-03-30 2016-03-30 測定装置、測定方法、及び、プログラム

Publications (1)

Publication Number Publication Date
WO2017168654A1 true WO2017168654A1 (ja) 2017-10-05

Family

ID=59963760

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2016/060509 Ceased WO2017168654A1 (ja) 2016-03-30 2016-03-30 測定装置、測定方法、及び、プログラム

Country Status (2)

Country Link
JP (1) JP6707627B2 (ja)
WO (1) WO2017168654A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110988989A (zh) * 2019-12-05 2020-04-10 大连民族大学 一种爆破振动速度幅值和方向修正的方法
GB2590429A (en) * 2019-12-17 2021-06-30 Stephen Mitchell John Intelligent speed mapping

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10142336A (ja) * 1996-11-06 1998-05-29 Mitsubishi Motors Corp 車両の走行レーン認識装置
JP2005025497A (ja) * 2003-07-02 2005-01-27 Nissan Motor Co Ltd 標識認識装置
JP2006160116A (ja) * 2004-12-08 2006-06-22 Toyota Central Res & Dev Lab Inc 車両運動推定装置及び移動体検出装置
JP2008008783A (ja) * 2006-06-29 2008-01-17 Toyota Motor Corp 車輪速パルス補正装置
JP2008249639A (ja) * 2007-03-30 2008-10-16 Mitsubishi Electric Corp 自己位置標定装置、自己位置標定方法および自己位置標定プログラム
US20100017128A1 (en) * 2007-06-05 2010-01-21 Gm Global Technology Operations, Inc. Radar, Lidar and camera enhanced methods for vehicle dynamics estimation
JP2013072782A (ja) * 2011-09-28 2013-04-22 Aisin Aw Co Ltd 移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラム
JP2014098613A (ja) * 2012-11-14 2014-05-29 Kddi Corp ジャイロセンサの較正機能を備えた角速度測定装置、較正プログラム及び方法
US20150025786A1 (en) * 2012-02-28 2015-01-22 Continental Automotive Gmbh Method And Device For Determining The Speed And/Or Position Of A Vehicle

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH10142336A (ja) * 1996-11-06 1998-05-29 Mitsubishi Motors Corp 車両の走行レーン認識装置
JP2005025497A (ja) * 2003-07-02 2005-01-27 Nissan Motor Co Ltd 標識認識装置
JP2006160116A (ja) * 2004-12-08 2006-06-22 Toyota Central Res & Dev Lab Inc 車両運動推定装置及び移動体検出装置
JP2008008783A (ja) * 2006-06-29 2008-01-17 Toyota Motor Corp 車輪速パルス補正装置
JP2008249639A (ja) * 2007-03-30 2008-10-16 Mitsubishi Electric Corp 自己位置標定装置、自己位置標定方法および自己位置標定プログラム
US20100017128A1 (en) * 2007-06-05 2010-01-21 Gm Global Technology Operations, Inc. Radar, Lidar and camera enhanced methods for vehicle dynamics estimation
JP2013072782A (ja) * 2011-09-28 2013-04-22 Aisin Aw Co Ltd 移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラム
US20150025786A1 (en) * 2012-02-28 2015-01-22 Continental Automotive Gmbh Method And Device For Determining The Speed And/Or Position Of A Vehicle
JP2014098613A (ja) * 2012-11-14 2014-05-29 Kddi Corp ジャイロセンサの較正機能を備えた角速度測定装置、較正プログラム及び方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110988989A (zh) * 2019-12-05 2020-04-10 大连民族大学 一种爆破振动速度幅值和方向修正的方法
GB2590429A (en) * 2019-12-17 2021-06-30 Stephen Mitchell John Intelligent speed mapping

Also Published As

Publication number Publication date
JP6707627B2 (ja) 2020-06-10
JPWO2017168654A1 (ja) 2019-02-14

Similar Documents

Publication Publication Date Title
JP2022113746A (ja) 判定装置
WO2019188745A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JP4199553B2 (ja) ハイブリッド航法装置
CN111380573B (zh) 用于校准运动的对象传感器的取向的方法
US12085653B2 (en) Position estimation device, estimation device, control method, program and storage media
JP7155284B2 (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
WO2017150106A1 (ja) 車載装置、及び、推定方法
JP7487388B2 (ja) 測定装置、測定方法、及び、プログラム
JP2024161105A (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
WO2018212292A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
JPWO2017168586A1 (ja) 算出装置、制御方法、プログラム及び記憶媒体
JPWO2017109978A1 (ja) 距離推定装置、距離推定方法及びプログラム
JP6707627B2 (ja) 測定装置、測定方法、及び、プログラム
JP6503477B2 (ja) 距離推定装置、距離推定方法及びプログラム
CN105759292B (zh) 一种差分gps与机器人的相对位置参数标定方法及系统
WO2017168588A1 (ja) 測定装置、測定方法、及び、プログラム
JP2017181195A (ja) 測定装置、測定方法、及び、プログラム
JP2017177940A (ja) 算出装置、制御方法、プログラム及び記憶媒体
TWI394944B (zh) 車輛姿態估測系統與方法
JP3551661B2 (ja) ナビゲーション装置
CN114080548A (zh) 车载传感器装置
WO2017109976A1 (ja) 距離推定装置、距離推定方法及びプログラム
WO2018163750A1 (ja) 距離推定装置、距離推定方法及びプログラム
WO2019124279A1 (ja) 情報処理装置
WO2018212290A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体

Legal Events

Date Code Title Description
WWE Wipo information: entry into national phase

Ref document number: 2018507956

Country of ref document: JP

NENP Non-entry into the national phase

Ref country code: DE

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

Ref document number: 16896873

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 16896873

Country of ref document: EP

Kind code of ref document: A1