WO2019188802A1 - 情報処理装置、制御方法、プログラム及び記憶媒体 - Google Patents

情報処理装置、制御方法、プログラム及び記憶媒体 Download PDF

Info

Publication number
WO2019188802A1
WO2019188802A1 PCT/JP2019/012133 JP2019012133W WO2019188802A1 WO 2019188802 A1 WO2019188802 A1 WO 2019188802A1 JP 2019012133 W JP2019012133 W JP 2019012133W WO 2019188802 A1 WO2019188802 A1 WO 2019188802A1
Authority
WO
WIPO (PCT)
Prior art keywords
measurement
unit
vehicle
lidar
positional deviation
Prior art date
Application number
PCT/JP2019/012133
Other languages
English (en)
French (fr)
Inventor
加藤　正浩
Original Assignee
パイオニア株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by パイオニア株式会社 filed Critical パイオニア株式会社
Publication of WO2019188802A1 publication Critical patent/WO2019188802A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes

Definitions

  • the present invention relates to a technique for detecting a positional deviation of a measurement unit.
  • Patent Document 1 discloses a technique for estimating a self-position by collating the output of a measurement sensor with the position information of a feature registered in advance on a map.
  • Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.
  • Data obtained from measurement units such as radar and cameras are coordinate system values based on the measurement unit, and are data that depends on the attitude of the measurement unit with respect to the vehicle. Need to be converted to Therefore, when a deviation occurs in the posture of the measurement unit, there is a possibility that an error occurs in the measurement value after conversion into the coordinate system based on the vehicle.
  • the present invention has been made to solve the above-described problems, and it is an object of the present invention to provide an information processing apparatus that can suitably detect a positional deviation of a measuring unit that measures a distance to an object with respect to a moving body. With a purpose.
  • the invention described in claim is an information processing apparatus, which obtains a measurement distance obtained by the measurement unit to the same object measured by each of at least three measurement units provided in the moving body. And a detection unit that detects, from the at least three measurement units, at least one measurement unit in which a displacement of the attachment position with respect to the moving body is generated based on a plurality of measurement distances acquired for each measurement unit. Have.
  • the invention described in the claims is a control method executed by the information processing apparatus, wherein the measurement unit measures up to the same object measured by each of at least three measurement units provided on the moving body. Based on a measurement value acquisition step for acquiring a distance and a plurality of measurement distances acquired for each of the measurement units, at least one measurement unit in which the displacement of the mounting position with respect to the moving body has occurred is measured with the at least three measurement units. And a detection step of detecting from the part.
  • the invention described in the claims is a program executed by a computer, and obtains a measurement distance by the measurement unit to the same object measured by each of at least three measurement units provided in the moving body. And at least one measurement unit in which the displacement of the mounting position with respect to the moving body is detected from the at least three measurement units based on the measurement distance acquisition unit to be performed and a plurality of measurement distances acquired for each measurement unit
  • the computer is caused to function as a detection unit.
  • FIG. 8 is a diagram in which measured values obtained by a rider in which no positional deviation occurs during traveling in FIGS. 7A to 7C are plotted in time series.
  • FIGS. 7A to 7C are plotted in time series. It is a flowchart regarding the detection of a deviation occurrence lidar and the estimation of the position and posture angle.
  • the information processing device acquires a measurement distance by the measurement unit to the same object measured by each of at least three measurement units provided in the moving body. Based on a plurality of measurement distances acquired for each measurement unit, an acquisition unit and a detection unit for detecting at least one measurement unit in which the displacement of the mounting position with respect to the moving body is generated from the at least three measurement units And having.
  • the “positional deviation” is not limited to the deviation of the center of gravity position of the measuring unit with respect to the moving body, but also includes a deviation of the orientation (posture) that does not involve the deviation of the center of gravity position.
  • the information processing apparatus can compare the time-series measurement distances of three or more measurement units, and can suitably detect a measurement unit in which a positional deviation has occurred.
  • the information processing apparatus further includes a movement trajectory calculation unit that calculates a movement trajectory of the moving body when the plurality of measurement values are acquired, and the detection unit includes the movement trajectory. And the measurement unit in which the positional deviation occurs is detected based on the plurality of measurement values for each measurement unit.
  • the information processing apparatus suitably compares the time series measurement distances of three or more measurement units based on the movement trajectory even when the same object is detected at different timings by a plurality of measurement units. , It is possible to detect a measurement unit in which a positional deviation has occurred.
  • the detection unit generates a measurement unit in which a distance between a position indicated by the plurality of measurement values and the movement locus differs from another measurement unit by a predetermined value or more. Is detected as a measuring unit.
  • the information processing apparatus can suitably detect the measurement unit in which the positional deviation has occurred by comparing the movement trajectory with the measurement result of each measurement unit.
  • the information processing apparatus is configured to generate the positional deviation based on the plurality of measurement values of the measurement unit other than the measurement unit in which the positional deviation has occurred and the movement trajectory.
  • a measurement expectation value calculation unit that calculates a measurement expectation value that is an expected value of a measurement value when it is assumed that there is no positional deviation in the measurement unit, a plurality of measurement expectation values, and the position deviation occurs
  • An estimation unit that estimates a position of the measurement unit in which the positional deviation occurs based on the plurality of measurement values of the measurement unit;
  • the “position” estimated by the estimation unit is not limited to the position of the center of gravity of the measurement unit, and may include the direction (posture) of the measurement unit.
  • the information processing apparatus can preferably estimate the position (including the posture) of the measurement unit in which the positional deviation occurs.
  • the estimation unit includes the estimated position of the measurement unit where the positional deviation occurs and the position of the measurement unit stored in the storage unit where the positional deviation occurs. Based on this, the amount of displacement is estimated. According to this aspect, the information processing apparatus can perform processing such as correction of the measurement result of the measurement unit in which the positional deviation has occurred, and can appropriately suppress a decrease in accuracy of processing using the measurement unit.
  • the estimation unit may detect, as the deviation amount, a deviation amount in the pitch direction, yaw direction, or roll direction of the measurement unit detected by the detection unit, or a three-dimensional measurement unit detected by the detection unit. It is preferable to calculate at least one of the shift amounts of the center of gravity position in the space.
  • a control method executed by the information processing apparatus wherein the measurement up to the same object is measured by each of at least three measurement units provided in the moving body.
  • the information processing apparatus can suitably detect the measurement unit in which the positional deviation has occurred.
  • the program executed by the computer is measured by the measuring unit up to the same object measured by each of at least three measuring units provided on the moving body. Based on a plurality of measurement distances acquired for each measurement unit and a measurement distance acquisition unit that acquires a distance, at least one measurement unit in which the displacement of the attachment position with respect to the moving body has occurred is measured by the at least three measurement units.
  • the computer is caused to function as a detection unit that detects from the unit.
  • the computer can suitably detect the measurement unit in which the positional deviation has occurred.
  • the program is stored in a storage medium.
  • FIG. 1A is a schematic configuration diagram of a driving support system according to the present embodiment.
  • the driving support system shown in FIG. 1A is mounted on a vehicle and has an in-vehicle device 1 that performs control related to driving support of the vehicle, and a plurality of lidars (Lidar: Light Detection and Ranging or Laser Illuminated Detection And Ranging) 2.
  • FIG. 1B is an overhead view of the vehicle showing an example of the arrangement of the lidar 2.
  • the in-vehicle device 1 is electrically connected to the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and based on these outputs, the position of the vehicle on which the in-vehicle device 1 is mounted ("own vehicle position"). Also called.) And the vehicle equipment 1 performs automatic driving
  • the in-vehicle device 1 stores a map database (DB: DataBase) 10 that stores road data and feature information that is information on a landmark (landmark) provided near the road.
  • DB DataBase
  • the in-vehicle device 1 detects a lidar 2 (also referred to as a “deviation-generating lidar”) in which a positional deviation (including an attitude deviation) occurs based on the measurement result of the feature by each lidar 2 and the occurrence of the deviation. Estimate the position and attitude angle of the rider. And the vehicle equipment 1 performs the process etc. which correct
  • the in-vehicle device 1 is an example of the “information processing device” in the present invention.
  • the lidar 2 (2A to 2F) emits a pulse laser to a predetermined angle range in the horizontal direction and the vertical direction, discretely measures the distance to an object existing in the outside world, and determines the position of the object.
  • the three-dimensional point cloud information shown is generated.
  • the lidar 2 includes an irradiation unit that emits laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and scan data based on a light reception signal output by the light receiving unit. Output unit.
  • the scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the distance to the object in the irradiation direction of the laser light specified based on the light reception signal described above, and is sent to the in-vehicle device 1. Supplied.
  • the riders 2A and 2D are provided in the front portion of the vehicle, and the riders 2C and 2F are provided in the rear portion of the vehicle.
  • the rider 2B is provided on the left side surface portion of the vehicle, and the rider 2E is provided on the right side surface of the vehicle.
  • the lidar 2 is an example of the “measurement unit” in the present invention.
  • the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5 each supply output data to the in-vehicle device 1.
  • FIG. 2 is a block diagram showing a functional configuration of the in-vehicle device 1.
  • the in-vehicle device 1 mainly includes an interface 11, a storage unit 12, an input unit 14, a control unit 15, and an information output unit 16. Each of these elements is connected to each other via a bus line.
  • the interface 11 acquires output data from sensors such as the lidar 2, the gyro sensor 3, the vehicle speed sensor 4, and the GPS receiver 5, and supplies the output data to the control unit 15. In addition, the interface 11 supplies a signal related to the traveling control of the vehicle generated by the control unit 15 to an electronic control unit (ECU: Electronic Control Unit) of the vehicle.
  • ECU Electronic Control Unit
  • the storage unit 12 stores a program executed by the control unit 15 and information necessary for the control unit 15 to execute a predetermined process.
  • the storage unit 12 includes a map DB 10 and rider installation information IL.
  • the lidar installation information IL is information related to the relative three-dimensional position and posture angle of each rider 2 measured at a certain reference time (for example, when there is no positional deviation such as immediately after alignment adjustment of the rider 2).
  • the attitude angle of the lidar 2 and the like is represented by a roll angle, a pitch angle, and a yaw angle (that is, Euler angle).
  • the input unit 14 is a button operated by the user, a touch panel, a remote controller, a voice input device, and the like, and receives an input for specifying a destination for route search, an input for specifying on / off of automatic driving, and the like.
  • the information output unit 16 is, for example, a display or a speaker that outputs based on the control of the control unit 15.
  • the control unit 15 includes a CPU that executes a program and controls the entire vehicle-mounted device 1.
  • the control unit 15 estimates the vehicle position based on the output signal of each sensor supplied from the interface 11 and the map DB 10, and controls vehicle driving support including automatic driving control based on the estimation result of the vehicle position. And so on.
  • the control unit 15 uses the measurement data output by the rider 2 as the reference for the attitude angle and position of the rider 2 recorded in the rider installation information IL. Conversion from the reference coordinate system to the coordinate system based on the vehicle.
  • the control unit 15 detects the lidar 2 in which the positional deviation has occurred, and estimates the position and posture angle of the lidar 2.
  • the control unit 15 is an example of the “measurement distance acquisition unit”, “detection unit”, “movement trajectory calculation unit”, “measurement expected value calculation unit”, “estimation unit”, and “computer” that executes the program in the present invention. is there.
  • control unit 15 may perform the own vehicle position estimation by various own vehicle position estimation methods using the output of the rider 2.
  • the control unit 15 uses the gyro sensor 3, the vehicle speed sensor based on the measured values of the distance and angle of the landmark 2 by the lidar 2 and the landmark position information extracted from the map DB 10. 4 and / or the vehicle position estimated from the output data of the GPS receiver 5 is corrected.
  • the control unit 15 predicts the vehicle position from output data from the gyro sensor 3, the vehicle speed sensor 4 and the like, and a measurement update step that corrects the predicted value of the vehicle position calculated in the immediately preceding prediction step; Are executed alternately.
  • FIG. 3 is a diagram showing the state variable vector x in two-dimensional orthogonal coordinates.
  • the vehicle position on the plane defined on the two-dimensional orthogonal coordinates of xy is represented by coordinates “(x, y)” and the direction (yaw angle) “ ⁇ ” of the vehicle.
  • the yaw angle ⁇ is defined as an angle formed by the traveling direction of the vehicle and the x-axis.
  • FIG. 4 is a diagram illustrating a schematic relationship between the prediction step and the measurement update step.
  • FIG. 5 shows an example of functional blocks of the control unit 15. As shown in FIG. 4, by repeating the prediction step and the measurement update step, calculation and update of the estimated value of the state variable vector “X” indicating the vehicle position are sequentially executed. Moreover, as shown in FIG. 5, the control part 15 has the position estimation part 21 which performs a prediction step, and the position estimation part 22 which performs a measurement update step.
  • the position prediction unit 21 includes a dead reckoning block 23 and a position prediction block 24, and the position estimation unit 22 includes a landmark search / extraction block 25 and a position correction block 26.
  • the state variable vector of the reference time (ie, current time) “k” to be calculated is represented as “X ⁇ (k)” or “X ⁇ (k)”.
  • the provisional estimated value (predicted value) estimated in the predicting step is appended with “ - ” on the character representing the predicted value, and the estimated value with higher accuracy updated in the measurement updating step. Is appended with “ ⁇ ” on the character representing the value.
  • T is used to obtain the movement distance and azimuth change from the previous time.
  • the position prediction block 24 of the control unit 15 adds the obtained moving distance and azimuth change to the state variable vector X ⁇ (k-1) at the time k-1 calculated in the immediately previous measurement update step, so that the time k A predicted value (also referred to as “predicted position”) X ⁇ (k) is calculated.
  • the landmark search / extraction block 25 of the control unit 15 associates the landmark position vector registered in the map DB 10 with the scan data of the lidar 2. Then, the landmark search / extraction block 25 of the control unit 15, when this association is possible, the measurement value “Z (k)” by the lidar 2 of the landmark that has been associated, and the predicted position X ⁇ ( k) and a landmark measurement value obtained by modeling the measurement processing by the lidar 2 using the landmark position vector registered in the map DB 10 (referred to as “measurement prediction value”) “Z ⁇ (k)”. And get respectively.
  • the measured value Z (k) is a vehicle coordinate system ("vehicle coordinate system") converted from a landmark distance and a scan angle measured by the rider 2 at time k into components with the vehicle traveling direction and the lateral direction as axes. Vector value). Then, the position correction block 26 of the control unit 15 calculates a difference value between the measured value Z (k) and the measured predicted value Z ⁇ (k).
  • the position correction block 26 of the control unit 15 then adds the Kalman gain “K (k) to the difference value between the measured value Z (k) and the measured predicted value Z ⁇ (k) as shown in the following equation (1). ”And adding this to the predicted position X ⁇ (k), an updated state variable vector (also referred to as“ estimated position ”) X ⁇ (k) is calculated.
  • the position correction block 26 of the control unit 15 is similar to the prediction step in that the covariance matrix P ⁇ (k) (simply referred to as P (k) corresponding to the error distribution of the estimated position X ⁇ (k). Is expressed from the covariance matrix P ⁇ (k). Parameters such as the Kalman gain K (k) can be calculated in the same manner as a known self-position estimation technique using an extended Kalman filter, for example.
  • the prediction step and the measurement update step are repeatedly performed, and the predicted position X ⁇ (k) and the estimated position X ⁇ (k) are sequentially calculated, so that the most likely vehicle position is calculated. .
  • FIG. 6A is a diagram illustrating the relationship between the vehicle coordinate system and the lidar coordinate system represented by two-dimensional coordinates.
  • the vehicle coordinate system has a coordinate center “x v ” along the traveling direction of the vehicle and a coordinate axis “y v ” along the lateral direction of the vehicle with the vehicle center as the origin.
  • the lidar coordinate system has a coordinate axis “x L ” along the front direction of the rider 2 (see arrow A ⁇ b> 2) and a coordinate axis “y L ” along the side surface direction of the rider 2.
  • the measurement point [x] at the time “k” viewed from the vehicle coordinate system [x v (k), y v (k)] T is converted to the coordinates [x L (k), y L (k)] T of the lidar coordinate system by the following equation (2) using the rotation matrix “C ⁇ 0 ”. Converted.
  • the transformation from the lidar coordinate system to the vehicle coordinate system may be performed using an inverse matrix (transpose matrix) of the rotation matrix. Therefore, the measurement point [x L (k), y L (k)] T obtained at the time k obtained in the lidar coordinate system is expressed in the coordinates [x v (k), y v of the vehicle coordinate system by the following equation (3). (K)] It is possible to convert to T.
  • FIG. 6B is a diagram illustrating the relationship between the vehicle coordinate system and the lidar coordinate system represented by three-dimensional coordinates.
  • a coordinate axis perpendicular to the coordinate axes x v and y v is “z v ”
  • a coordinate axis perpendicular to the coordinate axes x L and y L is “z L ”.
  • the roll angle of the rider 2 relative to the vehicle coordinate system is “L ⁇ 0 ”, the pitch angle is “L ⁇ 0 ”, the yaw angle is “L ⁇ 0 ”, the position of the rider 2 on the coordinate axis x v is “L x0 ”, and the coordinate axis y v is
  • the measurement point [x v0 (k), y v0 (k), z v0 ( k)] T is the following equation (4) using the direction cosine matrix “C 0 ” represented by the rotation matrices “C ⁇ 0 ”, “C ⁇ 0 ”, and “C ⁇ 0 ” corresponding to roll, pitch, and yaw. )
  • the transformation from the lidar coordinate system to the vehicle coordinate system may be performed using an inverse matrix (transpose matrix) of the direction cosine matrix. Therefore, the measurement point [x L0 (k), y L0 (k), z L0 (k)] T obtained in the lidar coordinate system T is the coordinate [x v0 of the vehicle coordinate system according to the following equation (5). (K), y v0 (k), z v0 (k)] can be converted to T.
  • the control unit 15 measures the same object (also referred to as “object Ltag”) with three or more lidars 2 and converts each lidar 2 into a vehicle coordinate system in time series. Compare the results. Then, the control unit 15 detects the lidar 2 having a large deviation from the measurement result of the other lidar 2 as a deviation occurrence lidar. At this time, the control unit 15 compares the measurement results of the riders 2 with reference to the movement trajectory of the vehicle, so that even if the measurement timing of the object Ltag is different between the riders 2, the control unit 15 can accurately perform the above-described operation. Make a comparison.
  • FIGS. 7A to 7C are diagrams showing, in time series, how the riders 2A to 2C measure the signs provided on the road on which the vehicle is traveling as the object Ltag. In the examples of FIGS. 7A to 7C, it is assumed that the vehicle is traveling at a constant speed while slightly shifting to the right in the lane.
  • the label that is the target object Ltag is present in the measurement target range of the lidar 2A, and the lidar 2A has a plurality of timings during a period in which the target object Ltag is present in the measurement target range of the lidar 2A.
  • a measurement value for the object Ltag is output.
  • the object Ltag is out of the measurement target range of the lidar 2A and exists in the measurement target range of the lidar 2B.
  • the lidar 2B outputs measurement values for the target object Ltag at a plurality of timings during a period in which the target object Ltag is within the measurement target range of the lidar 2B.
  • FIG. 7A the label that is the target object Ltag is present in the measurement target range of the lidar 2A
  • the lidar 2A has a plurality of timings during a period in which the target object Ltag is present in the measurement target range of the lidar 2A.
  • a measurement value for the object Ltag is output.
  • the object Ltag is out of the measurement target range of the lid
  • the object Ltag is out of the measurement target range of the lidar 2B and exists in the measurement target range of the lidar 2C.
  • the lidar 2C outputs measurement values for the target object Ltag at a plurality of timings during a period in which the target object Ltag is within the measurement target range of the lidar 2C.
  • FIGS. 8A to 8C are graphs obtained by plotting measured values obtained by the riders 2A to 2C in which no positional deviation occurs during traveling in FIGS. 7A to 7C in time series.
  • FIG. 8A is a time-series plot diagram of measured values with respect to the x direction of the vehicle coordinate system (that is, the traveling direction of the vehicle).
  • FIG. 8B is a time-series plot diagram of measured values with respect to the y direction (side direction of the vehicle) of the vehicle coordinate system
  • FIG. 8C shows the z direction of the vehicle coordinate system (the height of the vehicle). It is a time-series plot figure of the measured value with respect to (direction).
  • the point sequences 6ax, 6ay, 6az are point sequences representing the time series measurement values of the lidar 2A
  • the point sequences 6bx, 6by, 6bz are point sequences representing the time series measurement values of the lidar 2B
  • Point sequences 6cx, 6cy, 6cz are point sequences representing the time-series measured values of the rider 2C. Note that each point of these point sequences indicates, for example, the barycentric position of the point group of the object Ltag indicated by the point group data obtained from the lidar 2 at each time point.
  • Dashed lines 31 to 33 indicate the moving trajectory of the vehicle, and broken lines 31a to 33a parallel the moving trajectory of the vehicle indicated by the broken lines 31 to 33 in accordance with the time series of measured values of the riders 2A to 2C.
  • a moved line also referred to as a “movement locus parallel line” is shown. Note that the movement trajectory indicated by the broken lines 31 to 33 is, for example, the movement trajectory of the vehicle based on the vehicle coordinate system of the vehicle at a certain point in time, and is calculated based on the outputs of the gyro sensor 3 and the vehicle speed sensor 4.
  • the measured values of the lidars 2A to 2C are the lidar installation information IL.
  • the point sequences 6ax, 6ay, 6az indicating the measurement results of the rider 2A are arranged on the broken lines 31a to 33a parallel to the vehicle movement locus (see the broken lines 31 to 33), and the broken lines 31a to 31a passing through these point sequences are arranged.
  • Point sequences 6bx, 6by, and 6bz indicating the measurement results of the lidar 2B are arranged on 33a.
  • the point sequences 6cx, 6cy, 6cz indicating the measurement results of the rider 2C are parallel to the vehicle movement trajectory, and are arranged on the broken lines 31a to 33a passing through the point sequences indicating the measurement results of the riders 2A, 2B.
  • the plotted point sequence is translated by a predetermined distance from the calculated movement trajectory. They are arranged on the parallel lines of movement trajectories (broken lines 31a to 33a in FIG. 8). In other words, when there is no position shift in the lidar 2, the distance from the calculated movement locus is constant for each of the plotted point sequences. Therefore, for example, when the measurement results in the time series of the plurality of riders 2 having no positional deviation are converted into the vehicle coordinate system and plotted, the control unit 15 determines the movement locus based on the position of the plotted point sequence.
  • the lidar 2 corresponding to the point sequence is regarded as a position shift generation lidar.
  • the above-mentioned predetermined distance is set to a distance that minimizes the sum of the distances between the points of the plotted point sequence and the movement trajectory parallel line (preferably, the sum excluding the predetermined number of outliers). .
  • FIGS. 9A to 9C plot the measurement values obtained by the riders 2A to 2C in the time series when the position deviation occurs in the rider 2B during the traveling of FIGS. 7A to 7C.
  • FIG. 9A is a time-series plot of measured values with respect to the x direction of the vehicle coordinate system
  • FIG. 9B is a time-series plot of measured values with respect to the y direction of the vehicle coordinate system
  • FIG. 9C is a time-series plot of measured values with respect to the z direction of the vehicle coordinate system.
  • the broken lines 31 to 33 indicate the calculated movement trajectories
  • the broken lines 31a to 33a indicate the movement trajectory parallel lines.
  • the point sequences 7bx, 7by, and 7bz indicate the measurement values (also referred to as “measurement expectation values”) of the rider 2B that are expected when it is assumed that no positional deviation has occurred in the rider 2B.
  • a point sequence 6bx indicating the measurement result of the lidar 2B from the movement trajectory parallel line (see the broken lines 31a to 33a) obtained by translating the calculated movement trajectory, 6by and 6bz are separated from each other by a predetermined distance or more. Therefore, in this case, the control unit 15 recognizes the rider 2B as a shift occurrence rider. The control unit 15 shifts the rider 2 when at least one of the point sequences indicating the measurement results in each direction of the vehicle coordinate system of the rider 2 is separated from the movement locus parallel line by a predetermined distance or more. It may be recognized as a generated rider.
  • the control unit 15 is, for example, the point farthest from the movement trajectory parallel line in the target point sequence.
  • the distance may be calculated, or the average of the distances between each point in the target point sequence and the movement trajectory parallel line may be calculated.
  • the control unit 15 converts the time-series measurement results obtained from each of the three or more riders 2 into the vehicle coordinate system, and compares the measurement results for each rider 2 based on the movement trajectory. It is possible to suitably detect a position shift occurrence lidar.
  • the movement trajectory of the vehicle is calculated based on the outputs of the gyro sensor 3 and the vehicle speed sensor 4, but it can also be calculated from the calculation results at each time of the position estimation process of the control unit 15.
  • the calculated estimated vehicle position will be displaced, resulting in an inaccurate vehicle trajectory. There is a possibility.
  • the degree of inaccuracy is large, there arises a problem that it is difficult to detect a deviation occurrence lidar.
  • the expected measurement value of the lidar 2B indicated by the point sequence 7bx, 7by, 7bz is a value obtained by moving the point sequence indicating the actual measurement value of the lidar 2B on the set movement trajectory parallel line. As will be described later, it is preferably used for estimating the posture angle and position of the lidar 2B, which is a displacement-shifting lidar.
  • the control unit 15 calculates the measurement expected value of the deviation occurrence lidar based on the movement trajectory parallel line, and performs the least square method using the Newton method based on the set of the calculated measurement expectation value and the actual measurement value of the deviation occurrence lidar. To estimate the attitude angle and position of the displacement-generating lidar.
  • the measurement expected value of the deviation-generating lidar will be described with reference to FIGS. 9 (A) to (C).
  • the measurement result of the lidar 2B which is a position shift occurrence lidar
  • the control unit 15 determines from the point sequence 6bx, 6by, 6bz indicating each measurement value of the lidar 2B that is the position shift generation rider based on the set movement locus parallel line, from the point sequence 7bx on the movement locus parallel line. , 7by, 7bz are calculated as expected measurement values.
  • the control unit 15 estimates the position and posture angle of the rider 2B so that the calculated expected measurement value can be obtained.
  • the roll angle “L ⁇ ”, the pitch angle “L ⁇ ”, the yaw angle “L ⁇ ”, the position “L x ” in the x direction, the position “L x ” in the y direction, and the y direction by the least square method using the Newton method.
  • a method for estimating the position “L y ” and the position “L z ” in the z direction will be described.
  • the measured value of the landmark at time k by the deviation generation lidar is [x L (k), y L (k), z L (k)] T
  • the measured value [x L (k), y L (k) , Z L (k)] T is a measured expected value [x b (k), y b (k), z b (k)] T expressed in the vehicle coordinate system x v , y v , z v , corner L ⁇ , L ⁇ , L ⁇ , and position L x, L y, expressed by the following equation using the L z (6).
  • the solution of these six variables is obtained by substituting into the equation (7) using the measurement result and the measurement expected value at least twice.
  • the accuracy of the solution by the least square method can be improved as the number of measured values and measured expected values to be used is increased, preferably, all obtained measured values and measured expected values are used. Good.
  • initial values of posture angles and positions stored in the rider installation information IL are “L ⁇ 0 ”, “L ⁇ 0 ”, “L ⁇ 0 ”, “L x0 ”, Assuming that “L y0 ” and “L z0 ”, the following expression (12) similar to expression (7) is established.
  • Equation (13) the 9-row and 1-column matrix (that is, the left side) of the measurement values for 3 times is “z”, the 9-by-6 Jacobian matrix is “C”, and the initial values of the posture angle and position
  • the matrix of 6 rows and 1 column indicating the difference between the current value and the current value is “ ⁇ x”
  • the matrix of 9 rows and 1 column relating to the initial measurement values x L0 , y L0 , and z L0 obtained by Expression (12) is “d”.
  • Expression (13) is represented by the following expression.
  • z C ⁇ x + d
  • ⁇ x is expressed by the following equation (18).
  • control unit 15 can suitably estimate the attitude angle and position of the deviation-generating lidar by performing the following first to fifth steps.
  • the control unit 15 calculates ⁇ x based on Expression (18).
  • the control unit 15 sets x to the positions L x0 , L y0 , L z0 and the posture angles L ⁇ 0 , L ⁇ 0 , L ⁇ 0 , and the first step again.
  • the control unit 15 regards x obtained in the fourth step as a final solution, and positions L x , L y , L z indicated by x and the posture angle L ⁇ . , L ⁇ and L ⁇ are regarded as the current position and posture angle of the displacement-generating lidar.
  • FIG. 10 is an example of a flowchart relating to detection of a deviation occurrence lidar and estimation of a position and a posture angle.
  • the control unit 15 repeatedly executes the process of the flowchart of FIG.
  • the control unit 15 measures the same object Ltag with three or more riders 2 while the vehicle is traveling (step S101). Then, the control unit 15 converts the time-series measurement values for the object Ltag obtained by each rider 2 into the vehicle coordinate system, and plots the measurement values for each rider 2 converted into the vehicle coordinate system (step). S102). In this case, the control unit 15 extracts information on the initial position and posture angle of each rider 2 from the rider installation information IL, so that the time for the object Ltag obtained by each rider 2 is calculated based on the equation (5). Each measured value of the series is converted into a vehicle coordinate system.
  • control part 15 calculates the movement locus
  • the control unit 15 translates the calculated movement trajectory in accordance with the plotted point sequence of the measurement points (step S104). Thereby, the control unit 15 sets a movement locus parallel line obtained by translating the calculated movement locus. Then, the control unit 15 determines whether there is a point sequence of measurement points that are separated from the set movement locus parallel line by a predetermined distance or more (step S105). Then, when there is a point sequence of measurement points that are more than a predetermined distance away from the movement trajectory parallel line (step S105; Yes), the control unit 15 detects the lidar 2 corresponding to the point sequence as a deviation generation lidar (step S105). S106).
  • step S105 determines that there is no deviation occurrence lidar and returns the process to step S101 again.
  • control part 15 determines the measurement expected value of a shift
  • production lidar is detected (step S107).
  • the control unit 15 acquires a plurality of sets of measurement values and measurement expected values of the deviation occurrence lidar by determining a measurement expectation value for each of the plurality of measurement values for the deviation occurrence lidar.
  • control unit 15 performs a least-squares method using the Newton method based on a plurality of sets of measurement expected values and measurement values, and estimates the attitude angle and position of the deviation generation rider (step S108).
  • the control unit 15 calculates the estimated posture angle and position change amount of the position deviation occurrence lidar with respect to the posture angle and position of the position deviation occurrence rider before the occurrence of the deviation recorded in the rider installation information IL, and Based on the amount of change, each measurement value of the point cloud data output by the misregistration occurrence lidar is corrected.
  • the control unit 15 stores a map or the like indicating the correction amount of the measurement value for each change amount, and corrects the above-described measurement value by referring to the map or the like.
  • the measurement value may be corrected using the value of a predetermined ratio of the change amount as the correction amount of the measurement value.
  • the control unit 15 stops the use of the position shift occurrence lidar and outputs a predetermined warning by the information output unit 16 when detecting a shift in position or posture angle at which the change amount is equal to or greater than a predetermined threshold. May be.
  • the control unit 15 uses the calculated roll angle L ⁇ , pitch angle L ⁇ , yaw angle L ⁇ , x-direction position L x , y-direction position L y , and z-direction position L z to use the lidar 2.
  • Each measured value of the point cloud data output from the vehicle may be converted from the lidar coordinate system to the vehicle body coordinate system, and based on the converted data, the vehicle position estimation or automatic driving control may be executed. Accordingly, the measurement value of the rider 2 can be appropriately converted into the vehicle coordinate system based on the attitude angle and position of the rider 2 after the occurrence of the deviation.
  • the in-vehicle device 1 when the in-vehicle device 1 includes an adjustment mechanism such as an actuator for correcting the posture angle and position of each rider 2, the posture angle of the rider 2 based on the estimation result. And the position may be modified.
  • the in-vehicle device 1 calculates the estimated amount of change in posture angle and position on the basis of the posture angle and position recorded in the rider installation information IL, and the posture angle and position of the rider 2 by the amount of the change amount. Control is performed to drive the adjustment mechanism so as to correct.
  • the in-vehicle device 1 has a plurality of measured values of each rider 2 up to the object Ltag which is the same object measured by each of at least three riders 2 provided in the vehicle. To get. And the vehicle equipment 1 detects the at least 1 rider 2 from which the positional offset of the attachment position with respect to a vehicle has arisen based on the some measured value acquired for every rider 2. FIG. As a result, the in-vehicle device 1 can accurately detect the lidar 2 in which the positional deviation has occurred.
  • equation (20) is an equation equivalent to the equation (19).
  • initial values of posture angles and positions stored in the rider installation information IL are “L ⁇ 0 ”, “L ⁇ 0 ”, “L ⁇ 0 ”, “L x0 ”, Assuming “L y0 ” and “L z0 ”, the following equation (24) is established.
  • Expression (25) the 9-row and 1-column matrix (that is, the left side) of the measurement expected values for three times is “z”, the 9-by-6 Jacobian matrix is “C”, and the initial posture angle and position The matrix of 6 rows and 1 column indicating the difference between the current value and the current value is “ ⁇ x”, and the matrix of 9 rows and 1 column regarding the initial measurement expected values x b0 , y b0 , and z b0 obtained by the equation (24) is “d”. To do. In this case, Expression (25) is expressed by the following expression.
  • control unit 15 can preferably estimate the position and the posture angle of the deviation-generating lidar.
  • the configuration of the driving support system shown in FIG. 1 is an example, and the configuration of the driving support system to which the present invention is applicable is not limited to the configuration shown in FIG.
  • the electronic control device of the vehicle instead of having the in-vehicle device 1, the electronic control device of the vehicle may execute the processing shown in FIG.
  • the lidar installation information IL is stored in, for example, a storage unit in the vehicle, and the vehicle electronic control device is configured to be able to receive output data of various sensors such as the lidar 2.

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Traffic Control Systems (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
 　ここで、式（２５）において、３回分の計測期待値からなる９行１列の行列（即ち左辺）を「ｚ」、９行６列のヤコビ行列を「Ｃ」、姿勢角及び位置の初期値と現在値との差分を示す６行１列の行列を「Δｘ」、式（２４）により求まる初期計測期待値ｘｂ０、ｙｂ０、ｚｂ０に関する９行１列の行列を「ｄ」とする。この場合、式（２５）は、以下の式により表される。
　　　　　　　ｚ＝ＣΔｘ＋ｄ
　よって、制御部１５は、Δｘを、実施例と同様、式（１８）に基づき算出し、Δｘを「ｘ＝Δｘ＋ｘ０」の式に代入することで、最小２乗解を求めることができる。このｘを新たなｘ０として漸化式計算を繰り返し、ｘの変化が極小になれば、それが最終的なずれ発生ライダの位置姿勢となる。この場合、制御部１５は、車両座標系で示した計測期待値の行列から、式（２４）により求まる初期計測期待値の行列を減算することで、式（１８）のΔｚを算出する。
　このように、本変形例によっても、制御部１５は、ずれ発生ライダの位置及び姿勢角を好適に推定することができる。
　（変形例２）
　図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が図１０等に示す処理を実行してもよい。この場合、ライダ設置情報ＩＬは、例えば車両内の記憶部に記憶され、車両の電子制御装置は、ライダ２などの各種センサの出力データを受信可能に構成される。
　１　車載機
　２　ライダ
　３　ジャイロセンサ
　４　車速センサ
　５　ＧＰＳ受信機
　１０　地図ＤＢ

Claims (9)

  1. 　移動体に設けられた少なくとも３つの計測部の各々により計測される同一の対象物までの前記計測部による計測距離を取得する計測距離取得部と、
    　前記計測部ごとに取得された複数の計測距離に基づいて、前記移動体に対する取付位置の位置ずれが生じている少なくとも１つの計測部を前記少なくとも３つの計測部から検出する検出部と、
    を有する情報処理装置。
  2. 　前記複数の計測値が取得されたときの前記移動体の移動軌跡を算出する移動軌跡算出部をさらに備え、
    　前記検出部は、前記移動軌跡と、前記計測部ごとの前記複数の計測値とに基づいて、前記位置ずれが生じている計測部を検出する請求項１に記載の情報処理装置。
  3. 　前記検出部は、前記複数の計測値が示す位置と前記移動軌跡との距離が他の計測部と所定値以上異なる計測部を、前記位置ずれが生じている計測部として検出する請求項２に記載の情報処理装置。
  4. 　前記位置ずれが生じている計測部以外の計測部の前記複数の計測値と、前記移動軌跡とに基づき、前記位置ずれが生じている計測部に位置ずれが生じていないと仮定したときの計測値の期待値である計測期待値を算出する計測期待値算出部と、
    　複数の前記計測期待値と、前記位置ずれが生じている計測部の前記複数の計測値とに基づき、前記位置ずれが生じている計測部の位置を推定する推定部と、
    をさらに備える請求項２または３に記載の情報処理装置。
  5. 　前記推定部は、推定した前記位置ずれが生じている計測部の位置と、記憶部に記憶された前記位置ずれが生じている計測部の位置とに基づき、前記位置ずれのずれ量を推定する請求項４に記載の情報処理装置。
  6. 　前記推定部は、前記ずれ量として、前記検出部が検出した計測部のピッチ方向、ヨー方向、若しくはロール方向のずれ量、又は、前記検出部が検出した計測部の３次元空間での重心位置のずれ量の少なくとも一方を推定する請求項５に記載の情報処理装置。
  7. 　情報処理装置が実行する制御方法であって、
    　移動体に設けられた少なくとも３つの計測部の各々により計測される同一の対象物までの前記計測部による計測距離を取得する計測値取得工程と、
    　前記計測部ごとに取得された複数の計測距離に基づいて、前記移動体に対する取付位置の位置ずれが生じている少なくとも１つの計測部を前記少なくとも３つの計測部から検出する検出工程と、
    を有する制御方法。
  8. 　コンピュータが実行するプログラムであって、
    　移動体に設けられた少なくとも３つの計測部の各々により計測される同一の対象物までの前記計測部による計測距離を取得する計測距離取得部と、
    　前記計測部ごとに取得された複数の計測距離に基づいて、前記移動体に対する取付位置の位置ずれが生じている少なくとも１つの計測部を前記少なくとも３つの計測部から検出する検出部
    として前記コンピュータを機能させるプログラム。
  9. 　請求項８に記載のプログラムを記憶した記憶媒体。
PCT/JP2019/012133 2018-03-28 2019-03-22 情報処理装置、制御方法、プログラム及び記憶媒体 WO2019188802A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-063291 2018-03-28
JP2018063291 2018-03-28

Publications (1)

Publication Number Publication Date
WO2019188802A1 true WO2019188802A1 (ja) 2019-10-03

Family

ID=68059031

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/012133 WO2019188802A1 (ja) 2018-03-28 2019-03-22 情報処理装置、制御方法、プログラム及び記憶媒体

Country Status (1)

Country Link
WO (1) WO2019188802A1 (ja)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100076709A1 (en) * 2008-09-19 2010-03-25 Caterpillar Inc. Machine sensor calibration system
WO2018020950A1 (ja) * 2016-07-28 2018-02-01 株式会社神戸製鋼所 建設機械

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100076709A1 (en) * 2008-09-19 2010-03-25 Caterpillar Inc. Machine sensor calibration system
WO2018020950A1 (ja) * 2016-07-28 2018-02-01 株式会社神戸製鋼所 建設機械

Similar Documents

Publication Publication Date Title
WO2019188745A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2018181974A1 (ja) 判定装置、判定方法、及び、プログラム
US10634777B2 (en) Radar odometry for vehicle
US8041472B2 (en) Positioning device, and navigation system
CN111380573B (zh) 用于校准运动的对象传感器的取向的方法
WO2017199333A1 (ja) 情報出力装置、端末装置、制御方法、プログラム及び記憶媒体
EP3872454A1 (en) Measurement accuracy calculation device, host position estimation device, control method, program, and storage medium
CN113454487B (zh) 信息处理装置以及移动机器人
US20230010175A1 (en) Information processing device, control method, program and storage medium
JP6968877B2 (ja) 自己位置推定装置、制御方法、プログラム及び記憶媒体
JP2023164553A (ja) 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
CN113795726B (zh) 自身位置修正方法及自身位置修正装置
JP5748174B2 (ja) 移動体の相対姿勢計測方法と装置
WO2019188802A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2019188886A1 (ja) 端末装置、情報処理方法、プログラム、及び、記憶媒体
JP6707627B2 (ja) 測定装置、測定方法、及び、プログラム
JP2020008461A (ja) 自律移動体の位置推定装置
CN110749327B (zh) 一种合作环境下的车辆导航方法
WO2017168588A1 (ja) 測定装置、測定方法、及び、プログラム
JP2017181195A (ja) 測定装置、測定方法、及び、プログラム
JP2020165945A (ja) 自己位置推定方法及び自己位置推定装置
JP7409037B2 (ja) 推定装置、推定方法、推定プログラム
JP4730832B2 (ja) 車載レーダ装置
RU2781373C1 (ru) Способ коррекции собственного местоположения и устройство коррекции собственного местоположения
CN112325770B (zh) 一种车端单目视觉测量相对精度置信度评估方法及系统

Legal Events

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

Ref document number: 19774486

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19774486

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: JP