WO2020148894A1 - 運動状態判定装置 - Google Patents

運動状態判定装置 Download PDF

Info

Publication number
WO2020148894A1
WO2020148894A1 PCT/JP2019/001490 JP2019001490W WO2020148894A1 WO 2020148894 A1 WO2020148894 A1 WO 2020148894A1 JP 2019001490 W JP2019001490 W JP 2019001490W WO 2020148894 A1 WO2020148894 A1 WO 2020148894A1
Authority
WO
WIPO (PCT)
Prior art keywords
motion
path
unit
route
error
Prior art date
Application number
PCT/JP2019/001490
Other languages
English (en)
French (fr)
Inventor
響介 小西
Original Assignee
三菱電機株式会社
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 三菱電機株式会社 filed Critical 三菱電機株式会社
Priority to PCT/JP2019/001490 priority Critical patent/WO2020148894A1/ja
Priority to CN201980088726.6A priority patent/CN113287157B/zh
Priority to US17/298,067 priority patent/US20220122461A1/en
Priority to JP2020566075A priority patent/JP6952914B2/ja
Publication of WO2020148894A1 publication Critical patent/WO2020148894A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/166—Anti-collision systems for active traffic, e.g. moving vehicles, pedestrians, bikes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3453—Special cost functions, i.e. other than distance or default speed limit of road segments
    • G01C21/3461—Preferred or disfavoured areas, e.g. dangerous zones, toll or emission zones, intersections, manoeuvre types, segments such as motorways, toll roads, ferries

Definitions

  • the present invention relates to an exercise state determination device that determines a target exercise state.
  • the technique of classifying the target motion state from the observation data is known.
  • the target is defined as a movable object such as a vehicle, a ship, an aircraft, a robot, a person, or a bicycle.
  • a point at which the direction of lateral motion of the vehicle changes is extracted from the observation data representing the position of the vehicle, and if the distance between the change points is a certain value or more, the motion state of the vehicle meanders.
  • An apparatus for determining that is is disclosed.
  • the present invention is to solve the above problems, when the observation data includes an observation error, especially in an environment where the distance between the sensor and the target varies and the observation error is not constant, the determination of the target motion state can be performed.
  • the purpose is to reduce the error frequency.
  • a motion state determination device includes a sensor unit that acquires observation data of a target, a route data storage unit that stores route data that represents a shape of a route along which the target can move, and a motion specification of the target.
  • a path component prediction unit that calculates a component parallel to the path and a component that is perpendicular to the path of the prediction value and the prediction error, and the predicted value and the prediction error of the motion data calculated by the path component prediction unit, the observation data
  • a first coordinate conversion unit for converting into the same coordinate system as the first coordinate system represented, observation data acquired by the sensor unit, route data stored in the route data storage unit, and the first coordinate conversion unit Using the predicted value and the prediction error of the motion parameters converted in step 1, a filtering unit that calculates an estimated value and an estimated error of the target motion parameter in the first coordinate system;
  • a second coordinate conversion unit for converting the estimated value and the estimated error of the target motion data into a second coordinate system having coordinate axes of a component parallel to the path and a component perpendic
  • the present invention it is possible to reduce the frequency of erroneous determination of the target motion state due to the position change of the target on the observation data due to the observation error.
  • FIG. 3 is a block diagram showing the configuration of the exercise state determination device 100 according to the first embodiment.
  • FIG. 3 is a conceptual diagram showing a position in a route parallel direction and a position in a route vertical direction regarding a target vehicle 21 in the first embodiment.
  • FIG. FIG. 3 is a conceptual diagram showing coordinate conversion between route coordinates and observation coordinates in the first embodiment.
  • 3 is a block diagram showing a hardware configuration of the exercise state determination device 100 according to the first embodiment.
  • FIG. 3 is a flowchart showing the operation of the exercise state determination device 100 according to the first embodiment.
  • Embodiment 1 Hereinafter, embodiments of the present invention will be described.
  • FIG. 1 is a block diagram showing the configuration of the exercise state determination device 100 according to the first embodiment.
  • the target type is an automobile
  • the target motion states are two types: “vehicle is meandering” and “vehicle is not meandering”.
  • the vehicle whose motion state is to be determined is a vehicle traveling at a position distant from the vehicle (own vehicle) equipped with the sensor for observation.
  • the target vehicle whose motion state is to be determined may be the own vehicle.
  • target vehicle the case where the target of which the motion state is to be determined is referred to as “target vehicle”, and the motion state of the target vehicle has two types of “with meandering” and “without meandering” will be described.
  • the motion state determination device 100 includes a route parallel component prediction unit 10, a route vertical component prediction unit 11, a route data storage unit 12, a first coordinate conversion unit 13, a sensor unit 14, a filtering unit 15, and a first unit.
  • the two-coordinate conversion unit 16 and the state determination unit 17 are provided.
  • the route parallel component prediction unit 10 and the route vertical component prediction unit 11 can be collectively treated as the route component prediction unit 1.
  • the motion state determination device 100 determines whether the target vehicle 21 meanders based on the observation data acquired from the sensor and the route data stored in the route data storage unit 12.
  • the observation data is defined as the information showing the motion parameters (position, velocity and their time differential amount) of the target vehicle 21.
  • the route data is defined as information indicating the shape of a typical movement route of the target vehicle 21.
  • the route data is data representing the shape of the lane in which the target vehicle 21 is traveling. Particularly, when the line passing through the center of the lane is approximated as a set of points, the position data of each center point will be referred to as route data hereinafter.
  • the target may be a ship, an aircraft, a robot, a person, a bicycle, etc., in addition to the automobile.
  • the observation data when the target is a ship, the observation data may be a communication signal indicating the position of the ship (such as a signal from an automatic ship identification device), and the route data may be a typical route.
  • the observation data when the target is a person, the observation data may be a video image of the person, and the route data may be a sidewalk shape.
  • FIG. 2 is a conceptual diagram showing an example of a path parallel direction and a path vertical direction in the motion state determination device 100 according to the first embodiment.
  • the axis parallel to the route is defined as the set of line segments that connect the center point of the lane, which is the route data, to the front and back. Then, the distance from the origin of a certain lane center point to a point on the axis in the direction parallel to the route where the distance to the target vehicle 21 is minimum (route coordinate intersection) is defined as the position in the route parallel direction regarding the target vehicle 21. .. Further, the position in the path parallel direction and the motion specification obtained by differentiating the position with respect to time are referred to as "path parallel direction component of motion specification".
  • the axis in the direction perpendicular to the route is defined as a straight line perpendicular to the axis in the direction parallel to the route at the intersection of the route coordinate axes.
  • the distance from the intersection of the route coordinate axes to the point where the axis in the vertical direction of the route and the position of the target vehicle 21 intersect is defined as the position in the vertical direction of the route regarding the target vehicle 21.
  • the position in the vertical direction of the path and the motion data obtained by differentiating the position with respect to time are referred to as "path vertical component of motion data".
  • FIG. 3 is a conceptual diagram showing an example of an operation in the exercise state determination device 100 according to the first embodiment.
  • the left side of FIG. 3 is a conceptual diagram showing how observation data 23 is obtained when the target vehicle 21 meanders in a coordinate system based on the vehicle 20 equipped with the sensor.
  • the position with respect to the traveling direction of the vehicle 20 is referred to as “vertical position”, and the position in the direction perpendicular to the vertical direction is referred to as “horizontal position”.
  • observation coordinates the coordinate system in this observation data 23 will be referred to as “observation coordinates”.
  • FIG. 3 is a conceptual diagram when the estimated position of the meandering target vehicle 21 changes with time in the route parallel direction and the route vertical direction similarly to the left side.
  • path coordinates the coordinate system that represents the motion parameters in the direction parallel to the path and the direction perpendicular to the path.
  • path coordinates the coordinate system that represents the motion parameters in the direction parallel to the path and the direction perpendicular to the path.
  • the coordinate conversion from the observation coordinates to the route coordinates is performed by the “first coordinate conversion unit 13”, and the conversion from the route coordinates to the observation coordinates is performed by the “second coordinate conversion unit 16”. To do.
  • the motion state determination device 100 predicts the motion parameters of the target vehicle 21 in the next time frame on the route coordinates, converts the predicted value to the observation coordinates, and then the target based on the observation data 23 on the observation coordinates.
  • the motion specifications of the vehicle 21 are estimated (filtered). Then, the estimated value is converted into the route coordinate again by the coordinate change, and the presence or absence of meandering of the target vehicle 21 is determined based on the estimated value on the route coordinate.
  • the meandering in the direction orthogonal to the route can be extracted on the route coordinates.
  • the feature of the motion state estimation device is that the error of the estimated motion parameter (estimation error) is also coordinate-transformed together with the motion parameter and the presence or absence of meandering is determined using this estimation error. ..
  • the route parallel component predictor 10 in the route component predictor 1 receives the estimated value of the parallel component of the motion of the target vehicle 21 at the previous time and the estimated error of the parallel component of the route from the second coordinate conversion unit 16, and calculates the current time.
  • the predicted value of the path parallel component of the motion specification of the target vehicle 21 and the prediction error of the path parallel component are sent to the first coordinate conversion unit 13.
  • the route vertical component prediction unit 11 in the route component prediction unit 1 receives the estimated route vertical component value of the motion specification of the target vehicle 21 at the previous time and the estimated error of the route vertical component from the second coordinate conversion unit 16, and calculates the current time.
  • the route vertical component prediction value of the motion specification of the target vehicle 21 and the route vertical component prediction error are sent to the first coordinate conversion unit 13.
  • the route data storage unit 12 sends the route data to the first coordinate conversion unit 13 and the second coordinate conversion unit 16 when the first coordinate conversion unit 13 and the second coordinate conversion unit 16 execute the route data.
  • the route data may be stored in advance, or may be acquired and stored by a communication unit or the like each time it is used for processing.
  • a communication means for acquiring the route data an existing technology such as communication with a positioning satellite may be used.
  • the first coordinate transformation unit 13 receives from the route parallel component prediction unit 10 the route parallel component prediction value of the motion specifications of the target vehicle 21 at the current time and the prediction error of the route parallel component, and outputs the current value from the route vertical component prediction unit 11.
  • the route vertical component prediction value of the motion specification of the target vehicle 21 at the time and the prediction error of the route vertical component are received, and the route data is received from the route data storage unit 12. Then, the prediction value of the motion specification on the observation coordinate of the target vehicle 21 at the current time and the prediction error of the motion specification on the observation coordinate are sent to the filtering unit 15.
  • the sensor unit 14 is composed of devices for observing the position of the target vehicle 21.
  • it is an existing device such as a monocular camera, a stereo camera, a millimeter wave radar, a laser sensor, and a wireless signal receiver.
  • the sensor unit 14 sends the observation data of the target vehicle 21 at the current time to the filtering unit 15.
  • the means for acquiring the observation data (position of the target vehicle 21, etc.) from the image acquired by the observation device, the distance measurement value, etc. is, The existing technology of is used.
  • the filtering unit 15 receives from the first coordinate conversion unit 13 the predicted value of the motion specification on the observed coordinate of the target vehicle 21 in the current time frame and the prediction error of the motion specification on the observed coordinate, and the sensor unit 14 at the current time.
  • the observation data of the target vehicle 21 is received.
  • the estimated value of the motion specification on the observation coordinate of the target vehicle 21 at the current time and the estimation error of the motion specification on the observation coordinate are sent to the second conversion unit.
  • the second coordinate conversion unit 16 receives from the filtering unit 15 the estimated value of the motion data on the observed coordinate of the target vehicle 21 at the current time and the estimated error of the motion data on the observed coordinate, and the route data storage unit 12 receives the route data. To receive. Then, the estimated value of the motion specification on the route coordinate of the target vehicle 21 at the current time and the estimation error of the motion specification on the route coordinate are sent to the state determination unit 17. In the next time frame, the route parallel component prediction unit 10 is sent the estimated value of the route parallel component of the motion parameters of the target vehicle 21 at the previous time and the estimated error of the route parallel component, and the route vertical component prediction unit 11 is sent the previous time. The estimated value of the vertical component of the route and the estimation error of the vertical component of the route are sent.
  • the state determination unit 17 receives from the second coordinate conversion unit 16 an estimated value of the motion specification on the route coordinate of the target vehicle 21 at the current time and an estimation error of the motion specification on the route coordinate, and the state determination unit 17 of the target vehicle 21 at the current time.
  • the presence or absence of meandering is output as an output of the motion state determination device 100.
  • FIG. 4 is a diagram showing an example of the hardware configuration according to the first embodiment. Each hardware is connected to each other by a wired circuit such as a bus or a network.
  • the route parallel component prediction unit 10 the route vertical component prediction unit 11, the route data storage unit 12, the first coordinate conversion unit 13, the filtering unit 15, the second coordinate conversion unit 16, and the state determination unit 17 are The path parallel component prediction unit 10, the path vertical component prediction unit 11, the first coordinate conversion unit 13, the filtering unit 15, the second coordinate conversion unit 16, and the state determination unit 17, which are stored in the recording device 42, are operated by the arithmetic unit 41. To be executed.
  • the route parallel component prediction unit 10 input/output values to/from the recording device 42 during execution.
  • the processing intermediate data is written, read, and deleted.
  • the arithmetic unit 41 is a CPU (Central Processing Unit), an ECU (Electronic Control Unit), a GPU (Graphics Processing Unit), or the like.
  • the recording device 42 is an HDD (Hard Disk Drive), SSD (Solid State Drive), DRAM (Dynamic Random Access Memory), flash memory, or the like.
  • the output interface 43 is DVI (Digital Visual Interface, registered trademark), HDMI (High-Definition Multimedia Interface, registered trademark), USB (Universal Serial Bus), Ethernet (registered trademark), CAN (Controller Reserver), or the like.
  • FIG. 5 shows operations of the path parallel component prediction unit 10, the path vertical component prediction unit 11, the first coordinate conversion unit 13, the sensor unit 14, the filtering unit 15, the second coordinate conversion unit 16, and the state determination unit 17 at the current time. It is a flowchart showing.
  • a position vector representing the observation data As a preparation for the explanation of the operation, a position vector representing the observation data, a position vector representing the route data, a vector representing the estimated value of the motion parameters of the target vehicle 21, and a matrix representing the estimation error of the motion parameters are defined.
  • the observation data is data representing the position of the target vehicle 21.
  • the observation data representing the position of the target vehicle 21 in a certain k-th time frame is represented by the position vector z k in 2 rows and 1 column defined by the equation (1).
  • z k is a position vector on the observation coordinates
  • z k (1) is observation data representing the lateral position of the target vehicle 21
  • z k (2) is observation data representing the vertical position of the target vehicle 21. ..
  • the superscript of T represents the transpose of the matrix.
  • the route data is data representing the position of the center point of the lane on which the target vehicle 21 is traveling.
  • the position of the i-th center point of the lane on which the target vehicle 21 is traveling is represented by the position vector s i of 2 rows and 1 column defined by the equation (2).
  • s i is a position vector on the observation coordinates
  • s i (1) represents the horizontal position of the center point
  • s i (2) represents the vertical position of the center point.
  • the serial number i of each central point is 1 or more and I or less (I is an integer of 2 or more)
  • the route data is represented by I position vectors s i .
  • the serial numbers i of the central points are rearranged in ascending order of the vertical position. That is, s i (2) ⁇ s i+1 (2) always holds for any serial number i.
  • the center point of the lane which is the route data, is acquired far enough compared to the distance from the own vehicle 20 to the target vehicle 21, and another vehicle is observed in the middle of the vertical direction of any of the center points. Shall be done. That is, when the serial number i at each central point is 1 or more and I or less (I is an integer of 2 or more), it is assumed that the following always holds in any k-th time frame. (3) Then, the vector showing the estimated value of the motion specification of the target vehicle 21 is defined with respect to each of the observation coordinates and the route coordinates. First, the motion parameter estimated value of the target vehicle 21 at the observation coordinates at the time frame k is represented by the following four-row, one-column vector x ( Albanybs) k .
  • x k ( Whybs, 1) is the lateral position of the target vehicle
  • x k ( Whybs, 2) is the vertical position of the target vehicle
  • x k ( Whybs, 3) is the lateral speed of the target vehicle 21
  • X k ( Whybs, 4) represent the estimated value of the vertical speed of the target vehicle 21.
  • the motion parameter estimation value of the target vehicle 21 at the route coordinates in the time frame k is the following 2-row 1-column vector x k (para) representing the motion parameter estimation value of the route parallel component and the route vertical parameter. It is represented by the following 2-row, 1-column vector x k (vert) that represents the motion specification estimation value of the component.
  • x k (para, 1) is the position of the target vehicle 21 in the route parallel direction
  • x k (para, 2) is the speed of the target vehicle 21 in the route parallel direction
  • x k (vert, 1) is the target vehicle 21.
  • the vertical position of the vehicle, x k (vert, 2) represents the estimated value of the speed of the target vehicle 21 in the vertical direction.
  • FIGS. 6 and 7 are conceptual diagrams showing the vector of motion parameter estimation values defined as described above.
  • FIG. 6 shows the position component of the motion specification estimated value x k ( réellebs) in the observation coordinates
  • FIG. 7 shows the position component of the motion specification estimated values x k (para) and x k (vert) in the path coordinates.
  • the route data s i is shown as the origin in the direction parallel to the route in order to simplify the illustration.
  • the estimation error is represented in the form of an error covariance matrix of a vector representing motion specifications.
  • the error of the motion specification estimated value vector x k ( réellebs) in the observation coordinates will be expressed as P k ( réellebs) .
  • the errors of the motion parameter estimation value vectors x k (para) and x k (vert) in the path coordinates are expressed as P k (para) and P k (vert) , respectively.
  • two types of motion specifications that is, a position and a speed are used.
  • it is not always necessary to use the two types of motion parameters of position and velocity, and three or more types of motion parameters including acceleration and jerk components may be used.
  • step ST1 the route parallel component prediction unit 10 predicts the motion parameters of the route parallel component at the current time, and calculates the error of the motion parameter predicted value.
  • the following equation is calculated based on a motion model in which the motion parallel estimate x k (para) and the estimation error P k (para) at the previous time are input, and the route parallel position of the target vehicle 21 moves at a constant speed.
  • k ⁇ 1 (para) are calculated by.
  • Q k (para) is a 2-row 2-column error covariance matrix that represents the error of the motion model, and is a parameter that is set in advance.
  • ⁇ k (para) is a transition matrix for transitioning the motion specification vector from the previous time to the current time based on the motion model, and is the following matrix.
  • ⁇ k represents the elapsed time from the previous time to the current time.
  • the movement model is assumed to be constant velocity movement, but another movement model, such as a constant acceleration movement model with a specific acceleration, may be used.
  • step ST2 the path vertical component prediction unit 11 predicts the motion specifications of the path vertical component at the current time, and calculates the error of the motion specification prediction value.
  • the following equation is calculated based on a motion model in which the estimated vertical motion value x k (vert) and the estimated error P k (vert) at the previous time are input, and the vertical position of the target vehicle 21 moves at a constant speed.
  • k ⁇ 1 (vert) are calculated by. (10) (11)
  • Q k (vert) is a 2-row, 2-column error covariance matrix that represents the error of the motion model, and is a parameter that is set in advance.
  • ⁇ k (vert) is a transition matrix for transitioning the motion specification vector from the previous time to the current time based on the motion model, and is the following matrix. (12)
  • the movement model is assumed to be constant velocity movement, but another movement model, such as a constant acceleration movement model with a specific acceleration, may be used. Further, the motion model and parameters assumed in step ST1 and step ST2 may be different.
  • step ST3 the first coordinate conversion unit 13 converts the motion specification predicted value of the path coordinates into the motion specification predicted value of the observation coordinates. Also, the motion specification prediction error of the path coordinates is converted into the motion specification prediction error of the observation coordinates. In the conversion, the route data s i from the route data storage unit 12 is used.
  • k-1 (vert) , and s i are input,
  • k ⁇ 1 ( Whybs) of observation coordinates are calculated by the following formulas . (13)
  • the rotation matrices G j (para) and G j (vert) and the translation vector g j are defined as follows.
  • step ST4 the sensor unit 14 acquires the observation data, and the filtering unit 15 associates the observation data with the target vehicle 21.
  • the sensor unit 14 when observing the position of the target vehicle 21 with the sensor, in addition to the observation data representing the position of the target vehicle 21, erroneous observation data derived from other than the target vehicle 21 may be acquired. It can happen.
  • the sensor unit 14 is a vehicle observation sensor based on a visible light image such as a camera, the position of an object similar to a vehicle such as a signboard becomes erroneous observation data.
  • the sensor unit 14 is a vehicle observation sensor such as a radar based on the reflection intensity of electromagnetic waves, the position of a radio wave reflecting object such as a guardrail becomes erroneous observation data.
  • the observation data derived from the target vehicle 21 is not included in the observation data.
  • the sensor unit 14 is a camera
  • the observation data derived from the target vehicle 21 is not acquired when the target vehicle 21 viewed from the camera is hidden by the shield.
  • the sensor unit 14 is a radar
  • the reflected radio wave of the target vehicle 21 is temporarily buried in the signal noise, the observation data derived from the target vehicle 21 is not acquired.
  • this step ST4 by comparing the observation data acquired at the current time with the motion specification prediction value, whether the observation data derived from the target vehicle 21 exists, and if any, which observation Correspond to whether it is data.
  • the following Mahalanobis distance ⁇ representing the residual between the observation data and the predicted position is calculated for each observation data z k .
  • S k is an error covariance matrix regarding the residual between the predicted position of the target vehicle 21 and the observed data.
  • R is the error covariance matrix of the observation data z k, a parameter to be set in advance.
  • H is a matrix for extracting the position vector from the motion specifications in the observation coordinates. (24) It is defined as.
  • the filtering unit 15 calculates the motion parameter estimation value of the target vehicle 21 at the current time and the estimation error based on the observation data.
  • the observation data z k used here is the observation data associated with the target vehicle 21 in step ST4.
  • the motion parameter estimation value x k ( Wennbs) of the target vehicle 21 at the current time and the error covariance matrix P k ( réellebs) of the motion parameter estimation value are calculated by the following formulas. (25) (26)
  • H is the same as in the equation (24).
  • K k is defined as follows.
  • S k is the same as that in the equation (23), and the superscript “ ⁇ 1” represents an inverse matrix.
  • the observation data z k is an invalid value, it is considered that the target vehicle 21 is not observed at the current time, and the predicted value is set as the estimated value as follows.
  • step ST6 the second coordinate conversion unit 16 converts the motion specification estimated value of the observation coordinates into the motion specification estimated value of the route coordinates. Further, the motion specification estimation error of the observation coordinates is converted into the motion specification estimation value of the path coordinates.
  • the route data s i from the route data storage unit 12 is used.
  • x k ( specialistsbs) , P k ( réellebs) , and s i are input, and motion parameter estimated values x k (para) , x k (vert) and path coordinate motion are calculated by the following equations.
  • the specification estimation errors P k (para) and P k (vert) are calculated.
  • the rotation matrices G j (para) and G j (vert) have the same definitions as the equations (15) and (16) of step ST3, and the translation vector g j also has the same definition as the equation (17) of step ST3.
  • the method of calculating the serial number j of the route data is defined as j of 1 or more and I or less that satisfies the following conditions, with the center point sandwiching the target vehicle 21 in the front-rear direction at the position in the direction parallel to the route. (34) Note that the definition of ⁇ j is the same as the expression (20) in step ST3.
  • step ST7 the state determination unit 17 determines whether or not the target vehicle 21 meanders as the motion state of the target vehicle 21 based on the motion specification estimated value of the route coordinates and the estimation error.
  • the following Mahalanobis distance ⁇ k is calculated.
  • h is a matrix for extracting the position component from the motion specification in the direction vertical to the path, (36) was defined.
  • ⁇ k in the equation (35) indicates that the distance from the center of the route to the estimated position of the target vehicle 21 is significantly larger than the position estimation error. Therefore, if the time series graph of ⁇ k vibrates with an amplitude of a certain value or more, it is determined that the target vehicle 21 is meandering.
  • the amplitude of ⁇ k for example, the difference between the maximum value and the minimum value of ⁇ k between certain constant time frames is used as the amplitude, and ⁇ k is subjected to short-time Fourier transform to determine the peak value of the frequency component as the amplitude. And so on.
  • the presence or absence of meandering is determined based only on the motion parameters in the direction perpendicular to the route and the estimation error thereof, but the motion parameters in the direction parallel to the route and the estimation error thereof may be used. For example, depending on how large the position in the direction parallel to the route is compared with the estimation error, that is, how far the target vehicle 21 is from the sensor, the amplitude threshold value of ⁇ k for determining the meandering is continuously increased. You may do so.
  • the motion state determination device 100 configured as described above has the following effects.
  • the motion state determination device 100 according to the first embodiment sequentially calculates the error of the estimated values of the motion parameters, and determines the motion state of the target vehicle 21 based on this error.
  • the error of the observation data assumed in the filtering unit 15 is reflected in the error of the motion parameter estimation value used for the determination. That is, the state determination unit 17 determines the motion state of the target vehicle 21 based on the error of the observation data calculated using the predicted values of the motion specifications.
  • the motion parameters of the target vehicle 21 and the estimation error of the motion parameters are calculated for each of the component in the direction perpendicular to the route and the component in the direction parallel to the route, and these route coordinates are calculated.
  • the motion state of the target vehicle 21 is determined based on the above motion parameter estimation value and the estimation error value.
  • This effect is that when the route on which the target vehicle 21 travels is meandering, the meandering of the target vehicle 21 is determined based on, for example, motion parameter estimation values and estimation errors on a coordinate system with the direction as a reference.
  • the simple means cannot distinguish between the meandering along the path and the meandering in the direction deviating from the path, and the motion state determination device 100 according to the first embodiment can distinguish them. Appears as a remarkable effect.
  • the motion state determination device 100 predicts motion parameters in the direction parallel to the route and motion parameters in the direction perpendicular to the route.
  • an automobile or a two-wheeled vehicle accelerates and decelerates in a direction perpendicular to a route less frequently than an acceleration and deceleration in a direction parallel to the route. Therefore, by setting different motion models in the parallel direction and the vertical direction with respect to the route and the error of the different motion models, it becomes possible to more predict the motion values according to the actual motion of the target vehicle 21.
  • This effect makes it possible to reduce the prediction error of the motion parameters as compared with a simple means of predicting the motion parameters on the coordinate system with the direction as a reference, and as a result, the estimation error decreases and the estimation error It becomes possible to judge a minute change in the motion specification by comparison with, and finally, the error frequency of the motion state judgment can be reduced, which is a remarkable effect.
  • the sensor unit 14 that acquires the observation data of the target (for example, the target vehicle 21) and the route data that represents the shape of the route along which the target can move are stored.
  • the route data storage unit 12 for storing the route data
  • the route component prediction unit 1 for calculating the component of the target motion data and the component of the prediction error which are parallel to and perpendicular to the route, and the route component prediction unit 1.
  • a first coordinate conversion unit 13 for converting the predicted value and the prediction error of the calculated motion data into the same coordinate system as the first coordinate system representing the observation data; and the observation data acquired by the sensor unit 14.
  • the state determining unit 17 that determines the target motion state using the second coordinate conversion unit 16 that converts the target motion state and the estimated value and the estimated error of the target motion data converted by the second coordinate conversion unit 16. And are provided.
  • the present invention it is possible to reduce the frequency of erroneous determination of the target motion state due to the position variation of the target on the observation data due to the observation error.
  • the estimation error of the target motion parameter can be reduced by predicting the target motion parameter even if the observation data has an error.
  • the estimated motion values in the direction perpendicular to the route and its estimation error, and the motion parameters in the direction parallel to the route Since the state determination based on the time series data regarding the estimation error can be performed, the frequency of erroneous determination of the target motion state can be reduced.
  • the route component prediction unit 1 sets different motion models in the parallel direction and the vertical direction with respect to the route, and sets the motion model perpendicular to the component parallel to the route.
  • the feature is that each component is calculated.
  • Embodiment 2 In the first embodiment, the motion parameters in the direction parallel to the route are predicted based on one motion model, and the motion parameters in the direction perpendicular to the route are predicted based on the other motion model.
  • the target vehicle 21 can take a variety of motions, and the motions in the path direction and the vertical direction cannot be appropriately expressed by one motion model.
  • the target vehicle 21 when the route is congested by vehicles other than the target vehicle 21, the target vehicle 21 frequently performs acceleration/deceleration in the direction parallel to the route in order to adjust the inter-vehicle distance, but when the route is vacant, the target vehicle 21 There is a high possibility that the movement of the path in the direction parallel to will maintain a constant velocity.
  • the movement in the vertical direction of the route depends on the weight of the target vehicle 21 and the road surface environment. For example, when the target vehicle 21 is a large vehicle, acceleration/deceleration in the vertical direction of the route is less likely to occur than in a small vehicle. Further, the own vehicle 20 is not always able to acquire the congestion status, vehicle weight, road surface environment, etc. that characterize the motion of the target vehicle 21.
  • one type of motion model is used for each of the motion parameters in the direction parallel to the route and the motion parameters in the direction perpendicular to the route. If it is assumed and predicted, the prediction error becomes large, and finally, the motion state determination error based on the estimation error frequently occurs.
  • a plurality of predicted values having different motion models are calculated for the motion parameters in the direction parallel to the route and the motion parameters in the direction perpendicular to the route. Further, the estimated values of the motion parameters are calculated for the respective predicted values, and the motion state of the target vehicle 21 is determined based on the reasonable estimated value among them.
  • FIG. 8 is a block diagram showing the configuration of the exercise state determination device 100 according to the second embodiment.
  • the motion state determination device 100 according to the second embodiment includes a route parallel component prediction unit 10, a route vertical component prediction unit 11, a route data storage unit 12, a first coordinate conversion unit 13, and a sensor unit 14. , A filtering unit 15, a second coordinate conversion unit 16, and a state determination unit 17.
  • N pieces of the path parallel component prediction unit 10, the path vertical component prediction unit 11, the first coordinate conversion unit 13, the filtering unit 15, and the second coordinate conversion unit 16 are arranged in parallel (N is an integer of 2 or more).
  • the route data storage unit 12 and the sensor unit 14 are the same as those in the first embodiment.
  • the first coordinate conversion unit 13 and the second coordinate conversion unit 16 are different from the first embodiment in that N pieces are arranged in parallel, but the internal processing of each is the same as in the first embodiment.
  • n Is an integer of 1 or more and N or less.
  • the n-th route parallel component prediction unit 10 receives from the n-th second coordinate conversion unit 16 the route parallel component estimation value of the nth motion specification of the target vehicle 21 at the previous time and the estimation error of the route parallel component. , The route parallel component prediction value of the nth motion specification of the target vehicle 21 at the current time and the prediction error of the route parallel component are sent to the nth first coordinate conversion unit 13.
  • the n-th route vertical component prediction unit 11 receives from the n-th second coordinate conversion unit 16 the route vertical component estimation value of the nth motion specification of the target vehicle 21 at the previous time and the estimation error of the route vertical component. , The route vertical component prediction value of the nth motion specification of the target vehicle 21 at the current time and the prediction error of the route vertical component are sent to the nth first coordinate conversion unit 13.
  • the n-th first coordinate transformation unit 13 receives from the n-th route parallel component prediction unit 10 the route parallel component prediction value of the nth motion specification of the target vehicle 21 at the current time and the prediction error of the route parallel component.
  • a route vertical component prediction value of the nth motion specification of the target vehicle 21 at the current time and a route vertical component prediction error are received from the nth route vertical component prediction unit 11 and route data is stored from the route data storage unit 12. receive.
  • the predicted value of the n-th motion coordinate on the observation coordinate and the prediction error of the motion coordinate on the observation coordinate at the current time are sent to the n-th filtering unit 15.
  • the n-th filtering unit 15 calculates the predicted value of the n-th motion coordinate on the observed coordinate and the prediction error of the motion coordinate on the observed coordinate of the target vehicle 21 in the current time frame from the n-th first coordinate conversion unit 13. It receives and receives the observation data of the target vehicle 21 at the current time from the sensor unit 14. Then, the estimated value of the n-th motion coordinate on the observed coordinate of the target vehicle 21 at the current time and the estimation error of the motion coordinate on the observed coordinate are sent to the n-th second conversion unit. Further, the residual of the n-th predicted position of the target vehicle 21 at the current time and the observation data is sent to the state determination unit 17.
  • the n-th second coordinate conversion unit 16 receives from the n-th filtering unit 15 the estimated value of the n-th observation coordinate movement parameter of the target vehicle 21 at the current time and the estimation error of the observation coordinate movement parameter. , Receives route data from the route data storage unit 12. Then, the estimated value of the nth path coordinate motion specification of the target vehicle 21 at the current time and the estimation error of the path coordinate motion specification are sent to the state determination unit 17.
  • the n-th route parallel component prediction unit 10 is sent the route parallel component estimation value of the nth motion specification of the target vehicle 21 at the previous time and the estimation error of the route parallel component
  • the route vertical component estimation unit 11 sends the route vertical component estimation value of the nth motion specification of the target vehicle 21 at the previous time and the estimation error of the route vertical component.
  • the state determination unit 17 uses the 1st to Nth coordinate conversion units 16 to estimate the 1st to Nth movement parameters on the route coordinates of the target vehicle 21 at the current time and the estimated error of the movement parameters on the route coordinates. And the residuals between the observed data and the 1st to Nth predicted positions of the target vehicle 21 at the current time from the 1st to Nth filtering units 15. Then, the presence or absence of meandering of the target vehicle 21 at the current time is output as the output of the motion state determination device 100.
  • the hardware configuration according to the second embodiment is shown in FIG. 4 as in the first embodiment.
  • the ⁇ Nth filtering unit 15, the 1st to Nth second coordinate conversion unit 16, and the state determination unit 17 are stored in the recording device 42, and the 1st to Nth path parallel component prediction units 10 and 1 to Nth
  • the route data storage unit 12 is stored in the recording device 42.
  • the 1st to Nth path parallel component prediction units 10, the 1st to Nth path vertical component prediction units 11, the 1st to Nth first coordinate conversion units 13, and the 1st to Nth filtering units 15, 1 to N The second second coordinate conversion unit 16 and the state determination unit 17 write, read, and delete the input/output value and the processing intermediate data in the recording device 42 during execution.
  • FIG. 9 shows the 1st to Nth path parallel component prediction units 10, the 1st to Nth path vertical component prediction units 11, the 1st to Nth first coordinate conversion units 13, the sensor unit 14, and the 1st to Nth filtering.
  • 7 is a flowchart showing the operations of the unit 15, the 1st to N-th second coordinate conversion units 16, and the state determination unit 17 at the current time.
  • n-th path parallel component prediction unit 10 In preparation for the explanation of the operation, the n-th path parallel component prediction unit 10, the n-th path vertical component prediction unit 11, the n-th first coordinate conversion unit 13, the n-th filtering unit 15, and the n-th second coordinate.
  • a symbol relating to the nth motion specification of the target vehicle 21 calculated by the conversion unit 16 is defined (n is an integer of 1 or more and N or less).
  • the motion parameter estimation value of the target vehicle 21 at the n-th observation coordinate in the time frame k is represented by the following vector x k,n ( réellebs) .
  • x k,n ( réellebs, 1) is the lateral position of the target vehicle 21
  • x k, n ( réellebs, 2) is the vertical position of the target vehicle 21
  • x k, n ( réellebs, 3) is the target.
  • the lateral speed of the vehicle 21, x k,n ( Whybs, 4) represents an estimated value of the vertical speed of the target vehicle 21.
  • the motion parameter estimation value of the target vehicle 21 at the n-th route coordinate in the time frame k is a vector x k,n (para) of the following 2 rows and 1 column representing the motion parameter estimation value of the route parallel component.
  • a vector x k,n (vert) of the following 2 rows and 1 column representing the motion parameter estimation value of the path vertical component is defined as follows.
  • x k,n (para, 1) is the position of the target vehicle 21 in the direction parallel to the route
  • x k,n (para, 2) is the speed of the target vehicle 21 in the direction parallel to the route
  • x k,n (vert, 1) represents the position of the target vehicle 21 in the vertical direction of the route
  • x k,n (vert, 2) represents the estimated value of the speed of the target vehicle 21 in the vertical direction of the route.
  • the estimation error of each motion specification is also defined.
  • the error of the motion parameter estimation value vector x k,n ( 72bs) in the observation coordinates is expressed as P k,n ( réellebs) .
  • the errors of the motion parameter estimation value vectors x k,n (para) , x k,n (vert) in the path coordinates are expressed as P k,n (para) , P k,n (vert) , respectively.
  • step ST8 the exercise state determination device 100 selects an integer number from 1 to N that is not selected in the processing at the current time. It should be noted that the method of selecting which number among the unselected numbers is arbitrary. In the following description of steps ST9 to ST13, the process will be described when the number n is selected here.
  • the n-th path parallel component prediction unit 10 predicts the motion specifications of the n-th path parallel component at the current time, and calculates the error of the motion specification prediction value.
  • the n-th motion parameter estimation value x k,n (para) at the previous time and the estimation error P k,n (para) are input, and the path parallel position of the target vehicle 21 moves at a constant speed n.
  • k ⁇ 1,n (para) are calculated by the following equations. (40) (41)
  • Q k,n (para) is a 2-row, 2-column error covariance matrix that represents the error of the n-th motion model in the path parallel direction, and is a parameter that is set in advance.
  • ⁇ k,n (para) is a transition matrix for transitioning the motion specification vector from the previous time to the current time based on the n-th motion model in the path parallel direction, and is, for example, the following matrix. (42)
  • n (para) is a parameter representing the acceleration related to the path parallel component.
  • the n-th path vertical component prediction unit 11 predicts the motion specifications of the n-th path vertical component at the current time, and calculates the error of the motion specification prediction value.
  • the n-th motion parameter estimated value x k,n (vert) at the previous time and the estimated error P k,n (vert) are input, and the route vertical position of the target vehicle 21 moves at a constant speed.
  • k ⁇ 1,n (vert) are calculated by the following equations.
  • Q k,n (vert) is a 2-row, 2-column error covariance matrix that represents the error of the n-th motion model in the vertical direction of the path, and is a parameter that is set in advance.
  • ⁇ k,n (vert) is a transition matrix for transitioning the motion specification vector from the previous time to the current time based on the nth motion model in the vertical direction of the route, and is, for example, the following matrix.
  • n (vert) parameter representing the acceleration in the path the vertical component.
  • the motion model and parameters assumed in step ST9 and step ST10 may be different.
  • the nth first coordinate conversion unit 13 converts the nth motion specification predicted value of the path coordinates into the nth motion specification predicted value of the observation coordinates.
  • the n-th motion specification prediction error of the path coordinates is converted into the n-th motion specification prediction error of the observation coordinates.
  • the route data s i from the route data storage unit 12 is used.
  • serial number j of the route data in the above is defined as j of 1 or more and I or less, which is a center point between the front and rear of the target vehicle 21 and satisfies the following conditions.
  • ⁇ j is the same as the equation (20) in step ST3.
  • step ST12 the sensor unit 14 acquires the observation data, and the filtering unit 15 associates the observation data with the target vehicle 21.
  • the following Mahalanobis distance ⁇ n which represents the residual between the observation data and the predicted position, is calculated for each observation data z k .
  • S k,n is an error covariance matrix regarding the residual between the n-th predicted position of the target vehicle 21 and the observed data.
  • the observation data having the smallest ⁇ n is associated with the origin of the target vehicle 21, and the observation data is set as the observation data z k to be processed in the subsequent ST13. Further, if there is no observation data whose residual ⁇ n is less than the fixed value, or if there is no observation data acquired at the current time, an invalid value is substituted into the observation data processed in step ST13. .. In the second embodiment, the residual ⁇ n calculated here is also used in the exercise state determination in step ST16.
  • the n-th filtering unit 15 calculates the n-th motion parameter estimation value of the target vehicle 21 at the current time and the estimation error based on the observation data.
  • the observation data z k used here is the observation data associated with the target vehicle 21 in step ST4.
  • the error covariance matrix P k,n ( Whybs) of the n-th motion specification estimated value x k,n ( Whybs) of the target vehicle 21 and the motion specification estimated value at the current time is calculated by the following formula. (53) (54)
  • K k,n is defined as follows.
  • S k,n is the same as that in Expression (52).
  • the n-th second coordinate conversion unit 16 converts the n-th motion specification estimated value of the observation coordinates into the n-th motion specification estimated value of the path coordinates. Further, the n-th motion parameter estimation error of the observation coordinates is converted into the n-th motion parameter estimation value of the path coordinates. In the conversion, the route data s i from the route data storage unit 12 is used.
  • x k,n ( technicallybs) , P k,n ( réellebs) , and s i are input, and motion parameter estimated values x k,n (para) , x k,n of path coordinates are calculated by the following equations.
  • (Vert) and the motion parameter estimation errors P k,n (para) , P k,n (vert) of the path coordinates are calculated.
  • the rotation matrix G j (para) , G j (vert) and the translation vector g j are defined as in step ST6 of the first embodiment.
  • the calculation method of the serial number j of the route data is defined as j of 1 or more and I or less that satisfies the following conditions, with the center point sandwiching the target vehicle 21 in the front and rear direction at the position in the direction parallel to the route.
  • ⁇ j is the same as the equation (20) in step ST3.
  • step ST15 the exercise state determination device 100 determines whether or not there is an unselected integer number of 1 or more and less than N in the processing at the current time. If there is an unselected number, the process proceeds to step ST8, and if not, the process proceeds to step ST16.
  • the state determination unit 17 determines the motion state of the target vehicle 21 as the motion state of the target vehicle 21 based on the motion parameter estimation value and the estimation error at the N route coordinates, and the residual difference between the predicted position and the observation data. Determine whether there is meandering.
  • the following number n * is obtained using N residuals ⁇ n between the predicted position and the observed data calculated by the equation (51). (63)
  • the n * th predicted value that gives the smallest ⁇ n can be regarded as the predicted value calculated by the motion model that most closely matches the actual condition of the target vehicle 21 among the N types of motion models.
  • the following Mahalanobis distance ⁇ k,* is obtained. (64)
  • the larger the ⁇ k,* the more significant the position of the target vehicle 21 is from the center of the route. Therefore , if the time series graph of ⁇ k, * vibrates with an amplitude of a certain value or more, it is determined that the target vehicle 21 is meandering.
  • Delta k, * a in determining the amplitude for example, delta k between certain time frame, * the difference between the maximum value and the minimum value and the amplitude of the peak frequency components and short-time Fourier transform of the delta k A method such as using the value as the amplitude is used.
  • the presence or absence of meandering is determined based only on the motion parameters in the direction perpendicular to the route and the estimation error thereof, but the motion parameters in the direction parallel to the route and the estimation error thereof may be used.
  • the amplitude threshold value of ⁇ k, * for determining the meandering is continuously determined according to how large the position in the direction parallel to the route is compared with the estimation error, that is, how far the target vehicle 21 and the sensor are apart from each other. It may be set higher.
  • n * is calculated based on the magnitude of ⁇ n representing the residual, but an index representing the validity of the motion model, for example, an index such as AIC (Akaike Information Criteria) or BIC (Bayesian Information Criteria) is used. May be used to calculate n * .
  • steps ST9 to 14 are performed N times in order, but instead, steps ST9 to 14 are processed in parallel by a plurality of threads, and N path parallel component prediction units 10, Step ST16 may be executed after the processes of the path vertical component prediction unit 11, the first coordinate conversion unit 13, the filtering unit 15, and the second coordinate conversion unit 16 are completed.
  • the N path parallel component prediction units 10 and the N path vertical component prediction units 11 have N types of motions with different assumptions or parameter values of the motion model in prediction. Predicted specifications are calculated, motion parameters are estimated based on the predicted values, and the motion state of the target vehicle 21 is determined based on the predicted values and the prediction error, and the residual between the predicted values and the observed data. Configured as With this configuration, even when the target vehicle 21 can take various motions, the motion parameters based on the motion model based on the actual motion of the target vehicle 21 are predicted, and the motion parameters estimated from the predicted values are calculated. It is possible to determine the motion state based on this. It should be noted that calculating N kinds of motion parameter prediction values and prediction errors by the N path parallel component prediction units 10 is treated as a state in which the path parallel component prediction unit 10 calculates N sets of motion parameter prediction values. You can also
  • This effect is obtained by exercising from the motion specifications estimated based on one type of motion model when, for example, the attributes of the target vehicle 21 and the road surface state on which the target vehicle 21 travels are diverse and such information cannot be acquired. Compared with the case of determining the state, it is possible to determine the exercise state by using the motion parameters with less prediction error and estimation error. Manifests itself as an effect.
  • the path component prediction unit 1 is a component parallel to the path with respect to the predicted value and the prediction error of the N sets of the target motion parameters.
  • the first coordinate conversion unit 13 converts the predicted values and prediction errors of the N sets of motion parameters calculated by the path component prediction unit 1 into the first coordinate system.
  • the filtering unit 15 estimates the target motion data and the error of the target motion data for each of the N motion data prediction values and prediction errors converted by the first coordinate conversion unit 13.
  • the second coordinate conversion unit 16 converts the estimated values and the estimated errors of the N sets of target motion parameters calculated by the filtering unit 15 into the second coordinate system, respectively, and
  • the determination unit 17 is configured to determine the target motion state using the estimated values and the estimation error of the N sets of target motion parameters converted by the second coordinate conversion unit 16.
  • the path component prediction unit 1 sets different motion models for predicted values and prediction errors of N sets of the target motion parameters, It is characterized in that a component parallel to the path and a component perpendicular to the path are calculated. With this configuration, a motion model that conforms to the actual target motion can be selected from various motion models, and the frequency of erroneous motion state determination can be finally reduced.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Traffic Control Systems (AREA)
  • Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)
  • Testing And Monitoring For Control Systems (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

この発明の運動状態判定装置は、目標の観測データを取得するセンサ部と、目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部と、目標の運動諸元の予測値及び予測誤差の経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部と、経路成分予測部により算出された予測値及び予測誤差を、観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部と、観測データと経路データと第１座標変換部で変換された予測値及び予測誤差を用いて、目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部と、フィルタリング部で算出された推定値及び推定誤差を、経路に平行な成分と垂直な成分に変換する第２座標変換部と、その変換された推定値及び推定誤差を用いて、目標の運動状態を判定する状態判定部と、を備える。この構成によって、観測誤差によって目標の運動状態の判定を誤る頻度を低減できる。

Description

運動状態判定装置
　この発明は、目標の運動状態を判定する運動状態判定装置に関する。
　従来から、目標の運動状態を観測データから分類する方法する技術が知られている。ここで目標とは、車両、船舶、航空機、ロボット、人、自転車などの移動可能な物体と定義する。例えば特許文献１では、車両の位置を表す観測データから、車両の横方向の運動の向きが変化する点を抽出し、変化点間の距離が一定値以上ならば、その車両の運動状態は蛇行であると判定する装置が開示されている。
特開２０１４－６５７６号公報
　目標の観測データに誤差が含まれる場合、この観測誤差によって、運動状態の判定の誤りが頻発するという問題がある。特許文献１に記載された従来の技術では、観測誤差を含む観測データから車両の蛇行を判定するために、横位置の変化点の距離が一定値未満の場合、すなわち車両の横位置が細かく変化する場合は、その運動方向の変化は観測誤差によるものであるとみなし、その車両は蛇行していないと判定する。しかし一般に、観測を行うセンサから目標までの距離が遠いほど位置の観測誤差は大きくなる。そのため、特許文献１のように目標の位置のみによってのみ蛇行を判定する技術では、センサから遠い目標ほど、観測誤差を蛇行と判定する誤りが生じやすくなる。また、このような判定の誤りを減らすために、観測誤差が大きいことを前提とした場合、センサから近い目標ほど、目標の微小な蛇行を観測誤差と判定する誤りが生じやすくなる。
　本発明は上記の課題を解決するものであり、観測データに観測誤差が含まれる場合、特にセンサと目標との距離が変動し観測誤差が一定ではない環境においても、目標の運動状態に関する判定の誤り頻度を減らすことを目的とする。
　この発明に係る運動状態判定装置は、目標の観測データを取得するセンサ部と、前記目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部と、前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部と、前記経路成分予測部により算出された運動諸元の予測値及び予測誤差を、前記観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部と、前記センサ部で取得された観測データと、前記経路データ格納部に格納された経路データと、前記第１座標変換部で変換された運動諸元の予測値及び予測誤差を用いて、前記第１の座標系における前記目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部と、前記フィルタリング部により算出された前記目標の運動諸元の推定値及び推定誤差を、前記経路に平行な成分と垂直な成分を座標軸とする第２の座標系に変換する第２座標変換部と、前記第２座標変換部によって変換された前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する状態判定部と、を備えたことを特徴とする。
　本発明によれば、観測誤差による観測データ上の目標の位置変動によって目標の運動状態の判定を誤る頻度を減らすことができる。
実施の形態１に係る運動状態判定装置１００の構成を示すブロック図。 実施の形態１において対象車両２１に関する経路平行方向の位置と、経路垂直方向の位置を表した概念図。 実施の形態１において経路座標と観測座標との間の座標変換を表した概念図。 実施の形態１に係る運動状態判定装置１００のハードウェア構成を示すブロック図。 実施の形態１に係る運動状態判定装置１００の動作を示すフローチャート。 対象車両２１の観測座標における運動諸元ベクトルの例を示した概念図。 対象車両２１の経路座標における運動諸元ベクトルの例を示した概念図。 実施の形態２に係る運動状態判定装置１００の構成を示すブロック図。 実施の形態２に係る運動状態判定装置１００の動作を示すフローチャート。
　実施の形態１．
　以下、この発明の実施の形態について説明する。
　図１は、実施の形態１に係る運動状態判定装置１００の構成を示すブロック図である。この実施の形態では特に、目標の種類を自動車とし、目標の運動状態は「車両が蛇行している」「車両は蛇行していない」の２種類とする。また、運動状態を判定する対象の自動車は、観測を行うセンサを搭載している自動車（自車）から離れた位置を走行する自動車とする。なお、運動状態を判定する対象の自動車は自車であってもよい。以降、運動状態を判定する対象の目標を「対象車両」と記載し、対象車両の運動状態は「蛇行あり」「蛇行なし」の２種類である場合について説明する。
　運動状態判定装置１００は、図１に示すように、経路平行成分予測部１０、経路垂直成分予測部１１、経路データ格納部１２、第１座標変換部１３、センサ部１４、フィルタリング部１５、第２座標変換部１６、状態判定部１７を備える。ここで、経路平行成分予測部１０、経路垂直成分予測部１１はまとめて経路成分予測部１として扱える。運動状態判定装置１００は、センサから取得した観測データと、経路データ格納部１２に保存されている経路データを基に、対象車両２１の蛇行有無を判定する。
　ここで、観測データとは、対象車両２１の運動諸元（位置、速度およびそれらの時間微分量）を示した情報と定義する。以降では、観測データが対象車両２１の位置を表すデータである場合について説明する。また、経路データとは、対象車両２１の典型的な移動経路の形状を示した情報と定義する。以降では、経路データが、対象車両２１が走行している車線の形状を表すデータである場合について説明する。特に、車線の中央を通る線を点の集合として近似した場合における、各々の中央点の位置データを以降では経路データと呼ぶ。なお、目標は自動車の他に、船舶、航空機、ロボット、人、自転車などであってもよい。例えば目標が船舶の場合、観測データは船舶の位置を表す通信信号（自動船舶識別装置の信号など）とし、経路データは典型的な航路としてもよい。また例えば目標が人の場合、観測データは人を撮影した映像とし、経路データは歩道の形状としてもよい。
　続いて、運動状態判定装置１００の説明における用語として、「経路平行方向」と「経路垂直方向」を定義する。図２は、実施の形態１に係る運動状態判定装置１００における経路平行方向と経路垂直方向の一例を表す概念図である。
　まず経路平行方向の軸は、経路データである車線の中央点を前後に繋いだ線分の集合と定義する。そして、ある車線中央点の原点から、対象車両２１との距離が最小となる経路平行方向の軸上の点（経路座標軸交点）までの距離を、対象車両２１に関する経路平行方向の位置と定義する。また、この経路平行方向の位置およびその位置を時間微分した運動諸元を「運動諸元の経路平行方向成分」と呼ぶ。
　続いて、経路垂直方向の軸は、経路座標軸交点における経路平行方向の軸に対し垂直な直線と定義する。そして、経路座標軸交点から、経路垂直方向の軸と対象車両２１の位置が交わる点までの距離を、対象車両２１に関する経路垂直方向の位置と定義する。また、この経路垂直方向の位置およびその位置を時間微分した運動諸元を「運動諸元の経路垂直方向成分」と呼ぶ。
　上記の定義を踏まえ、運動状態判定装置１００の動作を図３を用いて説明する。図３は、実施の形態１に係る運動状態判定装置１００における、動作の一例を表す概念図である。図３左側は、センサを搭載した自車２０を基準とした座標系において、対象車両２１が蛇行している場合、蛇行に伴う観測データ２３が得られる様子を表す概念図である。図３左側において自車２０の進行方向に関する位置を「縦方向位置」、縦方向に対し垂直な方向の位置を「横方向位置」とし、観測データ２３はこれら縦方向、横方向の位置として表されるとする。以降では、この観測データ２３における座標系を「観測座標」と呼ぶ。
　一方、図３右側は、左側と同様に蛇行する対象車両２１の推定位置の時間変化を、経路平行方向および経路垂直方向において表した場合の概念図である。以降では、経路平行方向と経路垂直方向で運動諸元を表す座標系を「経路座標」と呼ぶ。また、図３中央の矢印で示すとおり、観測座標から経路座標への座標変換は「第１座標変換部１３」で行い、経路座標から観測座標への変換を「第２座標変換部１６」で行う。運動状態判定装置１００は、経路座標上において次の時刻フレームにおける対象車両２１の運動諸元の予測を行い、予測値を観測座標上へ変換したのちに、観測座標上において観測データ２３に基づく対象車両２１の運動諸元の推定（フィルタリング）を行う。そして再び座標変化によって推定値を経路座標へ変換し、経路座標上の推定値を基に対象車両２１の蛇行有無を判定する。
　このような動作により、例えば図３に示すように湾曲した経路に沿って対象車両２１が蛇行している場合においても、経路座標上では経路に直交した方向の蛇行が抽出できる。また、特に、運動諸元と併せて、推定された運動諸元の誤差（推定誤差）も座標変換し、この推定誤差を用いて蛇行有無を判定する点が、運動状態推定装置の特徴である。
　以下、図１における各機能ブロック間の関係を示す。なお、以降では、センサが観測データを出力する時間の区切りを「時刻フレーム」と記載する。また処理を実行する時刻フレームを「現時刻」と記載し、現時刻の１時刻フレーム前を「前時刻」と記載する。
　経路成分予測部１における経路平行成分予測部１０は、第２座標変換部１６から前時刻における対象車両２１の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を受け取り、現時刻における対象車両２１の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差を第１座標変換部１３へ送る。
　経路成分予測部１における経路垂直成分予測部１１は、第２座標変換部１６から前時刻における対象車両２１の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を受け取り、現時刻における対象車両２１の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差を第１座標変換部１３へ送る。
　経路データ格納部１２は、経路データを第１座標変換部１３および第２座標変換部１６が実行される際に、経路データを第１座標変換部１３および第２座標変換部１６へ送る。なお、経路データは予め保存されたものでも、処理に用いる際に、その都度、通信手段などによって取得して保存してもよいとする。経路データを取得する通信手段としては、例えば測位衛星との通信などの既存の技術を用いてもよい。
　第１座標変換部１３は、経路平行成分予測部１０から現時刻における対象車両２１の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差を受け取り、経路垂直成分予測部１１から現時刻における対象車両２１の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、現時刻における対象車両２１の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差をフィルタリング部１５へ送る。
　センサ部１４は、対象車両２１の位置を観測するための機器から構成される。例えば、単眼カメラ、ステレオカメラ、ミリ波レーダ、レーザセンサ、無線信号受信器などの既存の機器である。センサ部１４は現時刻における対象車両２１の観測データを、フィルタリング部１５へ送る。なお、観測機器が取得した画像や測距値などから、観測データ（対象車両２１の位置など）を取得する手段は、例えば映像から車両の特徴的な形状を抽出するアルゴリズムなどの、観測機器ごとの既存技術を用いる。
　フィルタリング部１５は、第１座標変換部１３から現時刻フレームにおける対象車両２１の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差を受け取り、センサ部１４から現時刻における対象車両２１の観測データを受け取る。そして、現時刻における対象車両２１の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差を第２変換部へ送る。
　第２座標変換部１６は、フィルタリング部１５から現時刻における対象車両２１の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、状態判定部１７へ現時刻における対象車両２１の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を送る。また次の時刻フレームにおいて、経路平行成分予測部１０に前時刻における対象車両２１の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を送り、経路垂直成分予測部１１に前時刻における対象車両２１の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を送る。
　状態判定部１７は、第２座標変換部１６から現時刻における対象車両２１の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を受け取り、現時刻における対象車両２１の蛇行の有無を運動状態判定装置１００の出力として出力する。
　図４は、実施の形態１に係るハードウェア構成の一例を示す図である。各ハードウェアはバス等の有線回路またはネットワークによって互いに繋がっている。運動状態判定装置１００における、経路平行成分予測部１０、経路垂直成分予測部１１、経路データ格納部１２、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６、状態判定部１７は記録装置４２に格納されており、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６、状態判定部１７は演算装置４１によって実行される。
　また、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６、状態判定部１７は、実行中に記録装置４２へ入出力値および処理中間データの書き込み、読み出し、削除を行う。演算装置４１はＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）、ＥＣＵ（Ｅｌｅｃｔｒｏｎｉｃ　Ｃｏｎｔｒｏｌ　Ｕｎｉｔ）またはＧＰＵ（Ｇｒａｐｈｉｃｓ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）等である。記録装置４２はＨＤＤ（Ｈａｒｄ　Ｄｉｓｃｋ　Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ　Ｓｔａｔｅ　Ｄｒｉｖｅ）、ＤＲＡＭ（Ｄｙｎａｍｉｃ　Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）、またはフラッシュメモリ等である。出力インターフェース４３はＤＶＩ（Ｄｉｇｉｔａｌ　Ｖｉｓｕａｌ　Ｉｎｔｅｒｆａｃｅ、登録商標）、ＨＤＭＩ（Ｈｉｇｈ－Ｄｅｆｉｎｉｔｉｏｎ　Ｍｕｌｔｉｍｅｄｉａ　Ｉｎｔｅｒｆａｃｅ、登録商標）、ＵＳＢ（Ｕｎｉｖｅｒｓａｌ　Ｓｅｒｉａｌ　Ｂｕｓ）、イーサネット（登録商標）、ＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ　Ａｒｅａ　Ｎｅｔｗｏｒｋ）バス等である。
　次に、図５のフローチャートを用いて、実施の形態１に係る運動状態判定装置１００の動作の詳細を説明する。図５は、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、センサ部１４、フィルタリング部１５、第２座標変換部１６、状態判定部１７の、現時刻における動作を示したフローチャートである。
　動作の説明の準備として、観測データを表す位置ベクトル、経路データを表す位置ベクトル、対象車両２１の運動諸元の推定値を表すベクトル、運動諸元の推定誤差を表す行列を定義する。観測データは、ここでは前記のとおり、対象車両２１の位置を表すデータとする。以降では、あるｋ番目の時刻フレームにおける対象車両２１の位置を表す観測データを、式（１）で定義する２行１列の位置ベクトルｚｋで表す。

Figure JPOXMLDOC01-appb-I000001
 　　　　　　　　　　　　　　（１）

ここで、ｚｋは観測座標上での位置ベクトルとし、ｚｋ （１）は対象車両２１の横位置を表す観測データ、ｚｋ （２）は対象車両２１の縦位置を表す観測データとする。また、Tの上添字は行列の転置を表す。
　経路データは、ここでは前記のとおり、対象車両２１が走行している車線の中央点の位置を表すデータとする。以降では、対象車両２１が走行している車線のｉ番目の中央点の位置を、式（２）で定義する２行１列の位置ベクトルｓｉで表す。

Figure JPOXMLDOC01-appb-I000002
　　　　　　　　　　　　　（２）

ここで、ｓｉは観測座標上での位置ベクトルとし、ｓｉ （１）は中央点の横位置を表し、ｓｉ （２）は中央点の縦位置を表す。各中央点の通し番号であるｉは１以上Ｉ以下（Ｉは２以上の整数）とし、経路データはＩ個の位置ベクトルｓｉによって表されるとする。なお、各中央点の通し番号であるｉは、縦位置の昇順に並べ替えられているとする。すなわち、任意の通し番号ｉに対して、ｓｉ （２）＜ｓｉ＋１ （２）が常に成り立つとする。
　また、経路データである車線の中央点は、自車２０から対象車両２１までの距離に比べて、十分に遠方まで取得されており、いずれかの中央点の縦方向の中間に他車が観測されるものとする。すなわち、各中央点の通し番号ｉが１以上Ｉ以下の場合（Ｉは２以上の整数）、任意の第ｋ時刻フレームにおいて以下が常に成立することを前提とする。

Figure JPOXMLDOC01-appb-I000003
　　 　　　　　　　　　　　 （３）

　続いて、対象車両２１の運動諸元の推定値を表すベクトルを、観測座標と経路座標についてそれぞれ定義する。まず時刻フレームｋでの観測座標における対象車両２１の運動諸元推定値は、以下の４行１列のベクトルｘ（оｂｓ） ｋで表す。

Figure JPOXMLDOC01-appb-I000004
　　　　　　　　　　　　　　　　　　　　　 　　　　　　　　　　　（４）


ここで、ｘｋ （оｂｓ、１）は対象車両２１の横方向位置、ｘｋ （оｂｓ、２）は対象車両２１の縦方向位置、ｘｋ （оｂｓ、３）は対象車両２１の横方向速度、ｘｋ （оｂｓ、４）は対象車両２１の縦方向速度の推定値を表す。
　続いて、時刻フレームｋでの経路座標における対象車両２１の運動諸元推定値は、経路平行成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ （ｐａｒａ）と、経路垂直成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ （ｖｅｒｔ）で表す。それぞれ以下のとおり定義する。

Figure JPOXMLDOC01-appb-I000005
　　　　　　　　　（５）

Figure JPOXMLDOC01-appb-I000006
  　　　　　　　　　（６）

ここで、ｘｋ （ｐａｒａ、１）は対象車両２１の経路平行方向の位置、ｘｋ （ｐａｒａ、２）は対象車両２１の経路平行方向の速度、ｘｋ （ｖｅｒｔ、１）は対象車両２１の経路垂直方向の位置、ｘｋ （ｖｅｒｔ、２）は対象車両２１の経路垂直方向の速度の推定値を表す。
　上記のように定義した運動諸元推定値のベクトルについて、その概念図を図６と図７に示す。図６では観測座標における運動諸元推定値ｘｋ （оｂｓ）の位置成分を、図７では経路座標における運動諸元推定値ｘｋ （ｐａｒａ）、ｘｋ （ｖｅｒｔ）の位置成分を示す。ただし、図７の経路座標に関しては、図示を簡略化するために経路データｓｉを経路平行方向の原点として図示した。
　次に、運動諸元の推定誤差を表す行列について定義する。推定誤差は、運動諸元を表すベクトルの誤差共分散行列の形で表す。以降では、観測座標における運動諸元推定値ベクトルｘｋ （оｂｓ）の誤差はＰｋ （оｂｓ）と表す。また、経路座標における運動諸元推定値ベクトルｘｋ （ｐａｒａ）、ｘｋ （ｖｅｒｔ）の誤差はそれぞれＰｋ （ｐａｒａ）、Ｐｋ （ｖｅｒｔ）と表す。なお、ここでは、例として運動諸元を位置および速度の２種類とした。しかし運動諸元は必ずしも位置と速度の２種類でなくてもよく、例えば加速度、ジャーク成分などを加えた３種類以上の運動諸元を用いてもよいものとする。
　以下、図５の各ステップについて説明する。ステップST１では、経路平行成分予測部１０が、現時刻における経路平行成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。このステップでは、前時刻における運動諸元推定値ｘｋ （ｐａｒａ）と推定誤差Ｐｋ （ｐａｒａ）を入力として、対象車両２１の経路平行位置が等速移動するという運動モデルに基づき、以下の式により運動諸元予測値ｘｋ｜ｋ－１ （ｐａｒａ）と予測誤差Ｐｋ｜ｋ－１ （ｐａｒａ）を算出する。

Figure JPOXMLDOC01-appb-I000007
　　　　　　　（７）

Figure JPOXMLDOC01-appb-I000008
　　（８）

ここで、Ｑｋ （ｐaｒa）は、運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ （ｐaｒa）は、運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、以下の行列とする。

Figure JPOXMLDOC01-appb-I000009
　　　　　　　　（９）

ここで、τｋは前時刻から現時刻までの経過時間を表す。なお、上記の式では運動モデルとして等速移動を仮定したが、他の運動モデル、例えば特定の加速度による等加速度運動モデルなどとしてもよい。
　ステップST２では、経路垂直成分予測部１１が、現時刻における経路垂直成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。このステップでは、前時刻における運動諸元推定値ｘｋ （ｖｅｒｔ）と推定誤差Ｐｋ （ｖｅｒｔ）を入力として、対象車両２１の経路垂直位置が等速移動するという運動モデルに基づき、以下の式により運動諸元予測値ｘｋ｜ｋ－１ （ｖｅｒｔ）と予測誤差Ｐｋ｜ｋ－１ （ｖｅｒｔ）を算出する。

Figure JPOXMLDOC01-appb-I000010
　　　　　　（１０）

Figure JPOXMLDOC01-appb-I000011
　（１１）

　ここで、Qｋ （ｖｅｒｔ）は、運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ （ｖｅｒｔ）は、運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、以下の行列とする。

Figure JPOXMLDOC01-appb-I000012
　　　　　　　（１２）

なお、上記の式では運動モデルとして等速移動を仮定したが、他の運動モデル、例えば特定の加速度による等加速度運動モデルなどとしてもよい。また、ステップＳＴ１とステップＳＴ２が仮定する運動モデルおよびパラメタは異なっていてもよい。
　ステップST３では、第１座標変換部１３が、経路座標の運動諸元予測値から観測座標の運動諸元予測値へ変換する。また、経路座標の運動諸元予測誤差から、観測座標の運動諸元予測誤差へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。このステップでは、ｘｋ｜ｋ－１ （ｐａｒａ）、Ｐｋ｜ｋ－１ （ｐａｒａ）、ｘｋ｜ｋ－１ （ｖｅｒｔ）、Ｐｋ｜ｋ－１ （ｖｅｒｔ）、ｓｉを入力として、以下の式により観測座標の運動諸元予測値ｘｋ｜ｋ－１ （оｂｓ）と、観測座標の運動諸元予測誤差Ｐｋ｜ｋ－１ （оｂｓ）を算出する。

Figure JPOXMLDOC01-appb-I000013
　　　　　（１３）

Figure JPOXMLDOC01-appb-I000014
　　 （１４）

ここで、回転行列Ｇｊ （ｐａｒａ）、Ｇｊ （ｖｅｒｔ）と、並進ベクトルｇｊは以下のとおり定義する。

Figure JPOXMLDOC01-appb-I000015
　　　　（１５）

Figure JPOXMLDOC01-appb-I000016
　　　　　（１６）

Figure JPOXMLDOC01-appb-I000017
　　　　　（１７）

上記の式における、経路データを構成する線分に平行な単位ベクトルｅｊ （ｐａｒａ）と、ｅｊ （ｐａｒａ）に垂直な単位ベクトルｅｊ （ｖｅｒｔ）と、経路方向原点からｓｊまでの経路方向の長さλｊは以下のとおり算出する。

Figure JPOXMLDOC01-appb-I000018
　　（１８）

Figure JPOXMLDOC01-appb-I000019
　　　　 （１９）

Figure JPOXMLDOC01-appb-I000020
　　　　　 （２０）

ベクトルに対する絶対値記号（｜｜）はベクトルのユークリッド距離と定義する。なお、上記における経路データの通し番号ｊは、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する。

Figure JPOXMLDOC01-appb-I000021
　　 　 　（２１）
　ステップST４では、センサ部１４が観測データを取得し、フィルタリング部１５が観測データと対象車両２１との対応付けを行う。センサの種類を問わず一般に、対象車両２１の位置をセンサにより観測する場合、対象車両２１の位置を表す観測データの他に、対象車両２１以外に由来する誤った観測データが取得される事態が生じうる。例えばセンサ部１４がカメラなどの可視光画像に基づく車両観測センサである場合、看板など車両に似た物体の位置が誤った観測データとなる。また例えばセンサ部１４がレーダなどの電磁波の反射強度に基づく車両観測センサである場合、ガードレール等の電波反射物体の位置が誤った観測データとなる。
　またセンサの種類を問わず一般に、対象車両２１の位置をセンサにより観測する場合、観測データの中に対象車両２１に由来する観測データが含まれていない事態が生じうる。例えばセンサ部１４がカメラの場合、遮蔽物によってカメラから見た対象車両２１が隠れた際に、対象車両２１に由来する観測データは取得されない。また例えばセンサ部１４がレーダの場合、対象車両２１の反射電波が一時的に信号雑音に埋もれた際に、対象車両２１に由来する観測データは取得されない。
　上記の理由により、このステップＳＴ４では、現時刻に取得した観測データと、運動諸元予測値との比較により、対象車両２１に由来する観測データが存在するか、また存在するならばいずれの観測データであるかの対応付けを行う。対応付けにおいては、各々の観測データｚｋに対して、観測データと予測位置との残差を表す以下のマハラノビス距離δを計算する。

Figure JPOXMLDOC01-appb-I000022
  　（２２）

ここでＳｋは対象車両２１の予測位置と観測データとの残差に関する誤差共分散行列であり

Figure JPOXMLDOC01-appb-I000023
　　（２３）

より算出する。ここでＲは観測データｚｋの誤差共分散行列であり、あらかじめ設定するパラメタである。またＨは、観測座標における運動諸元から位置ベクトルを取り出すための行列であり

Figure JPOXMLDOC01-appb-I000024
　　　　（２４）

と定義する。
　式（２２）により算出した残差δが小さいほど、その観測データは対象車両２１の予測値と整合していることを表すため、複数の観測データｚｋのうち、δが最小となる観測データを対象車両２１に由来すると対応付け、その観測データをステップＳＴ５で処理する観測データｚｋとする。また、前記のように対象車両２１に由来する観測データが１つも取得されなかった場合も起こりうるため、残差δが一定値未満の観測データが１つもなかった場合、または現時刻で取得した観測データが０個であった場合、ステップＳＴ５で処理する観測データには無効値を代入する。
　ステップST５では、フィルタリング部１５が、観測データに基づき現時刻における対象車両２１の運動諸元推定値と、推定誤差を算出する。ここで用いる観測データｚｋは、ステップＳＴ４において対象車両２１と対応付けられた観測データとする。
　現時刻における対象車両２１の運動諸元推定値ｘｋ （оｂｓ）と運動諸元推定値の誤差共分散行列Ｐｋ （оｂｓ）は、以下の式により算出する。

Figure JPOXMLDOC01-appb-I000025
　  （２５）


Figure JPOXMLDOC01-appb-I000026
　　　　（２６）

ここで、Ｈは式（２４）と同じとする。また、Ｋｋは以下のとおり定義する。

Figure JPOXMLDOC01-appb-I000027
　　　　　　　（２７）

ここで、Ｓｋは式（２３）と同じとし、上添字の「－１」は逆行列を表すとする。なお、観測データｚｋが無効値であった場合、現時刻では対象車両２１が観測されなかったとみなし、以下のように予測値を推定値とする。

Figure JPOXMLDOC01-appb-I000028
　　　　　　　　　（２８）

Figure JPOXMLDOC01-appb-I000029
　　　　　　　   （２９）　
　ステップST６では、第２座標変換部１６が、観測座標の運動諸元推定値から経路座標の運動諸元推定値へ変換する。また、観測座標の運動諸元推定誤差から、経路座標の運動諸元推定値へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。
　このステップでは、ｘｋ （оｂｓ）、Ｐｋ （оｂｓ）、ｓｉを入力として、以下の式により経路座標の運動諸元推定値ｘｋ （ｐａｒａ）、ｘｋ （ｖｅｒｔ）および経路座標の運動諸元推定誤差Ｐｋ （ｐａｒａ）、Ｐｋ （ｖｅｒｔ）を算出する。

Figure JPOXMLDOC01-appb-I000030
　　（３０）

Figure JPOXMLDOC01-appb-I000031
 　　（３１）

Figure JPOXMLDOC01-appb-I000032
　　（３２）

Figure JPOXMLDOC01-appb-I000033
 　（３３）

ここで、回転行列回転行列Ｇｊ （ｐａｒａ）、Ｇｊ （ｖｅｒｔ）はステップＳＴ３の式（１５）、（１６）と同じ定義とし、並進ベクトルｇｊもステップＳＴ３の式（１７）と同じ定義とする。ただし、経路データの通し番号ｊの算出方法は、経路平行方向の位置において、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する。

Figure JPOXMLDOC01-appb-I000034
　　　　　（３４）

なお、λｊの定義はステップＳＴ３の式（２０）と同じとする。
　ステップST７では、状態判定部１７が、経路座標の運動諸元推定値と推定誤差を基に、対象車両２１の運動状態として対象車両２１の蛇行有無を判定する。判定にあたり、以下のマハラノビス距離Δｋを算出する。

Figure JPOXMLDOC01-appb-I000035
　　（３５）

ここでｈは、経路垂直方向の運動諸元から位置成分を取り出すための行列であり、

Figure JPOXMLDOC01-appb-I000036
　　　　　　　（３６）

と定義した。
　式（３５）のΔｋが大きいほど、経路の中心から対象車両２１推定位置までの距離は、位置の推定誤差に比べ有意に離れていることを表している。したがってΔｋの時系列グラフが、ある一定以上の振幅をもって振動しているならば、対象車両２１は蛇行していると判定する。Δｋの振幅を判定する際には、例えばある一定時刻フレーム間でのΔｋの最大値と最小値の差を振幅とする、Δｋを短時間フーリエ変換し周波数成分のピーク値を振幅とする、などの方法を用いる。
　なお、上記では経路垂直方向の運動諸元とその推定誤差のみに基づき蛇行の有無を判定したが、経路平行方向の運動諸元とその推定誤差を用いてもよい。例えば、経路平行方向の位置がその推定誤差に比べどの程度大きいか、すなわち対象車両２１とセンサがどれだけ離れているかに応じて、蛇行を判定するΔｋの振幅しきい値を連続的に高くするなどとしてもよい。
　以上のように構成された実施の形態１に係る運動状態判定装置１００では、次の効果が得られる。まず初めに、実施の形態１に係る運動状態判定装置１００では、運動諸元の推定値の誤差を逐次算出し、この誤差に基づき対象車両２１の運動状態を判定する。判定に用いられる運動諸元推定値の誤差には、フィルタリング部１５において想定した観測データの誤差が反映されている。すなわち状態判定部１７では、運動諸元の予測値を用いて算出された観測データの誤差に基づき対象車両２１の運動状態を判定している。この構成により、観測データの誤差による運動諸元の変動と、真の運動諸元の変動との区別が可能となる。この効果は、特許文献１のような対象車両２１の位置のみに基づき運動状態を判定する従来技術と比べ、対象車両２１がセンサより遠方を走行しており観測データの誤差が大きい場合において、観測誤差による観測データ上の対象車両２１の位置変動が、対象車両２１の蛇行として誤って判定される事態の頻度を減らすことができる点で、顕著な効果として現れる。
　さらに、実施の形態１に係る運動状態判定装置１００では、対象車両２１の運動諸元と運動諸元の推定誤差を、経路垂直方向の成分と経路平行方向の成分についてそれぞれ算出し、これら経路座標上の運動諸元推定値と推定誤差の値に基づき対象車両２１の運動状態を判定する。この構成により、対象車両２１が走行する経路が任意の形状に湾曲している場合においても、経路に対して垂直な方向の運動諸元推定値とその推定誤差、経路に対して平行な方向の運動諸元とその推定誤差に関する時系列データに基づく状態判定が可能となる。この効果は、対象車両２１の走行する経路自体が蛇行している場合において、例えば方角を基準とした座標系上での運動諸元推定値と推定誤差に基づき対象車両２１の蛇行を判定するという単純な手段と比較すると、単純な手段では経路に沿った蛇行と経路から逸れた方向の蛇行とが区別できず、一方の実施の形態１に係る運動状態判定装置１００ではこれらを区別できる点で顕著な効果として現れる。
　さらに、実施の形態１に係る運動状態判定装置１００では、経路に対して平行方向の運動諸元と、経路に対して垂直方向の運動諸元をそれぞれ予測する。一般に自動車や二輪車などは、経路に対し平行な方向の加速減速に比べ、経路に対し垂直な方向に加速減速する頻度は少ない。従って、経路に対して平行方向と垂直方向で異なる運動モデル、異なる運動モデルの誤差を設定することで、より実際の対象車両２１の運動に則した運動諸元の予測値が可能となる。この効果は、例えば方角を基準とした座標系上において運動諸元を予測する単純な手段に比べ、運動諸元の予測誤差を低下させることが可能となり、その結果推定誤差が低下し、推定誤差との比較による運動諸元の微小な変動に対する判定が可能となり、最終的に運動状態判定の誤り頻度を低下できる点で、顕著な効果として現れる。
このように、実施の形態１に係る運動状態判定装置１００では、目標（例えば、対象車両２１）の観測データを取得するセンサ部１４と、前記目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部１２と、前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部１と、前記経路成分予測部１により算出された運動諸元の予測値及び予測誤差を、前記観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部１３と、前記センサ部１４で取得された観測データと、前記経路データ格納部１２に格納された経路データと、前記第１座標変換部１３で変換された運動諸元の予測値及び予測誤差を用いて、前記目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部１５と、前記フィルタリング部１５により算出された前記目標の運動諸元の推定値及び推定誤差を、前記経路に平行な成分と垂直な成分を座標軸とする第２の座標系に変換する第２座標変換部１６と、前記第２座標変換部１６によって変換された前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する状態判定部１７と、を備えたことを特徴とする。本発明によれば、観測誤差による観測データ上の目標の位置変動によって、目標の運動状態の判定を誤る頻度を減らすことができる。特に、本発明では、観測データに誤差がある場合であっても目標の運動諸元の予測を行うことによって、目標の運動諸元の推定誤差を小さくすることができる。その結果、観測データのみを用いる従来技術に対して、目標の運動状態の判定を誤る頻度を減らすことができる。また、目標が走行する経路が任意の形状に湾曲している場合においても、経路に対して垂直な方向の運動諸元推定値とその推定誤差、経路に対して平行な方向の運動諸元とその推定誤差に関する時系列データに基づく状態判定が可能となるため、目標の運動状態の判定を誤る頻度を減らすことができる。
　また、実施の形態１に係る運動状態判定装置１００では、経路成分予測部１は、前記経路に対して平行方向と垂直方向で異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分をそれぞれ算出することを特徴とする。この構成を用いることによって、より実際の対象車両２１の運動に則した運動諸元の予測値が可能となる。
　実施の形態２．
　実施の形態１では、１つの運動モデルに基づき経路平行方向の運動諸元を予測し、もう１つの運動モデルに基づき経路垂直方向の運動諸元を予測する。しかし、対象車両２１がとりうる運動は多様であり、経路方向と垂直方向の運動がそれぞれ１種類の運動モデルによって妥当に表現できるとは限らない。
　例えば、対象車両２１以外の車両によって経路上が混雑している場合、車間を調整するために対象車両２１は経路平行方向の加速減速を頻発させるが、経路上が空いている場合、対象車両２１の経路平行方向の運動は等速を維持する可能性が高い。また、経路垂直方向の運動は、対象車両２１の重量や路面環境などに依存し、例えば対象車両２１が大型車両の場合は小型車両に比べ経路垂直方向の加速減速は生じにくい。また、対象車両２１の運動を特徴づける、混雑状況や車両重量、路面環境などは自車２０が常に取得可能とは限らない。従って、対象車両２１の属性や路面環境などが多様であり、かつそれらの条件が取得できない場合において、経路平行方向の運動諸元と経路垂直方向の運動諸元を、それぞれ１種類の運動モデルを仮定し予測すると、予測誤差が大きくなり、最終的に推定誤差に基づく運動状態の判定誤りが頻発する。
　そこで、実施の形態２では、経路平行方向の運動諸元と、経路垂直方向の運動諸元について、運動モデルが異なる複数の予測値をそれぞれ算出する。また、それぞれの予測値について運動諸元の推定値を算出し、それらのうち妥当な推定値に基づき対象車両２１の運動状態を判定する。この構成により、対象車両２１の運動モデルが１種類で表現できないほど多様な場合においても、運動状態の判定を誤る頻度を減らすことができる。
　図８は、実施の形態２に係る運動状態判定装置１００の構成を示すブロック図である。図８に示すように、実施の形態２に係る運動状態判定装置１００は、経路平行成分予測部１０、経路垂直成分予測部１１、経路データ格納部１２、第１座標変換部１３、センサ部１４、フィルタリング部１５、第２座標変換部１６、状態判定部１７から構成される。ただし、経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６はそれぞれＮ個が並列されている（Ｎは２以上の整数）。ここで、経路データ格納部１２、センサ部１４は実施の形態１と同じである。また、第１座標変換部１３、第２座標変換部１６は、Ｎ個が並列されている点が実施の形態１と異なるが、それぞれの内部処理は実施の形態１と同じである。
　以下、図８における各機能ブロック間の関係を示す。なお、Ｎ個存在する経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６に関しては、ｎ番目の機能ブロックに関して説明する（ｎは１以上Ｎ以下の整数）。
　ｎ番目の経路平行成分予測部１０は、ｎ番目の第２座標変換部１６から前時刻における対象車両２１のｎ番目の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を受け取り、現時刻における対象車両２１のｎ番目の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差をｎ番目の第１座標変換部１３へ送る。
　ｎ番目の経路垂直成分予測部１１は、ｎ番目の第２座標変換部１６から前時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を受け取り、現時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差をｎ番目の第１座標変換部１３へ送る。
　ｎ番目の第１座標変換部１３は、ｎ番目の経路平行成分予測部１０から現時刻における対象車両２１のｎ番目の運動諸元の経路平行成分予測値と、経路平行成分の予測誤差を受け取り、ｎ番目の経路垂直成分予測部１１から現時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分予測値と、経路垂直成分の予測誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、現時刻における対象車両２１のｎ番目の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差をｎ番目のフィルタリング部１５へ送る。
　ｎ番目のフィルタリング部１５は、ｎ番目の第１座標変換部１３から現時刻フレームにおける対象車両２１のｎ番目の観測座標上運動諸元の予測値と、観測座標上運動諸元の予測誤差を受け取り、センサ部１４から現時刻における対象車両２１の観測データを受け取る。そして、現時刻における対象車両２１のｎ番目の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差をｎ番目の第２変換部へ送る。また、現時刻における対象車両２１のｎ番目の予測位置と観測データとの残差を、状態判定部１７へ送る。
　ｎ番目の第２座標変換部１６は、ｎ番目のフィルタリング部１５から現時刻における対象車両２１のｎ番目の観測座標上運動諸元の推定値と、観測座標上運動諸元の推定誤差を受け取り、経路データ格納部１２から経路データを受け取る。そして、状態判定部１７へ現時刻における対象車両２１のｎ番目の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を送る。また次の時刻フレームにおいて、ｎ番目の経路平行成分予測部１０に前時刻における対象車両２１のｎ番目の運動諸元の経路平行成分推定値と、経路平行成分の推定誤差を送り、ｎ番目の経路垂直成分予測部１１に前時刻における対象車両２１のｎ番目の運動諸元の経路垂直成分推定値と、経路垂直成分の推定誤差を送る。
　状態判定部１７は、１～Ｎ番目の第２座標変換部１６から現時刻における対象車両２１の１～Ｎ番目の経路座標上運動諸元の推定値と、経路座標上運動諸元の推定誤差を受け取り、また、１～Ｎ番目のフィルタリング部１５から、現時刻における対象車両２１の１～Ｎ番目の予測位置と観測データとの残差を受けとる。そして、現時刻における対象車両２１の蛇行の有無を運動状態判定装置１００の出力として出力する。
　実施の形態２に係るハードウェア構成は、実施の形態１と同様、図４にて示す。実施の形態２に係る運動状態判定装置１００における、１～Ｎ番目の経路平行成分予測部１０、１～Ｎ番目の経路垂直成分予測部１１、１～Ｎ番目の第１座標変換部１３、１～Ｎ番目のフィルタリング部１５、１～Ｎ番目の第２座標変換部１６、状態判定部１７は記録装置４２に格納されており、１～Ｎ番目の経路平行成分予測部１０、１～Ｎ番目の経路垂直成分予測部１１、１～Ｎ番目の第１座標変換部１３、１～Ｎ番目のフィルタリング部１５、１～Ｎ番目の第２座標変換部１６、状態判定部１７は演算装置４１によって実行される。また、経路データ格納部１２は記録装置４２に格納されている。
　また、１～Ｎ番目の経路平行成分予測部１０、１～Ｎ番目の経路垂直成分予測部１１、１～Ｎ番目の第１座標変換部１３、１～Ｎ番目のフィルタリング部１５、１～Ｎ番目の第２座標変換部１６、状態判定部１７は、実行中に記録装置４２へ入出力値および処理中間データの書き込み、読み出し、削除を行う。
　次に、図９のフローチャートを用いて、実施の形態２に係る運動状態判定装置１００の動作の詳細を説明する。図９は、１～Ｎ番目の経路平行成分予測部１０、１～Ｎ番目の経路垂直成分予測部１１、１～Ｎ番目の第１座標変換部１３、センサ部１４、１～Ｎ番目のフィルタリング部１５、１～Ｎ番目の第２座標変換部１６、状態判定部１７の、現時刻における動作を示したフローチャートである。なお、以降の説明における記号および添字のうち、説明が無いものは実施の形態１と同じ意味であるとする。
　動作の説明の準備として、ｎ番目の経路平行成分予測部１０、ｎ番目の経路垂直成分予測部１１、ｎ番目の第１座標変換部１３、ｎ番目のフィルタリング部１５、ｎ番目の第２座標変換部１６で算出される、対象車両２１のｎ番目の運動諸元に関する記号を定義する（ｎは１以上Ｎ以下の整数）。
　まず、ｎ番目の、時刻フレームｋでの観測座標における対象車両２１の運動諸元推定値は、以下のベクトルｘｋ，ｎ （оｂｓ）で表す。

Figure JPOXMLDOC01-appb-I000037
　（３７）

ここで、ｘｋ、ｎ （оｂｓ、１）は対象車両２１の横方向位置、ｘｋ、ｎ （оｂｓ、２）は対象車両２１の縦方向位置、ｘｋ、ｎ （оｂｓ、３）は対象車両２１の横方向速度、ｘｋ、ｎ （оｂｓ、４）は対象車両２１の縦方向速度の推定値を表す。
　また、ｎ番目の、時刻フレームｋでの経路座標における対象車両２１の運動諸元推定値は、経路平行成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ、ｎ （ｐａｒａ）と、経路垂直成分の運動諸元推定値を表す以下の２行１列のベクトルｘｋ、ｎ （ｖｅｒｔ）で表す。それぞれ以下のとおり定義する。

Figure JPOXMLDOC01-appb-I000038
　　　（３８）

Figure JPOXMLDOC01-appb-I000039
　　　　（３９）

ここで、ｘｋ、ｎ （ｐａｒａ、１）は対象車両２１の経路平行方向の位置、ｘｋ、ｎ （ｐａｒａ、２）は対象車両２１の経路平行方向の速度、ｘｋ、ｎ （ｖｅｒｔ、１）は対象車両２１の経路垂直方向の位置、ｘｋ、ｎ （ｖｅｒｔ、２）は対象車両２１の経路垂直方向の速度の推定値を表す。
　次に、各運動諸元の推定誤差に関しても定義する。観測座標における運動諸元推定値ベクトルｘｋ、ｎ （оｂｓ）の誤差はＰｋ、ｎ （оｂｓ）と表す。また、経路座標における運動諸元推定値ベクトルｘｋ、ｎ （ｐａｒａ）、ｘｋ、ｎ （ｖｅｒｔ）の誤差はそれぞれＰｋ、ｎ （ｐａｒａ）、Ｐｋ、ｎ （ｖｅｒｔ）と表す。
　以下、図９の各ステップについて説明する。ステップＳＴ８では、運動状態判定装置１００が、現時刻の処理において未選択の１以上Ｎ以下の整数番号を選択する。なお、未選択の番号のうち、いずれの番号を選択するかの方法は任意とする。以降のステップＳＴ９～１３の説明においては、ここでｎという番号が選択された場合について処理を説明する。
　ステップＳＴ９では、ｎ番目の経路平行成分予測部１０が、現時刻におけるｎ番目の経路平行成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。
　このステップでは、前時刻におけるｎ番目の運動諸元推定値ｘｋ、ｎ （ｐａｒａ）と推定誤差Ｐｋ、ｎ （ｐａｒａ）を入力として、対象車両２１の経路平行位置が等速移動するというｎ番目の運動モデルに基づき、以下の式によりｎ番目の運動諸元予測値ｘｋ｜ｋ－１、ｎ （ｐａｒａ）と予測誤差Ｐｋ｜ｋ－１、ｎ （ｐａｒａ）を算出する。

Figure JPOXMLDOC01-appb-I000040
　　　　　　　　（４０）

Figure JPOXMLDOC01-appb-I000041
  （４１）
　ここで、Qｋ、ｎ （ｐaｒa）は、経路平行方向に関するｎ番目の運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ、ｎ （ｐaｒa）は、経路平行方向に関するｎ番目の運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、例えば以下の行列とする。

Figure JPOXMLDOC01-appb-I000042
　　　　　　（４２）
なお、上記の式では経路平行成分に関するｎ番目の運動モデルとして等速移動を仮定したが、他の運動モデルとしてもよい。例えば、等加速度運動を仮定し、式（４０）の代わりに
　
Figure JPOXMLDOC01-appb-I000043
　 （４３）

などとしてもよい。ここで、ａｎ (ｐａｒａ)は経路平行成分に関する加速度を表すパラメータとした。
　ステップＳＴ１０では、ｎ番目の経路垂直成分予測部１１が、現時刻におけるｎ番目の経路垂直成分の運動諸元を予測し、その運動諸元予測値の誤差を算出する。このステップでは、前時刻におけるｎ番目の運動諸元推定値ｘｋ、ｎ （ｖｅｒｔ）と推定誤差Ｐｋ、ｎ （ｖｅｒｔ）を入力として、対象車両２１の経路垂直位置が等速移動するというｎ番目の運動モデルに基づき、以下の式によりｎ番目の運動諸元予測値ｘｋ｜ｋ－１、ｎ （ｖｅｒｔ）と予測誤差Ｐｋ｜ｋ－１、ｎ （ｖｅｒｔ）を算出する。

Figure JPOXMLDOC01-appb-I000044
　　　　　　　　（４４）

Figure JPOXMLDOC01-appb-I000045
 （４５）

ここで、Qｋ、ｎ （ｖｅｒｔ）は、経路垂直方向に関するｎ番目の運動モデルの誤差を表す２行２列の誤差共分散行列であり、あらかじめ設定するパラメタである。またΦｋ、ｎ （ｖｅｒｔ）は、経路垂直方向に関するｎ番目の運動モデルに基づき運動諸元ベクトルを前時刻から現時刻へ遷移させる遷移行列であり、例えば以下の行列とする。

Figure JPOXMLDOC01-appb-I000046
　　　　　（４６）
なお、上記の式では経路平行成分に関するｎ番目の運動モデルとして等速移動を仮定したが、他の運動モデルとしてもよい。例えば、等加速度運動を仮定し、式（４４）の代わりに

Figure JPOXMLDOC01-appb-I000047
　　　（４７）

などとしてもよい。ここでａｎ (ｖeｒｔ)は経路垂直成分に関する加速度を表すパラメタとした。また、ステップＳＴ９とステップＳＴ１０が仮定する運動モデルおよびパラメタは異なっていてもよい。
　ステップＳＴ１１では、ｎ番目の第１座標変換部１３が、経路座標のｎ番目の運動諸元予測値から観測座標のｎ番目の運動諸元予測値へ変換する。また、経路座標のｎ番目の運動諸元予測誤差から、観測座標のｎ番目の運動諸元予測誤差へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。このステップでは、ｘｋ｜ｋ－１、ｎ （ｐａｒａ）、Ｐｋ｜ｋ－１、ｎ （ｐａｒａ）、ｘｋ｜ｋ－１、ｎ （ｖｅｒｔ）、Ｐｋ｜ｋ－１、ｎ （ｖｅｒｔ）、ｓｉを入力として、以下の式により観測座標の運動諸元予測値ｘｋ｜ｋ－１、ｎ （оｂｓ）と、観測座標の運動諸元予測誤差Ｐｋ｜ｋ－１、ｎ （оｂｓ）を算出する。

Figure JPOXMLDOC01-appb-I000048
　　　　（４８）

Figure JPOXMLDOC01-appb-I000049
 （４９）

ここで、回転行列Ｇｊ （ｐａｒａ）、Ｇｊ （ｖｅｒｔ）および並進ベクトルｇｊは実施の形態１のステップＳＴ３と同じ定義とする。なお、上記における経路データの通し番号ｊは、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する

Figure JPOXMLDOC01-appb-I000050
　　　　　（５０）

λｊの定義はステップＳＴ３の式（２０）と同じとする。
　ステップＳＴ１２では、センサ部１４が観測データを取得し、フィルタリング部１５が観測データと対象車両２１との対応付けを行う。対応付けにおいては、各々の観測データｚｋに対して、観測データと予測位置との残差を表す以下のマハラノビス距離δｎを計算する。

Figure JPOXMLDOC01-appb-I000051
　（５１）

ここで、Ｓｋ、ｎは対象車両２１のｎ番目の予測位置と観測データとの残差に関する誤差共分散行列であり

Figure JPOXMLDOC01-appb-I000052
　　　　　　（５２）

より算出する。
　複数の観測データｚｋのうち、δｎが最小となる観測データを対象車両２１由来であると対応付け、その観測データを直後のＳＴ１３で処理する観測データｚｋとする。また、残差δｎが一定値未満の観測データが１つもなかった場合、または現時刻で取得した観測データが０個であった場合、ステップＳＴ１３で処理する観測データには無効値を代入する。なお、実施の形態２ではここで算出した残差δｎを、ステップＳＴ１６の運動状態判定でも用いる。
　ステップＳＴ１３では、ｎ番目のフィルタリング部１５が、観測データに基づき現時刻における対象車両２１のｎ番目の運動諸元推定値と、推定誤差を算出をする。ここで用いる観測データｚｋは、ステップＳＴ４において対象車両２１と対応付けられた観測データとする。現時刻における対象車両２１のｎ番目の運動諸元推定値ｘｋ、ｎ （оｂｓ）と運動諸元推定値の誤差共分散行列Ｐｋ、ｎ （оｂｓ）は、以下の式により算出する。

Figure JPOXMLDOC01-appb-I000053
　　（５３）

Figure JPOXMLDOC01-appb-I000054
　　　　 （５４）

ここで、Ｋｋ、ｎは以下のとおり定義する。

Figure JPOXMLDOC01-appb-I000055
    　　（５５）

ここで、Ｓｋ、ｎは式（５２）と同じとする。
　なお、観測データｚｋが無効値であった場合、現時刻では対象車両２１が観測されなかったとみなし、以下のように予測値を推定値とする。

Figure JPOXMLDOC01-appb-I000056
　　　　 （５６）

Figure JPOXMLDOC01-appb-I000057
　　　　　（５７）
　ステップＳＴ１４では、ｎ番目の第２座標変換部１６が、観測座標のｎ番目の運動諸元推定値から経路座標のｎ番目の運動諸元推定値へ変換する。また、観測座標のｎ番目の運動諸元推定誤差から、経路座標のｎ番目の運動諸元推定値へ変換する。変換においては、経路データ格納部１２から経路データｓｉを用いる。
　このステップでは、ｘｋ、ｎ （оｂｓ）、Ｐｋ、ｎ （оｂｓ）、ｓｉを入力として、以下の式により経路座標の運動諸元推定値ｘｋ、ｎ （ｐａｒａ）、ｘｋ、ｎ （ｖｅｒｔ）および経路座標の運動諸元推定誤差Ｐｋ、ｎ （ｐａｒａ）、Ｐｋ、ｎ （ｖｅｒｔ）を算出する。

Figure JPOXMLDOC01-appb-I000058
　　（５８）

Figure JPOXMLDOC01-appb-I000059
　　 （５９）

Figure JPOXMLDOC01-appb-I000060
　　 （６０）

Figure JPOXMLDOC01-appb-I000061
　　　（６１）

ここで、回転行列回転行列Ｇｊ （ｐａｒａ）、Ｇｊ （ｖｅｒｔ）および並進ベクトルｇｊは実施の形態１のステップＳＴ６と同じ定義とする。なお、経路データの通し番号ｊの算出方法は、経路平行方向の位置において、対象車両２１を前後に挟む中央点とし、以下の条件を満たす１以上Ｉ以下のｊと定義する。

Figure JPOXMLDOC01-appb-I000062
   　　　（６２）

λｊの定義はステップＳＴ３の式（２０）と同じとする。
　ステップＳＴ１５では、運動状態判定装置１００が、現時刻の処理において未選択の１以上Ｎ未満の整数番号が存在するか否かを判定する。未選択の番号が存在するならばステップＳＴ８へ遷移し、存在しないならばステップＳＴ１６へ遷移する。
　ステップＳＴ１６では、状態判定部１７が、Ｎ個の経路座標における運動諸元推定値と推定誤差、そして予測位置と観測データとの残差を基に、対象車両２１の運動状態として対象車両２１の蛇行有無を判定する。
　まず、式（５１）によって算出された、Ｎ個の、予測位置と観測データとの残差δｎを用い、以下の番号ｎ＊を求める。

Figure JPOXMLDOC01-appb-I000063
　　（６３）

　前述のδｎの定義から明らかなように、δｎが小さいほど、ｎ番目の運動モデルによって予測された位置が観測データに近いことを表している。従って、最も小さなδｎを与えるｎ＊番目の予測値が、N種類の運動モデルのうち最も対象車両２１の実態に即した運動モデルによって算出された予測値とみなすことができる。
　ｎ＊を算出した後に、以下のマハラノビス距離Δｋ、＊を求める。

Figure JPOXMLDOC01-appb-I000064
　　（６４）

このΔｋ、＊が大きいほど、対象車両２１の位置が経路の中心から有意に離れていることを表している。従ってΔｋ、＊の時系列グラフが、ある一定以上の振幅をもって振動しているならば、対象車両２１は蛇行していると判定する。Δｋ、＊の振幅を判定する際には、例えばある一定時刻フレーム間でのΔｋ、＊の最大値と最小値の差を振幅とする、Δｋを短時間フーリエ変換し周波数成分のピーク値を振幅とする、などの方法を用いる。
　なお、上記では経路垂直方向の運動諸元とその推定誤差のみに基づき蛇行の有無を判定したが、経路平行方向の運動諸元とその推定誤差を用いてもよい。例えば、経路平行方向の位置がその推定誤差に比べどの程度大きいか、すなわち対象車両２１とセンサがどれだけ離れているかに応じて、蛇行を判定するΔｋ、＊の振幅しきい値を連続的に高くするなどとしてもよい。
　また、上記では残差を表すδｎの大きさに基づき、ｎ＊を算出したが、運動モデルの妥当性を表す指標、例えばＡＩＣ（Akaike Information Criteria）やＢＩＣ（Bayesian Information Criteria）などの指標を用いてｎ＊を算出してもよい。
　なお、図９のフローチャートでは、ステップＳＴ９～１４の処理を順にＮ回実行する動作としたが、代わりにステップＳＴ９～１４を複数のスレッドで並列処理し、Ｎ個の経路平行成分予測部１０、経路垂直成分予測部１１、第１座標変換部１３、フィルタリング部１５、第２座標変換部１６の処理が完了したのちに、ステップＳＴ１６を実行してもよい。
　以上のように構成された実施の形態２に係る運動状態判定装置１００では、次の効果が得られる。実施の形態２に係る運動状態判定装置１００では、Ｎ個の経路平行成分予測部１０と、Ｎ個の経路垂直成分予測部１１によって、予測における運動モデルの前提またはパラメタ値が異なるＮ通りの運動諸元予測値を算出し、それぞれの予測値に基づき運動諸元を推定し、それらの予測値と予測誤差、また予測値と観測データとの残差に基づき対象車両２１の運動状態を判定するよう構成した。この構成により、対象車両２１が多様な運動をとりうる場合においても、実際の対象車両２１の運動に則した運動モデルに基づく運動諸元を予測し、その予測値から推定された運動諸元に基づく運動状態判定が可能となる。なお、Ｎ個の経路平行成分予測部１０によってＮ通りの運動諸元予測値及び予測誤差を算出することは、経路平行成分予測部１０によってＮ組の運動諸元予測値を算出する状態として扱うこともできる。
　この効果は、例えば対象車両２１の属性や対象車両２１が走行する路面状態などが多様であり、かつそれらの情報が取得できない場合において、１種類の運動モデルに基づき推定された運動諸元から運動状態を判定した場合に比べ、予測誤差および推定誤差が小さい運動諸元を用いて運動状態の判定が可能となるため、最終的に運動状態判定を誤る頻度を減らすことができる点が、特に顕著な効果として現れる。
　このように、実施の形態２に係る運動状態判定装置１００では、前記経路成分予測部１は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ前記経路に平行な成分と垂直な成分を算出し、前記第１座標変換部１３は、前記経路成分予測部１により算出されたＮ組の運動諸元の予測値及び予測誤差を、前記第１の座標系に変換し、前記フィルタリング部１５は、前記第１座標変換部１３で変換されたＮ組の運動諸元の予測値及び予測誤差のそれぞれの組に対して、前記目標の運動諸元の推定値及び推定誤差を算出し、前記第２座標変換部１６は、前記フィルタリング部１５により算出されたＮ組の前記目標の運動諸元の推定値及び推定誤差をそれぞれ前記第２の座標系に変換し、前記状態判定部１７は、前記第２座標変換部１６により変換されたＮ組の前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定することを特徴とする。この構成によって、目標が多様な運動をとりうる場合においても、実際の目標の運動に則した運動モデルに基づく運動諸元を予測し、その予測値から推定された運動諸元に基づく運動状態判定が可能となる。
　また、実施の形態２に係る運動状態判定装置１００では、前記経路成分予測部１は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分を算出することを特徴とする。この構成によって、さまざまな運動モデルの中から実際の目標の運動に則した運動モデルを選択することができ、最終的に運動状態判定を誤る頻度を減らすことができる。
１：経路成分予測部、１０：経路平行成分予測部、１１：経路垂直成分予測部、１２：経路データ格納部、１３：第１座標変換部、１４：センサ部、１５：フィルタリング部、１６：第２座標変換部、１７：状態判定部、２０：自車、２１：対象車両、２３：観測データ、４１：演算装置、４２：記録装置、４３：出力インターフェース、１００：運動状態判定装置

Claims (5)

  1. 目標の観測データを取得するセンサ部と、
    前記目標が移動可能な経路の形状を表す経路データを格納する経路データ格納部と、
    前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分と垂直な成分をそれぞれ算出する経路成分予測部と、
    前記経路成分予測部により算出された運動諸元の予測値及び予測誤差を、前記観測データを表す第１の座標系と同じ座標系に変換する第１座標変換部と、
    前記センサ部で取得された観測データと、前記経路データ格納部に格納された経路データと、前記第１座標変換部で変換された運動諸元の予測値及び予測誤差を用いて、前記目標の運動諸元の推定値及び推定誤差を算出するフィルタリング部と、
    前記フィルタリング部により算出された前記目標の運動諸元の推定値及び推定誤差を、前記経路に平行な成分と垂直な成分を座標軸とする第２の座標系に変換する第２座標変換部と、
    前記第２座標変換部によって変換された前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する状態判定部と、
    を備えたことを特徴とする運動状態判定装置。
  2. 前記経路成分予測部は、
    前記目標の運動諸元の予測値及び予測誤差の前記経路に平行な成分を算出する経路平行成分予測部と、
    前記目標の運動諸元の予測値及び予測誤差の前記経路に垂直な成分を算出する経路垂直成分予測部
    を備えたことを特徴とする請求項１に記載の運動状態判定装置。
  3. 　前記経路成分予測部は、前記経路に対して平行方向と垂直方向で異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分をそれぞれ算出する
    ことを特徴とする請求項１又は請求項２に記載の運動状態判定装置。
  4. 前記経路成分予測部は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ前記経路に平行な成分と垂直な成分を算出し、
    前記第１座標変換部は、前記経路成分予測部により算出されたＮ組の運動諸元の予測値及び予測誤差を、前記第１の座標系に変換し、
    前記フィルタリング部は、前記第１座標変換部で変換されたＮ組の運動諸元の予測値及び予測誤差のそれぞれの組に対して、前記目標の運動諸元の推定値及び推定誤差を算出し、
    前記第２座標変換部は、前記フィルタリング部により算出されたＮ組の前記目標の運動諸元の推定値及び推定誤差をそれぞれ前記第２の座標系に変換し、
    前記状態判定部は、前記第２座標変換部により変換されたＮ組の前記目標の運動諸元の推定値及び推定誤差を用いて、前記目標の運動状態を判定する
    ことを特徴とする請求項１から３のいずれか１項に記載の運動状態判定装置。
  5. 　前記経路成分予測部は、Ｎ組の前記目標の運動諸元の予測値及び予測誤差に対してそれぞれ異なる運動モデルを設定して、前記経路に平行な成分と垂直な成分を算出する
    ことを特徴とする請求項１又は請求項２に記載の運動状態判定装置。
PCT/JP2019/001490 2019-01-18 2019-01-18 運動状態判定装置 WO2020148894A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
PCT/JP2019/001490 WO2020148894A1 (ja) 2019-01-18 2019-01-18 運動状態判定装置
CN201980088726.6A CN113287157B (zh) 2019-01-18 2019-01-18 运动状态判定装置
US17/298,067 US20220122461A1 (en) 2019-01-18 2019-01-18 Motion state determination apparatus
JP2020566075A JP6952914B2 (ja) 2019-01-18 2019-01-18 運動状態判定装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2019/001490 WO2020148894A1 (ja) 2019-01-18 2019-01-18 運動状態判定装置

Publications (1)

Publication Number Publication Date
WO2020148894A1 true WO2020148894A1 (ja) 2020-07-23

Family

ID=71614131

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/001490 WO2020148894A1 (ja) 2019-01-18 2019-01-18 運動状態判定装置

Country Status (4)

Country Link
US (1) US20220122461A1 (ja)
JP (1) JP6952914B2 (ja)
CN (1) CN113287157B (ja)
WO (1) WO2020148894A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102022210626A1 (de) 2021-10-18 2023-04-20 Mitsubishi Electric Corporation Kursvorhersagevorrichtung

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20210129913A (ko) * 2020-04-21 2021-10-29 주식회사 만도모빌리티솔루션즈 운전자 보조 시스템

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005077167A (ja) * 2003-08-29 2005-03-24 Mitsubishi Electric Corp 目標追尾装置
JP2005274300A (ja) * 2004-03-24 2005-10-06 Mitsubishi Electric Corp 目標追尾装置
JP2012251904A (ja) * 2011-06-03 2012-12-20 Mitsubishi Electric Corp 追尾装置

Family Cites Families (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH0990027A (ja) * 1995-09-20 1997-04-04 Mitsubishi Electric Corp 目標曲進判定装置
SE510844C2 (sv) * 1997-11-03 1999-06-28 Celsiustech Syst Ab Automatisk kompensering av systematiska fel vid målföljning med flera sensorer
JP4144538B2 (ja) * 2003-11-07 2008-09-03 日産自動車株式会社 車両用運転操作補助装置および車両用運転操作補助装置を備えた車両
CN1775601A (zh) * 2005-11-18 2006-05-24 吉林大学 车辆行驶轨迹预估及车道偏离评价方法
CN101074989A (zh) * 2006-05-19 2007-11-21 华夏科技股份有限公司 利用全球卫星定位系统预估交通工具的行驶路径的方法
JP2008077349A (ja) * 2006-09-20 2008-04-03 Toyota Motor Corp 車両状態量推定装置及びその装置を用いた車両操舵制御装置
CN101661104B (zh) * 2009-09-24 2012-04-25 北京航空航天大学 基于雷达/红外量测数据坐标转换的目标跟踪方法
JP5664059B2 (ja) * 2010-09-17 2015-02-04 株式会社デンソー 車両用軌跡推定装置
JP2014026516A (ja) * 2012-07-27 2014-02-06 Nissan Motor Co Ltd 目標経路生成装置及び目標経路生成方法
WO2014083910A1 (ja) * 2012-11-30 2014-06-05 日本電気株式会社 対象物追跡システム、対象物追跡方法、画像処理装置およびその制御方法と制御プログラム
JP6248559B2 (ja) * 2013-11-13 2017-12-20 株式会社デンソー 車両用走行軌跡算出装置
US9620015B2 (en) * 2015-07-13 2017-04-11 Nissan North America, Inc. Kinematic path prediction of vehicles on curved paths
JP6540335B2 (ja) * 2015-07-30 2019-07-10 富士通株式会社 走行状況表示プログラム、走行状況表示装置、及び走行状況表示方法
CN106950562B (zh) * 2017-03-30 2020-02-18 电子科技大学 一种基于预测值量测转换的状态融合目标跟踪方法
CN109211248A (zh) * 2018-07-31 2019-01-15 哈尔滨工程大学 一种基于多传感器的智能车辆导航系统及其导航方法
CN108765961B (zh) * 2018-08-03 2021-03-16 东华理工大学 一种基于改进型限幅平均滤波的浮动车数据处理方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005077167A (ja) * 2003-08-29 2005-03-24 Mitsubishi Electric Corp 目標追尾装置
JP2005274300A (ja) * 2004-03-24 2005-10-06 Mitsubishi Electric Corp 目標追尾装置
JP2012251904A (ja) * 2011-06-03 2012-12-20 Mitsubishi Electric Corp 追尾装置

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102022210626A1 (de) 2021-10-18 2023-04-20 Mitsubishi Electric Corporation Kursvorhersagevorrichtung

Also Published As

Publication number Publication date
CN113287157A (zh) 2021-08-20
JP6952914B2 (ja) 2021-10-27
JPWO2020148894A1 (ja) 2021-05-20
US20220122461A1 (en) 2022-04-21
CN113287157B (zh) 2022-08-16

Similar Documents

Publication Publication Date Title
US11579629B2 (en) Temporal information prediction in autonomous machine applications
JP7254823B2 (ja) 物体の検出および特徴付けのためのニューラルネットワーク
JP2021523443A (ja) Ｌｉｄａｒデータと画像データの関連付け
Aeberhard et al. High-level sensor data fusion architecture for vehicle surround environment perception
US11854401B2 (en) Temporal information prediction in autonomous machine applications
US20230110116A1 (en) Advanced driver assist system, method of calibrating the same, and method of detecting object in the same
US11042999B2 (en) Advanced driver assist systems and methods of detecting objects in the same
US10890658B2 (en) Vehicle group control device
US20190325597A1 (en) Simultaneous Localization And Mapping Constraints In Generative Adversarial Networks For Monocular Depth Estimation
WO2015155833A1 (ja) 衝突防止装置
CN114080634A (zh) 使用锚定轨迹的代理轨迹预测
CN112562405A (zh) 一种雷达视频智能融合与预警方法及系统
CN114495064A (zh) 一种基于单目深度估计的车辆周围障碍物预警方法
JP6936098B2 (ja) 対象物推定装置
JP2009175929A (ja) ドライバ状態推定装置及びプログラム
WO2020148894A1 (ja) 運動状態判定装置
JP6439757B2 (ja) 画像処理装置および画像処理方法
CN113176562B (zh) 多目标跟踪方法、装置、电子设备及可读存储介质
CN113281718B (zh) 一种基于激光雷达场景流估计的3d多目标跟踪系统及方法
JP2024012160A (ja) 目標状態推定方法、装置、電子機器及び媒体
CN114846523A (zh) 使用记忆注意力的多对象跟踪
JPWO2020170301A1 (ja) 情報処理装置、プログラム及び情報処理方法
CN115115084A (zh) 使用占用流场预测代理在环境中的未来移动
CN114817765A (zh) 基于地图的目标航向消歧
CN113269118A (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: 19910038

Country of ref document: EP

Kind code of ref document: A1

ENP Entry into the national phase

Ref document number: 2020566075

Country of ref document: JP

Kind code of ref document: A

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19910038

Country of ref document: EP

Kind code of ref document: A1