WO2017158951A1 - 物体検知システム、異常判定方法、及びプログラム - Google Patents
物体検知システム、異常判定方法、及びプログラム Download PDFInfo
- Publication number
- WO2017158951A1 WO2017158951A1 PCT/JP2016/086235 JP2016086235W WO2017158951A1 WO 2017158951 A1 WO2017158951 A1 WO 2017158951A1 JP 2016086235 W JP2016086235 W JP 2016086235W WO 2017158951 A1 WO2017158951 A1 WO 2017158951A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- moving body
- measuring device
- change
- movement
- parameter
- Prior art date
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/16—Anti-collision systems
Definitions
- the present invention relates to a system, a method, and a program for determining an abnormality of an object detection device in an object detection device that is attached to a moving body and detects an object existing around while moving.
- Patent Document 1 discloses a method for detecting a shift in the light projecting direction of a laser beam with respect to an object detection device attached to an automobile.
- the distance between the object detection device and the overhead marker at the moment when the laser beam is no longer reflected by the overhead marker and cannot be detected, and the object detection device is based on the calculated distance.
- the degree of deviation from the mounting angle is detected.
- An object of the present invention is to accurately grasp an abnormality of a measuring apparatus that detects an object in an object detection system that moves while detecting an object existing around the object.
- An object detection system includes a moving body, a measurement device, a coordinate prediction unit, and a determination unit.
- the moving body is a movable object.
- the measuring device is attached to the moving body and moves together with the moving body.
- the measuring device measures a coordinate value representing a position of an object existing around the moving body as viewed from the measuring device.
- the coordinate prediction unit is a predicted coordinate that represents the position of the object viewed from the moving body after the movement based on the first coordinate value, the position information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement. Calculate the value.
- the first coordinate value represents the position of the object measured before the moving body moves.
- the second coordinate value represents the position of the object measured after the moving body is moved.
- the measurement apparatus uses the position viewed from the object measurement apparatus at a predetermined time (before the movement of the moving body) as the first coordinate value from the predetermined time.
- the position of the object viewed from the measuring device after a certain amount of time has elapsed (after the moving body has moved) is measured as the second coordinate value.
- a predicted coordinate value representing the position of the object viewed from the moving object is calculated.
- the determination unit compares the predicted coordinate value with the second coordinate value to determine an abnormality occurring in the measurement apparatus.
- the amount of change in the position and orientation of the measurement device calculated from the change in the measurement value of the object position by the measurement device not the information that is difficult to grasp, such as the timing at which the object is no longer observed by the measurement device
- the abnormality of the measuring device is determined based on easy-to-understand information such as the amount of change in the position and posture of the measuring device calculated from the change in the position and posture of the moving body. Thereby, abnormality of a measuring device can be grasped more correctly.
- the object detection system may further include a moving body control unit.
- the moving body control unit controls the movement of the moving body.
- changes in the position and posture of the moving body before and after the movement are calculated based on the control amount of the moving body in the moving body control unit.
- position of a moving body can be calculated comparatively easily.
- the object detection system may further include a movement amount measuring device.
- the movement amount measuring apparatus measures an amount representing movement of the moving body before and after movement. Thereby, the change of the position and attitude
- the change in the position of the moving body before and after the movement may be expressed as a change in the reference position fixed with respect to the moving body before and after the movement of the moving body.
- the second change amount is calculated based on the change in the reference position before and after the movement of the moving body, and the relative relationship between the attachment position and orientation of the measurement device with respect to the reference position. Thereby, the second change amount can be calculated relatively easily.
- the positional relationship between the mounting position and the reference position may be constant.
- the second change amount can be calculated on the assumption that the mounting position of the measuring device is fixed.
- the first change amount and the second change amount can be easily compared.
- An object detection system includes a moving body, a first parameter calculation unit, a second parameter calculation unit, a storage unit, and a determination unit.
- the moving body is a movable object.
- the measuring device is a measuring device that is attached to a moving body and moves together with the moving body.
- the measuring device measures a coordinate value representing a position of an object existing around the moving body as viewed from the measuring device.
- the first parameter calculation unit calculates a first parameter.
- the first parameter is a parameter that represents a change in the position and orientation of the measurement device that is calculated based on a change between the first coordinate value and the second coordinate value.
- the first coordinate value represents the position of the object measured before the moving body moves.
- the second coordinate value represents the position of the object measured after the moving body is moved.
- the second parameter calculation unit calculates the second parameter.
- the second parameter is a parameter representing changes in the position and posture of the moving body before and after movement.
- the storage unit stores the third parameter.
- the third parameter is a parameter that represents the position and orientation of the measuring device with respect to the moving body at a certain time before the moving body moves.
- a determination part determines the change with respect to a certain time of the attachment position and attitude
- the change of the attachment position and posture with respect to a certain point is determined. Thereby, the change of the attachment position and attitude
- the determination unit may calculate the presence / absence or degree of change of the measurement device mounting position and posture with respect to a certain point in time. Thereby, it can be determined how much the mounting state of the measuring apparatus has changed.
- the determination unit may calculate the presence / absence of change or the degree of change based on the value of the determinant of the matrix generated from the first parameter, the second parameter, and the third parameter. Thereby, how much the mounting state of the measuring apparatus has changed can be quantified.
- the change in the position of the moving body before and after the movement may be expressed as a change in the reference position fixed with respect to the moving body before and after the movement of the moving body.
- the second parameter can be calculated relatively easily.
- An abnormality determination method is an abnormality determination method for a measuring apparatus attached to a movable body that is a movable object.
- the abnormality determination method includes the following steps. A step of causing the measuring device to measure the first coordinate value representing the position of the object existing around the moving body viewed from the measuring device before the moving body moves. A step of causing the measuring device to measure the second coordinate value representing the position of the object viewed from the measuring device after the moving body is moved. A step of calculating a predicted coordinate value representing the position of the object viewed from the moving body after the movement, based on the first coordinate value, the measurement apparatus position information with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement. When, A step of determining an abnormality of the measuring device by comparing the predicted coordinate value and the second coordinate value.
- the amount of change in the position and orientation of the measurement device calculated from the change in the measurement value of the object position by the measurement device is not difficult information such as the timing at which the object is no longer observed by the measurement device.
- the abnormality of the measuring device is determined based on easy-to-understand information such as the amount of change in the position and posture of the measuring device calculated from the change in the position and posture of the moving body. Thereby, abnormality of a measuring device can be grasped exactly.
- An abnormality determination method is an abnormality determination method of an attachment state of a measuring device attached to a movable body that is a movable object.
- the abnormality determination method includes the following steps. A step of causing the measuring device to measure the first coordinate value representing the position of the object existing around the moving body viewed from the measuring device before the moving body moves. A step of causing the measuring device to measure the second coordinate value representing the position of the object viewed from the measuring device after the moving body is moved. A step of calculating a first parameter representing a change in the position and orientation of the measuring device based on a change between the first coordinate value and the second coordinate value.
- the change of the attachment position and posture with respect to a certain point is determined. Thereby, the change of the attachment position and attitude
- a program according to still another aspect of the present invention is a program that causes a computer to execute the abnormality determination method.
- the figure which shows the structure of an object detection system The figure which shows the structure of a measuring apparatus.
- FIG. 1 is a diagram illustrating a configuration of an object detection system.
- the object detection system 100 according to the first embodiment is a system that assists an operation by a driver of the moving body 1 that is, for example, various vehicles such as an automobile.
- the object detection system 100 includes a moving body 1.
- the moving body 1 is a movable object, for example, a vehicle.
- the moving body 1 includes wheels 2a, 2b, 2c, and 2d.
- the wheels 2a and 2b can rotate around an axis via a speed reduction mechanism on the output rotation shaft of a power function (for example, an engine and / or an electric motor) at the front of the moving body 1 in the straight traveling direction (FIG. 1). It is attached.
- a power function for example, an engine and / or an electric motor
- the wheels 2c and 2d are attached to the rear part of the moving body 1 in the straight traveling direction so as to be rotatable around the axis.
- the object detection system 100 includes four measuring devices 4a, 4b, 4c, and 4d.
- the measuring device 4 a is attached to the forefront part of the moving body 1 in the straight traveling direction and detects an object existing in front of the moving body 1.
- the measuring device 4 b is attached to the rearmost part of the moving body 1 in the straight traveling direction, and detects an object existing behind the moving body 1.
- the measuring device 4 c is attached to the left side surface of the moving body 1 in the straight traveling direction, and detects an object existing on the left side of the moving body 1.
- the measuring device 4d is attached to the right side surface of the moving body 1 in the straight traveling direction, and detects an object present on the right side of the moving body 1. Since the four measuring devices 4a to 4d are attached to the moving body 1, when the moving body 1 moves, it moves together with the movement of the moving body.
- the measuring devices 4a to 4d are TOF (Time Of Flight) sensors that measure the distance to the object B to be detected.
- the measurement device is not limited to this, and for example, a stereo camera that measures the distance from the image difference between the left and right cameras, a laser range finder (Laser Range Finder, LRF), or another type of distance measurement sensor may be used. Good.
- LRF Laser Range Finder
- the object detection system 100 includes a control unit 5.
- the control unit 5 includes a CPU (Central Processing Unit), a storage device (RAM (Random Access Memory), ROM (Read Only Memory), SSD (Solid State Drive), HDD (Hard Disk Drive, etc.) and the like.
- a computer system provided with an A / D, a D / A converter, and the like.
- the control unit 5 receives detection signals from the measurement devices 4a to 4d, and determines the positional relationship between the object B and the moving body 1 existing around based on the detection signals.
- the control unit 5 is connected to the moving body control unit 3, and the moving body control unit 3 further determines the braking function, power function, and / or moving direction of the moving body 1 provided on the wheels 2a and 2b.
- control unit 5 outputs positional relationship information between the object B and the moving body 1, and the moving body control unit 3 that has received the information outputs a traveling function on behalf of the driver of the object detection system 100 as necessary. Control.
- the configurations of the control unit 5 and the moving body control unit 3 will be described in detail later.
- the object detection system 100 includes a movement amount measuring device 6.
- the movement amount measuring device 6 measures an amount representing movement of the moving body 1. Examples of the moving amount measuring device 6 include a speed sensor that measures the speed of the moving body 1, an acceleration sensor that measures acceleration, and / or an encoder that measures the amount of rotation of the wheels 2a to 2d, and the angular velocity of the moving body 1. A gyro sensor to be measured can be used.
- a GPS (Global Positioning System) device may be used as the movement amount measuring device 6. Since the object detection system 100 includes the movement amount measuring device 6, changes in the position and posture of the moving body 1 before and after the movement can be determined based on the actually measured values related to the movement.
- the control unit 5 calculates the measurement devices 4a to 4a before and after the movement of the moving body 1 calculated based on the position data D1 and D2 generated from the data acquired by the measurement devices 4a to 4d before and after the movement of the moving body 1.
- Measurement devices 4a to 4d before and after the movement of the moving body 1 calculated based on the amount of change in the position and orientation of 4d (referred to as the first change amount M1) and the change in the position and orientation of the moving body 1 before and after the movement.
- the second change amount M2 Based on the comparison with the change amount of the position and orientation
- Examples of the measuring devices 4a to 4d determined in the present embodiment include a change in the mounting state of the measuring devices 4a to 4d on the moving body 1 and a failure of the measuring devices 4a to 4d.
- Changes in the mounting state of the measuring devices 4a to 4d to the moving body 1 to be determined include, for example, changes in the mounting position and mounting angle of the measuring devices 4a to 4d in the moving body 1, and the moving bodies of the measuring devices 4a to 4d. There is a fixed degree to 1.
- the failure of the measuring devices 4a to 4d to be determined for example, the displacement of the mounting position of the output unit 41 and the detection surface DS (described later) inside the measuring devices 4a to 4d, the detection window ( There are dirt, scratches, etc. on a window that transmits measurement light Lm and reflected light Lr described later.
- the abnormality determination method for the measuring devices 4a to 4d in the control unit 5 will be described in detail later.
- the object detection system 100 can assist the driving of the moving body 1 by the driver based on the positional relationship between the object B and the moving body 1 detected by the measuring devices 4a to 4d.
- the abnormality of the measuring devices 4a to 4d can be determined while the moving body 1 is traveling.
- FIG. 2 is a diagram illustrating the configuration of the measurement apparatus. Since the four measuring devices 4a to 4d have the same configuration, the configuration of the measuring device 4a will be described below as an example.
- the measuring device 4 a has an output unit 41.
- the output unit 41 is, for example, a light source that outputs measurement light Lm in the infrared region toward the object B that is a detection target. As shown in FIG. 2, in the present embodiment, the output unit 41 preferably outputs the measurement light Lm that has spread over a wide space in which the object detection system 100 moves. This is because the measuring device 4a can simultaneously detect the object B in a wide range of the space in which the object detection system 100 moves.
- the measuring device 4a has a plurality of detectors 43-1, 43-2, ... 43-n.
- Each of the plurality of detection units 43-1 to 43-n is disposed at a predetermined position on the detection surface DS (semiconductor substrate), for example, and is reflected light that is generated when the measurement light Lm is reflected by the object B. Lr is detected.
- the detection units 43-1 to 43-n are, for example, charge coupled devices (Charge Coupled Devices) or CMOS (Complementary MOS) devices.
- the plurality of detection units 43-1 to 43-n are arranged in the vertical direction and the horizontal direction on the detection surface DS to form an array.
- the plurality of detection units 43-1 to 43-n can form a CCD image sensor or a CMOS image sensor on the detection surface DS.
- a switching element for example, a MOS-FET
- an address line is connected to the switching element, and when a signal is applied to the address line, the switching element is turned on, and the detection unit connected to the turned on switching element and the control unit 5 include Signal transmission / reception becomes possible.
- the measuring device 4a has a lens 45.
- the lens 45 condenses the reflected light Lr in a region of the detection surface DS where the plurality of detection units 43-1 to 43-n are formed. As a result, an image of the object B in a wide range can be formed in an area where the plurality of detection units 43-1 to 43-n are formed.
- the measuring apparatus 4a can acquire data by projecting the object B in the real space on which the object detection system 100 moves onto predetermined coordinates.
- FIG. 3A is a diagram illustrating a configuration of a control unit.
- Part or all of the functions of the elements of the control unit 5 described below may be realized as a program that can be executed by a computer system that configures the control unit 5.
- the program may be stored in a storage area formed in a storage device of the computer system.
- some or all of the functions of each element of the control unit 5 may be realized in hardware by a custom IC or the like.
- the control unit 5 includes a storage unit 51.
- the storage unit 51 stores various data, for example, a part of a storage area provided in a storage device of a computer system.
- the control unit 5 includes a position data generation unit 52.
- the position data generation unit 52 generates position data D1 and D2.
- the position data D1 and D2 are generated as a set of coordinate values representing the relative position of the object B detected by the measuring device 4a as viewed from each measuring device.
- the position data generation unit 52 generates the position data D1 and D2 as follows.
- the position data generation unit 52 connects the first detection unit 43-1 and the position data generation unit 52 by applying a signal to the address line corresponding to the first detection unit 43-1.
- a signal (for example, a current or voltage signal) indicating whether or not the detection unit 43-1 has detected the reflected light Lr is input.
- the position data generation unit 52 detects the difference between the time when the signal indicating whether or not the reflected light Lr is detected and the time when the output unit 41 outputs the measurement light Lm. Calculated as information T1. If the reflected light Lr is not detected, the signal detection information T1 is set to infinity (or a very large value) because no signal is input from the detection unit 43-1.
- Position information can be generated by associating the coordinate values (x1, y1) that define the arrangement position on the detection surface DS of the detection unit 43-1 with the signal detection information T1. Thereafter, the position data generation unit 52 executes the above-described process for all the other detection units 43-2 to 43-n by sequentially changing the address lines to which the signal is applied, so that n position data A set of information (x1, y1, T1), (x2, y2, T2),... (Xn, yn, Tn) is generated.
- the position data generation unit 52 converts each of the n pieces of position information aggregates (x1, y1, T1), (x2, y2, T2),... (Xn, yn, Tn),
- the position data D1 and D2 are generated by converting into coordinate values in a coordinate system representing the real space in which the moving body 1 moves. Specifically, taking the case where the p-th position information (xp, yp, Tp) is converted into the above coordinate value as an example, the position data generation unit 52 firstly sets the signal detection information Tp (time unit).
- the position data generation unit 52 enlarges the coordinate value (xp, yp) defined on the detection surface DS at an enlargement rate determined based on, for example, the distance zp or the focal length (of the lens 45).
- the coordinate value of the object B in the coordinate system representing the real space with the measuring device 4a as the origin is calculated as (Xp, Yp).
- the position data D1 and D2 are obtained by executing the conversion of the coordinate values for all the n pieces of position information.
- the control unit 5 includes a first change amount calculation unit 53 (an example of a first parameter calculation unit).
- the first change amount calculation unit 53 includes a first coordinate value that is a coordinate value of the object measured by the measuring device 4a before the moving body 1 moves, and an object measured by the measuring device 4a after the moving body 1 moves.
- a first change amount M1 representing a change in the position and orientation of the measuring device 4a before and after the movement of the moving body 1 is calculated based on a change between the second coordinate value and the second coordinate value.
- the first change amount calculation unit 53 selects the first coordinate value from the first position data D1 generated before the movement of the moving body 1, and generates the first change value generated after the movement of the moving body 1.
- a second coordinate value is selected from the two-position data D2, and a change amount between the first coordinate value and the second coordinate value is calculated as a first change amount M1.
- the calculation method of the first change amount M1 in the first change amount calculation unit 53 will be described in detail later.
- the control unit 5 includes a second change amount calculation unit 54 (an example of a second parameter calculation unit).
- the second change amount calculation unit 54 is a second change that represents a change in the position and orientation of the measuring device 4a before and after the movement of the moving body 1 based on changes in the position and orientation of the moving body 1 before and after the movement of the moving body 1.
- the amount M2 is calculated. Specifically, the second change amount calculation unit 54 changes the reference position fixed with respect to the moving body 1 before and after the movement of the moving body 1, the mounting positions Ps1 to Ps4 and the postures of the measuring devices 4a to 4d.
- the second change amount M2 is calculated based on the relative positional relationship with respect to the reference position.
- the relative position of the mounting position and / or posture (mounting angle) of the measuring device 4a used for calculating the second change amount M2 with respect to the reference position is determined at the time of mounting and does not change unless there is an abnormality.
- the second change amount calculation unit 54 moves the moving body 1 based on the measured values of acceleration, speed, and / or position of the moving body 1 measured by the movement amount measuring device 6. The amount of movement before and after is calculated. Thereby, the movement amount before and after the movement of the mobile body 1 can be calculated based on the measured physical quantity.
- the second change amount calculation unit 54 is configured to move the vehicle 1 (the object detection system 100) such as the accelerator opening, the brake strength, and / or the steering angle of the steering wheel input to the mobile body control unit 3 (described later). ) And / or the movement amount of the moving body 1 based on the control amount relating to the movement of the moving body 1 determined by the moving body control unit 3 based on the positional relationship between the object B and the moving body 1. May be calculated. Thereby, the movement amount of the moving body 1 can be calculated relatively easily. A method of calculating the second change amount M2 in the second change amount calculation unit 54 will be described in detail later.
- the control unit 5 includes a determination unit 55.
- the determination unit 55 is calculated based on a change between the first coordinate value and the second coordinate value before and after the movement of the moving body 1 (that is, a change in the position of the object viewed from the measurement device 4a before and after the movement).
- the abnormality of the measuring device 4a is determined by comparing the first change amount with the second change amount calculated based on the change in the position and posture of the moving body 1 before and after the movement.
- the determination unit 55 takes into account a measurement error of the measurement device 4a or the movement amount measurement device 6 and the value calculated as the difference between the first change amount M1 and the second change amount M2 is a predetermined value.
- the threshold value ⁇ If it is equal to or greater than the threshold value ⁇ , it is determined that an abnormality has occurred in the measuring device 4a. On the other hand, if the value calculated as the difference between the first change amount M1 and the second change amount M2 is smaller than the threshold ⁇ , it is determined that the measuring device 4a is operating normally.
- the determination unit 55 may display on the display (not shown) of the object detection system 100 that an abnormality has occurred in the measurement device 4a (abnormal alarm). Or the determination part 55 may notify the said abnormality alarm with an audio
- An abnormality determination method of the measuring device 4a based on the difference between the first change amount M1 and the second change amount M2 in the determination unit 55 will be described in detail later.
- the control unit 5 includes a real space data output unit 56.
- the real space data output unit 56 outputs real space data VD.
- the mobile body control unit 3 (FIG. 1) receives the real space data VD, and the control amount (for example, accelerator opening, brake strength, and / or steering angle of the steering wheel, etc.) by the driver of the mobile body 1 (object detection system 100). ) And output to the second change amount calculation unit 54.
- the moving body control unit 3 controls the control amount (accelerator opening degree, brake strength, and / or steering angle, etc.) of the moving body 1 (object detection system 100) based on the positional relationship between the moving body 1 and the object B. ).
- the moving body control unit 3 determines that the object B is present in the vicinity of the moving body 1, the moving body control unit 3 gives a command to increase the brake strength and the accelerator opening (or the engine and / or the electric motor output control mechanism).
- a command to change to 0 and / or a command to change the steering angle of the steering wheel in a direction away from the object B are output to a driving function (not shown) such as a power function, a brake, and / or a steering wheel.
- a driving function not shown
- the mobile body 1 object detection system 100
- the mobile body control part 3 may output the instruction
- the control part 5 determines whether abnormality, such as the attachment state of the measuring apparatus 4a, and a failure generate
- FIG. 4 is a flowchart showing the operation of the object detection system.
- the position data generation unit 52 generates the position data D1 and D2 as described above, and stores them in the storage unit 51 (step S1).
- the storage unit 51 stores the first position data D1 generated before the movement of the moving body 1 (position data generated last time) and the second position data D2 generated after the movement of the moving body 1 (current time). Generated position data).
- the second change amount calculation unit 54 obtains the control amount of the moving body 1 from the acquisition of the first position data D1 to the acquisition of the second position data D2, and the movement amount measuring device 6 and / or the moving body. Obtained from the control unit 3 and stored as movement amount data D3.
- the control unit 5 determines the abnormality occurring in the measuring device 4a using the position data D1 and D2 and the movement amount data D3 (step S2).
- the control unit 5 measures the first change amount M1 representing a change in the position and orientation (mounting angle) of the measurement device 4a calculated based on the position data D1 and D2, and the measurement device calculated based on the movement amount data D3. Based on the comparison with the second change amount M2 representing the change in the position and orientation of 4a, the abnormality of the measuring device 4a is determined.
- the abnormality determination process of the measuring device 4a in step S2 will be described in detail later.
- step S2 when it is determined that an abnormality has occurred in the measuring device 4a (in the case of “No” in step S3), the moving body control unit 3 determines that the object B and the moving body 1 It is determined that the positional relationship cannot be accurately grasped. As a result, the process proceeds to step S7 without detecting the object B. If it is determined that an abnormality has occurred in the measurement device 4a, the determination unit 55 notifies the driver of the object detection system 100 to that effect.
- the driver of the moving body 1 that has recognized the abnormality of the measuring apparatus 4a for example, stops the movement of the moving body 1 and adjusts the mounting position of the measuring apparatus 4a as necessary, for example, in the measuring apparatus 4a. You can eliminate the abnormalities.
- an abnormality of the measuring device 4a for example, a window that transmits the measuring light Lm and reflected light Lr of the measuring device 4a is dirty, water droplets are attached, or there are scratches on the window. is there. This is because if the window that transmits the measurement light Lm or the reflected light Lr is dirty or has water droplets attached thereto, or if the window is damaged, the optical path of the measurement light Lm or the reflected light Lr changes from the normal optical path. It is.
- the measurement device 4a can be operated normally by removing dirt or water droplets from the window of the measurement device 4a or replacing the measurement device 4a that has caused an abnormality.
- the moving body control unit 3 determines that the positional relationship between the object B and the moving body 1 can be accurately grasped. . In this case, the moving body control unit 3 performs detection of the object B in the vicinity of the moving body 1 using the second position data D2 generated after the movement of the moving body 1 (step S4). Specifically, the moving body control unit 3 confirms whether there is a coordinate value included in the second position data D2 that has a Z coordinate value equal to or less than a predetermined value.
- the moving body control unit 3 issues a command to apply a brake, a command to set the accelerator opening to 0, and / or a command to change the steering wheel angle so as to avoid a collision with the object B in the vicinity of the moving body 1.
- Output to a driving function such as a power function, a brake, and / or a steering wheel (step S6).
- step S5 When it is determined that no coordinate value having a Z coordinate value equal to or smaller than a predetermined value exists in the second position data D2 and the object B does not exist in the vicinity of the moving body 1 (in the case of “No” in step S5) ) The process proceeds to step S7 without the mobile body control unit 3 performing control on the mobile body 1 so as to avoid the collision with the object B.
- the control unit 5 determines whether or not to stop the operation of the object detection system 100 (step S7). For example, when the stop of the object detection system 100 is instructed by the driver of the moving body 1 or the like (in the case of “Yes” in step S7), the control unit 5 stops the operation of the object detection system 100. On the other hand, when the stop of the object detection system 100 is not instructed (in the case of “No” in step S7), the above steps S1 to S6 are continuously executed.
- the object detection system 100 determines whether or not the object B exists in the vicinity of the moving body 1 when there is no abnormality in the measuring device 4a, and If the object B exists, the moving body 1 can be controlled to avoid a collision with the object B. On the other hand, when an abnormality has occurred in the measuring device 4a, the object detection system 100 can notify the driver of the moving body 1 or the like of an abnormality alarm. As a result, the driver can recognize the abnormality of the measuring device 4a and adjust the measuring device 4a as necessary to eliminate the occurring abnormality.
- FIG. 5 is a flowchart showing an abnormality determination process of the measuring apparatus.
- the moving object 1 is a real space in which an object B1 that is a building that is long in the horizontal direction and an object B2 that is a pedestal arranged in front of the object B1 exist.
- FIG. 6 is a diagram illustrating an example of real space. In the example shown in FIG.
- the reference position O of the moving body 1 at the time t1 moves to the reference position O ′ at the time t2, and the moving body 1 changes the moving direction (that is, the posture) by a predetermined angle. .
- the moving direction that is, the posture
- the first change amount calculation unit 53 calls the coordinate values (first coordinate values P1 to P4) of the object at time t1 from the first position data D1 generated at time t1. Is acquired (step S21).
- FIG. 7 is a diagram illustrating an example of position data before movement.
- Feature point candidates can be extracted using point detection methods such as the Harris method, KLT method, principal curvature method, etc. by expanding the position information into two-dimensional coordinates set on the detection surface DS.
- the reason for selecting the four feature points L1 to L4 is that, as will be described later, the first change amount calculation unit 53 uses the first change amount M1 (first parameter S M ) as a 4 ⁇ 4 matrix. It is for calculating.
- two structures attached to the upper part of the object B1 that is a building are feature points L1 and L2, and two structures on the object B2 that is a base are feature points L3 and L4. Is selected.
- the first change amount calculation unit 53 acquires the coordinate values of the selected feature points as the first coordinate values P1 to P4. For example, the first change amount calculation unit 53 sets the first coordinate value P1 to (X 1 , Y 1 , Z 1 ), the first coordinate value P2 to (X 2 , Y 2 , Z 2 ), and the first coordinate It is assumed that the value P3 is determined as (X 3 , Y 3 , Z 3 ) and the first coordinate value P4 is determined as (X 4 , Y 4 , Z 4 ).
- the first change amount calculation unit 53 searches the feature point corresponding to the feature point selected in the first position data D1 from the second position data D2 generated at the time t2 by image recognition or the like,
- the coordinate values of the corresponding feature points found by the search are acquired as the second coordinate values P1 ′ to P4 ′ after the movement of the moving body 1 in the same manner as in the above step S21 (step S22).
- the second coordinate value P1 ′ is (X 1 ′, Y 1 ′, Z 1 ′)
- the second coordinate value P2 ′ is (X 2 ′, Y 2 ′, Z 2 ′)
- the second coordinate value is 'the (X 3' P3, Y 3 ', Z 3') and 'a (X 4' second coordinate value P4, Y 4 ', Z 4 ') and was determined to.
- the first change amount calculation unit 53 After obtaining the first coordinate values P1 to P4 and the second coordinate values P1 ′ to P4 ′, the first change amount calculation unit 53 obtains the obtained first coordinate values P1 to P4 and the second coordinate values P1 ′ to P4 ′.
- the first change amount M1 representing the change in the position and orientation of the measuring device 4a before and after the moving body 1 is moved is calculated (step S23).
- the first position data D1 and the second position data D2 are respectively expressed as coordinate values of the objects B1 and B2 in the coordinate system (that is, the coordinate system viewed from the measurement device 4a) set in the measurement device 4a.
- the position of the measuring device 4a changes from the attachment position Ps1 to the attachment position Ps1 ′ between time t1 and time t2, as shown in FIG. Further, as the moving body 1 moves, the Z axis representing the normal direction of the detection surface DS of the measuring device 4a changes to the Z ′ axis as shown in FIG. As described above, by changing the position and orientation of the measuring device 4a, the images of the objects B1 and B2 indicated by the first position data D1 (dotted lines in FIGS. 7 and 8) are the second position data D2. It changes as shown by the solid line in FIG. FIG. 8 is a diagram illustrating an example of the position data after movement.
- the moving direction of the measuring device 4a (moving body 1) is opposite to the moving direction of the image indicated by the first position data D1 and the second position data D2.
- the moving body 1 moves to the right in the drawing, while the images of the objects B1 and B2 indicated by the second position data D2 are the objects B1 and B2 indicated by the first position data D1.
- the first change amount calculation unit 53 of the present embodiment moves the coordinate values of the second coordinate values P1 ′ to P4 ′ to the first coordinate values P1 to P4 (that is, indicated by the second position data D2).
- an image of an object B1, B2 which is a mobile and deformed to coordinate conversion to the image of the object B1, B2 shown in a first position data D1) for calculating a first parameter S M.
- the first change amount calculation unit 53 by using the homogeneous coordinate representation in three-dimensional space to calculate a first parameter S M.
- the rotational motion and the translational motion of the three-dimensional space can be expressed by one matrix.
- (X 1 , Y 1 , Z 1 , 1), (X 2 , Y 2 , Z 2 , 1) are respectively obtained from the first coordinate values P1 to P4 acquired in step S22. ), (X 3 , Y 3 , Z 3 , 1) and (X 4 , Y 4 , Z 4 , 1) are generated.
- Equation 1 by multiplying both sides by the inverse matrix of the matrix that is not the first parameter S M on the right side, the first parameter S M (4 ⁇ 4 matrix) as shown in Equation 2 below. Can be calculated as the first change amount M1. Since the right side of Equation 2 includes only the known coordinate values calculated in Steps S21 and S22, the first change amount calculation unit 53 uses Equation 2 to express a specific value represented as a numerical value.
- the first parameter S M (first change amount M1) can be calculated.
- the inverse matrix on the right side of Equation 2 can be calculated by a method of calculating an inverse matrix such as a sweep-out method.
- the first change amount calculation unit 53 a first parameter S M calculated as specific numerical values in the storage unit 51 as the first change amount M1.
- the second change amount calculation unit 54 calculates a second change amount M2.
- the second change amount calculation unit 54 assumes that the mounting position and the mounting angle of the measuring device 4a have not changed before and after the movement of the moving body 1 (that is, between the time t1 and the time t2). Based on changes in position and orientation due to movement of the moving body 1 during t2, and relative relations of the mounting positions Ps1, Ps1 ′ and the mounting angle of the measuring device 4a to the reference positions O, O ′ of the moving body 1. Thus, the second change amount M2 is calculated.
- the above-mentioned “relative relationship of the mounting positions Ps1 and Ps1 ′ of the measuring device 4a and the mounting angle to the reference positions O and O ′ of the moving body 1” refers to the reference positions O and O ′ in the present embodiment. It is defined as the amount of movement when translated to Ps1 and Ps1 ′. Since the normal direction of the detection surface DS of the measuring device 4a in the normal state is determined to be parallel to the straight direction of the moving body 1, the measuring device 4a has the reference positions O and O ′ of the moving body 1 at the mounting angle. There is no relative relationship to.
- the third parameter U is It is defined as Equation 3 below.
- Equation 3 the third parameter U indicated by the measurement device.
- the reference position is the origin (0, 0, 0
- the coordinate value of the mounting position of the measuring device 4a is (0, 0, c)
- Equation 3 Both a and b of the third parameter U indicated by are 0.
- FIG. 9 is a diagram illustrating an example of an attachment position of the measurement device.
- the third parameter U is defined as representing the position and orientation of the measuring device 4a with respect to the moving body 1 at a certain time before the moving body 1 moves. Furthermore, in this embodiment, it is assumed that the attachment position and the attachment angle of the measuring device 4a are not changed before and after the movement of the moving body 1 unless there is an abnormality. Therefore, the third parameter U is a constant value set in advance. That is, the element values a, b, and c of the third parameter U do not change after being set at a certain point in time. Therefore, the third parameter U is set in advance and stored in the storage unit 51.
- the second change amount calculation unit 54 performs an actual measurement value relating to movement from the movement amount measurement device 6 acquired as movement amount data D3 between time t1 and time t2, and Alternatively, the change in the position and orientation of the moving body 1 before and after the movement is calculated from the control amount of the moving body 1 output from the moving body control unit 3 (step S24). For example, when an actual speed value is input from the movement amount measuring device 6, the movement amount data D3 as the speed acquired between the time t1 and the time t2 is accumulated, or the speed is integrated by time. The moving distance of the moving body 1 that has moved between time t1 and time t2 can be calculated.
- the movement distance is calculated by calculating the speed from the acceleration and then integrating the speed by accumulation or time. It can. Further, the moving distance can be calculated as described above from the speed and acceleration calculated from the accelerator opening degree of the moving body 1 output from the moving body control unit 3.
- the movement amount measuring device 6 or the moving body control unit 3 inputs the change in the angle (direction) of the moving body 1 or the steering angle of the steering wheel as the movement amount data D3, the above moving distance is taken into consideration as necessary.
- the angle change of the moving body 1 from the time t1 to the time t2 can be calculated.
- the position in the moving space at the time t1 and the position in the moving space at the time t2 are acquired as the movement amount data D3 from the movement amount measuring device 6, the movement is calculated from the difference between these two positions. Changes in the position and posture of the body 1 can be calculated.
- the moving amount of the reference position of the moving body 1 is defined as a matrix T (referred to as a second parameter T) that moves the reference position O at time t1 to the reference position O ′ at time t2, and the calculated position of the moving body 1
- the second parameter T is calculated as in the following Expression 4, for example.
- ⁇ X, ⁇ Y, and ⁇ Z of each element in the fourth column are respectively the coordinate value of the reference position O of the moving body 1 as ⁇ X in the X-axis direction and ⁇ Y, Z-axis in the Y-axis direction. This corresponds to the movement in the direction by ⁇ Z. That is, if the coordinate value of the reference position O before movement is (Ox, Oy, Oz, 1), it means that the coordinate value of the reference position O ′ after movement is changed to (Ox + ⁇ X, Oy + ⁇ Y, Oz + ⁇ Z, 1). is doing. That is, ⁇ X, ⁇ Y, ⁇ Z of each element in the fourth column corresponds to a change in the position of the moving body 1.
- the other element of the second parameter T represents the rotation around the axis of the coordinate system with the reference positions O and O ′ as the origin, and corresponds to the change in the posture (movement direction) of the moving body 1.
- the second parameter T shown above is a matrix for rotating a coordinate system whose origin is the reference position O by ⁇ around the X axis, ⁇ around the Y axis, and ⁇ around the Z axis (that is, the mobile body 1 is rotated). It is.
- the rotation angles ⁇ , ⁇ , and ⁇ of the second parameter T which direction (right rotation or left rotation around the axis) is positive according to the definition of the X, Y, and Z axes in the coordinate system with the reference position as the origin. The angle can be determined as appropriate.
- the second change amount calculation unit 54 stores the second parameter T calculated as described above in the storage unit 51.
- the second change amount calculation unit 54 reads the second parameter T and the third parameter U from the storage unit 51.
- the second change amount M2 is calculated (step S26).
- the second change amount M2 is, for example, (i) moving the attachment position Ps1 of the measurement device 4a before movement to the reference position O before movement, (ii) changing the reference position O before movement to the reference position after movement (Iii) The reference position O ′ after the movement is moved to the attachment position Ps1 ′ of the measurement apparatus 4a after the movement.
- the second change amount M2 is calculated as follows.
- an expression representing the above operations (i) to (iii) is generated using the second parameter T and the third parameter U.
- the third parameter U and its inverse matrix U ⁇ 1 are preset constant values.
- the second parameter T can be specifically calculated from the change in the position and posture of the moving body 1 before and after the movement in step S24. Therefore, the second change amount calculation unit 54 is expressed as “UTU ⁇ 1 ” using the third parameter U (U ⁇ 1 ) and the second parameter T that has already been specifically calculated in step S24.
- the second change amount M2 can be calculated specifically.
- the second change amount calculation unit 54 stores the second change amount M2 calculated as described above in the storage unit 51.
- the determination unit 55 determines whether or not an abnormality has occurred in the measurement device 4a based on the difference between the first change amount M1 and the second change amount M2. Is determined (step S27).
- the relationship between the abnormality of the measuring device 4a and the difference between the first change amount M1 and the second change amount M2 will be described.
- the positional relationship between the detection surface DS and the reference positions O and O ′ of the moving body 1 and the angle of the detection surface DS with respect to the normal direction in the normal direction do not change before and after the movement of the moving body 1.
- the first change amount M1 (first parameter S M ) calculated as a matrix for moving the second coordinate values P1 ′ to P4 ′ to the first coordinate values P1 to P4 is the second parameter T and the third parameter.
- FIG. 10 is a diagram illustrating an example of a state in which the mounting state of the measuring device is changed.
- FIG. 11 is a diagram illustrating an example of the second position data acquired after the mounting state of the measuring device is changed.
- the images of the objects B1 and B2 indicated in the second position data D2 ′ acquired when the mounting state of the measuring device 4a changes are acquired when the mounting state does not change.
- the image is shifted further to the left side of the paper than the images of the objects B1 and B2 indicated by the second position data D2.
- the first parameter S M ′ (first change) calculated as a matrix for moving the second coordinate values P1 ′′ to P4 ′′ selected from the second position data D2 ′ to the first coordinate values P1 to P4.
- the amount M1 ′) is different from the first parameter S M (first change amount M1) calculated when the mounting state does not change. That is, when the mounting state of the measuring device 4a changes before and after the movement, the first change amount M1 ′ does not coincide with the second change amount M2.
- the dirt and scratches may bend the optical path of the measurement light Lm and reflected light Lr, These lights are scattered (refracted).
- dirt and scratches on the optical member produce the same effect as changing the mounting position and mounting angle of the measuring device 4a from the state before the dirt and scratches are attached to the optical member. Accordingly, whether or not the measuring device 4a is faulty or abnormal, such as contamination of the optical member, can be determined based on whether or not the first change amounts M1 and M1 ′ match the second change amount M2.
- the determination unit 55 the first parameter S M, S M ', the second parameter T, and is generated from the third parameter U, the first parameter S M representing a first change amount M1, S M 'And a determinant (det (S M -UTU -1 )) of a difference matrix S M -UTU -1 which is a value calculated as a difference between the matrix UTU -1 representing the second change amount M2 and It is determined whether or not the first change amount M1 and the second change amount M2 match (that is, whether or not there is an abnormality in the measurement device 4a).
- the determination unit 55 determines whether the first change amount M1 and the second change amount M2 are Are determined to match, and the measurement device 4a is determined to be normal. Thereafter, the abnormality determination process ends.
- the predetermined threshold value ⁇ is appropriately determined in consideration of the measurement error of the measuring device 4a and the moving amount measuring device 6, the allowable mounting position and the mounting angle of the measuring device 4a, and the like. it can.
- the determination unit 55 determines the first change amount M1 and the second change amount. It is determined that M2 does not match, and it is determined that an abnormality has occurred in the measuring device 4a. At this time, the determination unit 55, for example, sounds a warning sound using the speaker of the control unit 5, displays a message notifying that the mounting state has changed on the display of the control unit 5 or the like, or turns on a warning lamp. Or the driver is notified that the mounting state of the measuring device 4a has changed (step S28). Thereafter, the attachment state determination process ends.
- the determination unit 55 may calculate the degree of abnormality occurring in the measurement apparatus 4a using the determinant det (S M ⁇ UTU ⁇ 1 ). For example, the relationship between the change in the value of the determinant det (S M ⁇ UTU ⁇ 1 ) and the change in the value of the attachment position and the attachment angle of the measuring device 4a is acquired in advance and stored in the storage unit 51, and the matrix When the expression det (S M ⁇ UTU ⁇ 1 ) is equal to or greater than a predetermined threshold ⁇ , the determination unit 55 determines whether the change in the value of the determinant det (S M ⁇ UTU ⁇ 1 ) and the attachment position of the measuring device 4a Using the relationship with the change in the value of the mounting angle, the mounting position of the measuring device 4a and the mounting angle from a certain point in time or the degree of abnormality of the measuring device 4a (for example, contamination of the optical member or adhesion of scratches) Degree) can be calculated.
- the determinant det S M
- the object detection system 100 is not difficult information such as the timing at which the object is no longer observed by the measuring device 4a, but the first coordinate values P1 to P4 and the second coordinate values.
- Changes in the position and orientation of the measuring device 4a calculated from changes in measured values by the measuring device 4a of P1 ′ to P4 ′ that is, first parameters S M , S M ′, first change amounts M1, M1 ′
- the change in the position and orientation of the measuring device 4a calculated based on the change in the position and orientation of the moving body 1 before and after movement (ie, the second parameter T) and the third parameter U (ie, the second change amount M2).
- the abnormality of the measuring device 4a is determined based on information that is easily acquired by the object detection system 100 without difficulty. Thereby, the attachment state of the measuring apparatus 4a can be grasped more accurately.
- the method for determining the abnormality of the measuring device 4a has been described.
- the other measuring devices 4b to 4d can similarly determine the abnormality.
- the second change amount M2 (matrix UTU ⁇ 1 ) represents the position and orientation of each measuring device 4b to 4d with respect to a predetermined moving body. Calculation is performed using the matrix U.
- the control unit 5 always obtains the second parameter T and the second change amount M2 while the moving body 1 is moving.
- the abnormality determination of the measuring devices 4a to 4d is executed.
- the present invention is not limited to this.
- the second parameter T and the second change amount T2 are used as the second parameter T and the second change amount M2 used in the previous determination.
- the calculation of the two change amounts M2 may be omitted.
- abnormality determination is performed using the first change amount M1 and the second change amount M2.
- the determination may be made by other methods.
- the control unit 5 instead of the first change amount calculation unit 53 and the second change amount calculation unit 54 (FIG. 3A) in the control unit 5 of the first embodiment, the control unit 5 has a coordinate prediction unit 57 as shown in FIG. 3B. You may have.
- FIG. 3B is a diagram illustrating a configuration of a control unit according to another embodiment.
- the coordinate prediction unit 57 moves by using the first coordinate value representing the position of the object measured before the movement, the position information of the measuring device with respect to the moving body, and the amount of change in the position and orientation of the reference position before and after the movement.
- the position of the subsequent object is predicted (predicted coordinate value) and compared with the second coordinate value representing the actual position of the moved object.
- the difference may be calculated as a distance between two coordinate values. According to this method, since comparison is performed using coordinate values, it is easy to evaluate the comparison result, and the number of feature points to be selected does not have to be four.
- the present invention can be widely applied to abnormality determination of a measuring device in an object detection system that detects an object existing around a moving body such as a vehicle.
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
周囲に存在する物体を検出しながら移動する物体検知システムにおいて、物体を検出する測定装置の異常を的確に把握する。物体検知システムは、移動体と共に移動し、移動体の周囲に存在する物体の特徴点の測定装置(4a~4d)から見た位置を表す座標値を測定する測定装置(4a~4d)と、第1座標値と第2座標値との間の変化に基づいて測定装置(4a~4d)の位置及び姿勢の変化を表す第1変化量(M1、M1')を算出する第1変化量算出部(53)と、移動前後における移動体の位置及び姿勢の変化に基づいて測定装置(4a~4d)の位置及び姿勢の変化を表す第2変化量(M2)を算出する第2変化量算出部(54)と、第1変化量(M1、M1')と第2変化量(M2)とを比較することにより測定装置(4a~4d)の異常を判定する判定部(55)と、を備える。
Description
本発明は、移動体に取り付けられ、移動しながら周囲に存在する物体を検出する物体検知装置において、当該物体検知装置の異常を判定するシステム、方法及びプログラムに関する。
従来、自動車などの車両(移動体)の周囲に存在する物体を検出して、さらに検出された物体と移動体との位置関係に基づいて、移動体の移動を制御するシステムが知られている。このようなシステムでは、検出された物体と移動体との位置関係を正確に判断するためには、物体を検出する測定装置の移動体への取付状態を把握することが重要である。
例えば、特許文献1には、自動車に取り付けられた対象物検出装置に関して、レーザ光の投光方向のずれを検知する方法が開示されている。当該方法においては、レーザ光が頭上標識に反射されなくなって頭上標識を検出できなくなった瞬間における対象物検出装置と頭上標識との間の距離を算出し、それに基づいて対象物検出装置が元の取付角度からどの程度ずれたかを検知している。
上記の特許文献1の検知方法では、投光したレーザ光が頭上標識にて反射されず当該レーザ光が検出できなくなったタイミングを捉えているので、例えば、頭上標識が連続して存在する場合には、取付角度のずれを検知することが困難であった。また、移動体の移動中に、レーザ光を検出できなくなったタイミングを正確に検知することは難しく、そのため現在の取付角度を正確に把握することは難しかった。
また、特許文献1の検知方法では、頭上標識に着目しているため、正面に頭上標識が存在しておらず道路脇にしか物体がない場合や、道路が直進でなく曲がっている場合には使用することができなかった。
また、特許文献1の検知方法では、頭上標識に着目しているため、正面に頭上標識が存在しておらず道路脇にしか物体がない場合や、道路が直進でなく曲がっている場合には使用することができなかった。
本発明の課題は、周囲に存在する物体を検出しながら移動する物体検知システムにおいて、物体を検出する測定装置の異常を的確に把握することにある。
以下に、課題を解決するための手段として複数の態様を説明する。これら態様は、必要に応じて任意に組み合せることができる。
本発明の一見地に係る物体検知システムは、移動体と、測定装置と、座標予測部と、判定部と、を備える。移動体は、移動可能な物体である。測定装置は、移動体に取り付けられ、移動体と共に移動する。測定装置は、移動体の周囲に存在する物体の測定装置から見た位置を表す座標値を測定する。
座標予測部は、第1座標値、移動体に対する測定装置の位置情報、及び移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の移動体から見た物体の位置を表す予測座標値を算出する。第1座標値は、移動体の移動前において測定された物体の位置を表す。第2座標値は、移動体の移動後において測定された物体の位置を表す。
本発明の一見地に係る物体検知システムは、移動体と、測定装置と、座標予測部と、判定部と、を備える。移動体は、移動可能な物体である。測定装置は、移動体に取り付けられ、移動体と共に移動する。測定装置は、移動体の周囲に存在する物体の測定装置から見た位置を表す座標値を測定する。
座標予測部は、第1座標値、移動体に対する測定装置の位置情報、及び移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の移動体から見た物体の位置を表す予測座標値を算出する。第1座標値は、移動体の移動前において測定された物体の位置を表す。第2座標値は、移動体の移動後において測定された物体の位置を表す。
上記の物体検知システムでは、移動体の移動中に、測定装置が、ある所定の時刻(移動体の移動前)における物体の測定装置から見た位置を第1座標値として、当該所定の時刻からある程度の時間が経過後(移動体の移動後)の物体の測定装置から見た位置を第2座標値として測定する。次に、移動体の移動前において測定された物体の位置を表す第1座標値、移動体に対する測定装置の位置情報、及び移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の移動体から見た物体の位置を表す予測座標値を算出する。
その後、判定部が、予測座標値と第2座標値とを比較して、測定装置に発生している異常を判定する。
その後、判定部が、予測座標値と第2座標値とを比較して、測定装置に発生している異常を判定する。
上記の物体検知システムでは、測定装置にて物体が観測されなくなるタイミングといった把握が困難な情報ではなく、物体の位置の測定装置による測定値の変化から算出される測定装置の位置及び姿勢の変化量や、移動体の位置及び姿勢の変化から算出される測定装置の位置及び姿勢の変化量といった把握しやすい情報に基づいて、測定装置の異常を判定している。これにより、測定装置の異常をより的確に把握できる。
物体検知システムは、移動体制御部をさらに備えていてもよい。移動体制御部は、移動体の移動を制御する。この場合、移動前後における移動体の位置及び姿勢の変化は、移動体制御部における移動体の制御量に基づいて算出される。これにより、比較的容易に移動体の位置及び姿勢の変化を算出できる。
物体検知システムは、移動量測定装置をさらに備えていてもよい。移動量測定装置は、移動体の移動前後の移動を表す量を測定する。これにより、移動前後の移動体の位置及び姿勢の変化を、移動に関する実測値に基づいて決定できる。
移動体の移動前後の位置の変化は、移動体の移動前後における、移動体に対して固定された基準位置の変化として表されてもよい。この場合、第2変化量は、移動体の移動前後における基準位置の変化と、測定装置の取付位置及び姿勢の基準位置に対する相対的な関係と、に基づいて算出される。これにより、第2変化量を比較的簡単に算出できる。
取付位置と基準位置との位置関係は一定であってもよい。これにより、測定装置の取付位置が固定であることを前提として、第2変化量を算出できる。その結果、第1変化量と第2変化量との比較が容易になる。
本発明の他の見地に係る物体検知システムは、移動体と、第1パラメタ算出部と、第2パラメタ算出部と、記憶部と、判定部と、を備える。移動体は、移動可能な物体である。測定装置は、移動体に取り付けられ、移動体と共に移動する測定装置である。測定装置は、移動体の周囲に存在する物体の測定装置から見た位置を表す座標値を測定する。第1パラメタ算出部は、第1パラメタを算出する。第1パラメタは、第1座標値と第2座標値との間の変化に基づいて算出される、測定装置の位置及び姿勢の変化を表すパラメタである。第1座標値は、移動体の移動前において測定された物体の位置を表す。第2座標値は、移動体の移動後において測定された物体の位置を表す。
第2パラメタ算出部は、第2パラメタを算出する。第2パラメタは、移動前後における移動体の位置および姿勢の変化を表すパラメタである。記憶部は、第3パラメタを記憶する。第3パラメタは、移動体が移動するより前のある時点における、移動体に対する測定装置の位置及び姿勢を表すパラメタである。判定部は、第1パラメタ、第2パラメタ、及び第3パラメタに基づいて、測定装置の取付位置及び姿勢のある時点に対する変化を判定する。
上記の物体検知システムでは、測定装置にて物体が観測されなくなるタイミングといった把握が困難な情報ではなく、第1パラメタ、第2パラメタ、及び第3パラメタといった把握しやすい情報に基づいて、測定装置の取付位置及び姿勢のある時点に対する変化を判定している。これにより、測定装置の取付位置及び姿勢の変化を、より精度よく把握できる。
判定部は、測定装置の取付位置及び姿勢のある時点に対する変化の有無または変化の度合いを算出してもよい。これにより、測定装置の取付状態がどの程度変化したかを判定できる。
判定部は、第1パラメタ、第2パラメタ、及び第3パラメタから生成される行列の行列式の値に基づいて、変化の有無または変化の度合いを算出してもよい。これにより、測定装置の取付状態がどの程度変化したかを数値化できる。
移動前後の移動体の位置の変化は、移動体の移動前後における、移動体に対して固定された基準位置の変化として表されてもよい。これにより、第2パラメタを比較的簡単に算出できる。
本発明のさらに他の見地に係る異常判定方法は、移動可能な物体である移動体に取り付けられた測定装置の異常判定方法である。異常判定方法は、以下のステップを含む。
◎移動体の移動前に、移動体の周囲に存在する物体の測定装置から見た位置を表す第1座標値を測定装置に測定させるステップ。
◎移動体の移動後に、物体の測定装置から見た位置を表す第2座標値を測定装置に測定させるステップ。
◎第1座標値と、移動体に対する測定装置位置情報と移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の移動体から見た物体の位置を表す予測座標値を算出するステップと、
◎予測座標値と第2座標値とを比較することにより、測定装置の異常を判定するステップ。
◎移動体の移動前に、移動体の周囲に存在する物体の測定装置から見た位置を表す第1座標値を測定装置に測定させるステップ。
◎移動体の移動後に、物体の測定装置から見た位置を表す第2座標値を測定装置に測定させるステップ。
◎第1座標値と、移動体に対する測定装置位置情報と移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の移動体から見た物体の位置を表す予測座標値を算出するステップと、
◎予測座標値と第2座標値とを比較することにより、測定装置の異常を判定するステップ。
上記の異常判定方法では、測定装置にて物体が観測されなくなるタイミングといった把握が困難な情報ではなく、物体の位置の測定装置による測定値の変化から算出される測定装置の位置及び姿勢の変化量や、移動体の位置及び姿勢の変化から算出される測定装置の位置及び姿勢の変化量といった把握しやすい情報に基づいて、測定装置の異常を判定している。これにより、測定装置の異常を的確に把握できる。
本発明のさらに他の見地に係る異常判定方法は、移動可能な物体である移動体に取り付けられた測定装置の取付状態の異常判定方法である、異常判定方法は、以下のステップを含む。
◎移動体の移動前に、移動体の周囲に存在する物体の測定装置から見た位置を表す第1座標値を測定装置に測定させるステップ。
◎移動体の移動後に、物体の測定装置から見た位置を表す第2座標値を測定装置に測定させるステップ。
◎第1座標値と第2座標値との間の変化に基づいて、測定装置の位置及び姿勢の変化を表す第1パラメタを算出するステップ。
◎移動体の移動前後における移動体の位置及び姿勢の変化を表す第2パラメタを算出するステップ。
◎移動体が移動するより前のある時点において記憶部に記憶され、移動体に対する測定装置の位置を表す第3パラメタを、記憶部から読み出すステップ。
◎第1パラメタ、第2パラメタ、及び第3パラメタに基づいて、測定装置の取付位置及び姿勢のある時点に対する変化を判定するステップ。
◎移動体の移動前に、移動体の周囲に存在する物体の測定装置から見た位置を表す第1座標値を測定装置に測定させるステップ。
◎移動体の移動後に、物体の測定装置から見た位置を表す第2座標値を測定装置に測定させるステップ。
◎第1座標値と第2座標値との間の変化に基づいて、測定装置の位置及び姿勢の変化を表す第1パラメタを算出するステップ。
◎移動体の移動前後における移動体の位置及び姿勢の変化を表す第2パラメタを算出するステップ。
◎移動体が移動するより前のある時点において記憶部に記憶され、移動体に対する測定装置の位置を表す第3パラメタを、記憶部から読み出すステップ。
◎第1パラメタ、第2パラメタ、及び第3パラメタに基づいて、測定装置の取付位置及び姿勢のある時点に対する変化を判定するステップ。
上記の異常判定方法では、測定装置にて物体が観測されなくなるタイミングといった把握が困難な情報ではなく、第1パラメタ、第2パラメタ、及び第3パラメタといった把握しやすい情報に基づいて、測定装置の取付位置及び姿勢のある時点に対する変化を判定している。これにより、測定装置の取付位置及び姿勢の変化を的確に把握できる。
本発明のさらに他の見地に係るプログラムは、上記の異常判定方法をコンピュータに実行させるプログラムである。
周囲に存在する物体を検出しながら移動する物体検知システムにおいて、周囲に存在する物体を検出する測定装置の異常を的確に把握できる。
1.第1実施形態
(1)物体検知システムの構成
以下、第1実施形態に係る物体検知システム100の構成について、図1を用いて説明する。図1は、物体検知システムの構成を示す図である。第1実施形態に係る物体検知システム100は、例えば、自動車などの各種車両である移動体1の運転者による操作をアシストするシステムである。
(1)物体検知システムの構成
以下、第1実施形態に係る物体検知システム100の構成について、図1を用いて説明する。図1は、物体検知システムの構成を示す図である。第1実施形態に係る物体検知システム100は、例えば、自動車などの各種車両である移動体1の運転者による操作をアシストするシステムである。
物体検知システム100は、移動体1を備える。移動体1は、移動可能な物体であって、例えば車両である。移動体1は、車輪2a、2b、2c、2dを備える。車輪2a、2bは、移動体1の直進方向(図1)の前部において、動力機能(例えば、エンジン及び/又は電動モータ)の出力回転軸に、減速機構を介して軸回りに回転可能に取り付けられている。一方、車輪2c、2dは、移動体1の直進方向の後部に、軸回りに回転可能に取り付けられている。
物体検知システム100は、4つの測定装置4a、4b、4c、4dを備える。図1に示すように、測定装置4aは、移動体1の直進方向の最前部に取り付けられ、移動体1の前方に存在する物体を検出する。測定装置4bは、移動体1の直進方向の最後部に取り付けられ、移動体1の後方に存在する物体を検出する。測定装置4cは、移動体1の直進方向の左側面に取り付けられ、移動体1の左側方に存在する物体を検出する。測定装置4dは、移動体1の直進方向の右側面に取り付けられ、移動体1の右側方に存在する物体を検出する。4つの測定装置4a~4dは、移動体1に取り付けられているので、移動体1が移動すると、当該移動体の移動と共に移動する。
本実施形態において、測定装置4a~4dは、検出対象の物体Bまでの距離を測定するTOF(Time Of Flight)センサである。しかし、測定装置はこれに限られず、例えば、左右2つのカメラの画像差から距離を測定するステレオカメラ、レーザレンジファインダ(Laser Range Finder、LRF)などの他の方式の測距センサを用いてもよい。本実施形態における測定装置4a~4dの構成については、後ほど詳しく説明する。
物体検知システム100は、制御部5を備える。制御部5は、CPU(Central Processing Unit)と、記憶装置(RAM(Random Access Memory)、ROM(Read Only Memory)、SSD(Solid State Drive)又はHDD(Hard Disk Drive)など)と、各種インターフェース(例えば、A/D、D/A変換器など)を備えたコンピュータシステムである。
制御部5は、測定装置4a~4dからの検出信号を入力し、当該検出信号に基づいて、周囲に存在する物体Bと移動体1との間の位置関係を決定する。制御部5は、移動体制御部3と接続されており、さらに移動体制御部3は車輪2a、2bに設けられたブレーキの制動機能、動力機能、及び/又は、移動体1の移動方向を変更するハンドルの回転機能などの走行機能に接続されている。制御部5は、物体Bと移動体1との間の位置関係情報を出力し、それを受け取った移動体制御部3は必要に応じて物体検知システム100の運転者に代わって、走行機能を制御する。制御部5、移動体制御部3の構成については、後ほど詳しく説明する。
制御部5は、測定装置4a~4dからの検出信号を入力し、当該検出信号に基づいて、周囲に存在する物体Bと移動体1との間の位置関係を決定する。制御部5は、移動体制御部3と接続されており、さらに移動体制御部3は車輪2a、2bに設けられたブレーキの制動機能、動力機能、及び/又は、移動体1の移動方向を変更するハンドルの回転機能などの走行機能に接続されている。制御部5は、物体Bと移動体1との間の位置関係情報を出力し、それを受け取った移動体制御部3は必要に応じて物体検知システム100の運転者に代わって、走行機能を制御する。制御部5、移動体制御部3の構成については、後ほど詳しく説明する。
物体検知システム100は、移動量測定装置6を備える。移動量測定装置6は、移動体1の移動を表す量を測定する。移動量測定装置6としては、例えば、移動体1の速度を測定する速度センサ、加速度を測定する加速度センサ、及び/又は、車輪2a~2dの回転量を測定するエンコーダ、移動体1の角速度を測定するジャイロセンサーなどを用いることができる。その他、移動量測定装置6として、GPS(Grobal Positioning System)装置を用いてもよい。
物体検知システム100が移動量測定装置6を備えることにより、移動前後の移動体1の位置及び姿勢の変化を、移動に関する実測値に基づいて決定できる。
物体検知システム100が移動量測定装置6を備えることにより、移動前後の移動体1の位置及び姿勢の変化を、移動に関する実測値に基づいて決定できる。
制御部5は、移動体1の移動前後において測定装置4a~4dにて取得されたデータから生成される位置データD1、D2に基づいて算出される、移動体1の移動前後の測定装置4a~4dの位置及び姿勢の変化量(第1変化量M1と呼ぶ)と、移動前後の移動体1の位置及び姿勢の変化に基づいて算出される、移動体1の移動前後の測定装置4a~4dの位置及び姿勢の変化量(第2変化量M2と呼ぶ)との比較に基づいて、測定装置4a~4dの異常を判定する。
本実施形態において判定される測定装置4a~4dとしては、例えば、測定装置4a~4dの移動体1への取付状態の変化、及び、測定装置4a~4dの故障がある。
判定される測定装置4a~4dの移動体1への取付状態の変化としては、例えば、測定装置4a~4dの移動体1における取付位置及び取付角度の変化や、測定装置4a~4dの移動体1への固定度合いなどがある。
一方、判定される測定装置4a~4dの故障としては、例えば、測定装置4a~4d内部における出力部41や検出面DS(後述)の取付位置のずれや、測定装置4a~4dの検出窓(後述する測定光Lmや反射光Lrを透過させる窓)の汚れや傷などがある。
制御部5における測定装置4a~4dの異常判定方法については、後ほど詳しく説明する。
判定される測定装置4a~4dの移動体1への取付状態の変化としては、例えば、測定装置4a~4dの移動体1における取付位置及び取付角度の変化や、測定装置4a~4dの移動体1への固定度合いなどがある。
一方、判定される測定装置4a~4dの故障としては、例えば、測定装置4a~4d内部における出力部41や検出面DS(後述)の取付位置のずれや、測定装置4a~4dの検出窓(後述する測定光Lmや反射光Lrを透過させる窓)の汚れや傷などがある。
制御部5における測定装置4a~4dの異常判定方法については、後ほど詳しく説明する。
上記の構成を備えることにより、物体検知システム100は、測定装置4a~4dにより検出された物体Bと移動体1との位置関係に基づいて、移動体1の運転者による運転をアシストできる。また、物体検知システム100においては、移動体1の走行中に、測定装置4a~4dの異常を判定できる。
(2)測定装置の構成
次に、測定装置4a~4dの構成について、図2を用いて説明する。図2は、測定装置の構成を示す図である。4つの測定装置4a~4dは同じ構成を有しているので、測定装置4aの構成を例にとって以下説明する。
測定装置4aは、出力部41を有する。出力部41は、例えば、検出対象である物体Bに向けて、赤外領域の測定光Lmを出力する光源である。図2に示すように、本実施形態において、出力部41は、物体検知システム100が移動する空間の広い範囲に広がった測定光Lmを出力することが好ましい。なぜなら、測定装置4aは、物体検知システム100が移動する空間の広い範囲にある物体Bを同時に検出できるからである。
次に、測定装置4a~4dの構成について、図2を用いて説明する。図2は、測定装置の構成を示す図である。4つの測定装置4a~4dは同じ構成を有しているので、測定装置4aの構成を例にとって以下説明する。
測定装置4aは、出力部41を有する。出力部41は、例えば、検出対象である物体Bに向けて、赤外領域の測定光Lmを出力する光源である。図2に示すように、本実施形態において、出力部41は、物体検知システム100が移動する空間の広い範囲に広がった測定光Lmを出力することが好ましい。なぜなら、測定装置4aは、物体検知システム100が移動する空間の広い範囲にある物体Bを同時に検出できるからである。
測定装置4aは、複数の検出部43-1、43-2、・・・43-nを有する。複数の検出部43-1~43-nのそれぞれは、例えば、検出面DS(半導体基板)上の所定の位置に配置され、測定光Lmが物体Bにて反射されることにより発生する反射光Lrを検出する。検出部43-1~43-n、例えば、電荷結合素子(Charge Coupled Device)、又は、CMOS(Complementary MOS)素子である。
また、図2に示すように、複数の検出部43-1~43-nは、検出面DS上の上下方向及び左右方向に配置されてアレイを形成している。これにより、複数の検出部43-1~43-nは、検出面DS上に、CCDイメージセンサ又はCMOSイメージセンサを形成できる。
複数の検出部43-1~43-nのそれぞれには、当該検出部と外部の制御部5とを接続/切断するためのスイッチング素子(例えば、MOS-FET)が接続されている。また、スイッチング素子にはアドレス線が接続されており、アドレス線に信号が印加されたときに、当該スイッチング素子はONされ、ONされたスイッチング素子に接続された検出部と、制御部5とが信号送受信可能となる。
複数の検出部43-1~43-nのそれぞれには、当該検出部と外部の制御部5とを接続/切断するためのスイッチング素子(例えば、MOS-FET)が接続されている。また、スイッチング素子にはアドレス線が接続されており、アドレス線に信号が印加されたときに、当該スイッチング素子はONされ、ONされたスイッチング素子に接続された検出部と、制御部5とが信号送受信可能となる。
測定装置4aは、レンズ45を有している。レンズ45は、反射光Lrを、検出面DSのうち複数の検出部43-1~43-nが形成された領域に集光する。これにより、広い範囲にある物体Bの像を、複数の検出部43-1~43-nが形成された領域に結像できる。
上記の構成を有することにより、測定装置4aは、物体検知システム100が移動する実空間にある物体Bを、所定の座標上に投影してデータを取得できる。
(3)制御部の構成
以下、本実施形態に係る物体検知システム100の制御部5の構成を、図3Aを用いて説明する。図3Aは、制御部の構成を示す図である。以下に説明する制御部5の各要素の機能の一部又は全部は、制御部5を構成するコンピュータシステムにて実行可能なプログラムとして実現されていてもよい。このとき、当該プログラムは、コンピュータシステムの記憶装置に形成された記憶領域に記憶されていてもよい。また、制御部5の各要素の機能の一部又は全部は、カスタムICなどによりハードウェア的に実現されていてもよい。
制御部5は、記憶部51を有する。記憶部51は、各種データを記憶する、例えば、コンピュータシステムの記憶装置に設けられた記憶領域の一部である。
以下、本実施形態に係る物体検知システム100の制御部5の構成を、図3Aを用いて説明する。図3Aは、制御部の構成を示す図である。以下に説明する制御部5の各要素の機能の一部又は全部は、制御部5を構成するコンピュータシステムにて実行可能なプログラムとして実現されていてもよい。このとき、当該プログラムは、コンピュータシステムの記憶装置に形成された記憶領域に記憶されていてもよい。また、制御部5の各要素の機能の一部又は全部は、カスタムICなどによりハードウェア的に実現されていてもよい。
制御部5は、記憶部51を有する。記憶部51は、各種データを記憶する、例えば、コンピュータシステムの記憶装置に設けられた記憶領域の一部である。
制御部5は、位置データ生成部52を有する。位置データ生成部52は、位置データD1、D2を生成する。位置データD1、D2は、測定装置4aにて検出された物体Bの各測定装置から見た相対的な位置を表す座標値の集合体として生成される。
位置データ生成部52は、具体的には、以下のようにして位置データD1、D2を生成する。以下、測定装置4aの位置データD1、D2を生成する場合を例に説明する。
位置データ生成部52は、最初に、1番目の検出部43-1に対応するアドレス線に信号を印加することで、1番目の検出部43-1と位置データ生成部52とを接続し、検出部43-1が反射光Lrを検出したか否かを示す信号(例えば、電流又は電圧信号)を入力する。当該信号を入力した後、位置データ生成部52は、反射光Lrを検出したか否かを示す信号を入力した時刻と、出力部41が測定光Lmを出力した時刻と、の差分を信号検知情報T1として算出する。
なお、反射光Lrが検出されなかった場合には、検出部43-1からの信号の入力がないので、信号検知情報T1は無限大(あるいは非常に大きな値)に設定される。
位置データ生成部52は、具体的には、以下のようにして位置データD1、D2を生成する。以下、測定装置4aの位置データD1、D2を生成する場合を例に説明する。
位置データ生成部52は、最初に、1番目の検出部43-1に対応するアドレス線に信号を印加することで、1番目の検出部43-1と位置データ生成部52とを接続し、検出部43-1が反射光Lrを検出したか否かを示す信号(例えば、電流又は電圧信号)を入力する。当該信号を入力した後、位置データ生成部52は、反射光Lrを検出したか否かを示す信号を入力した時刻と、出力部41が測定光Lmを出力した時刻と、の差分を信号検知情報T1として算出する。
なお、反射光Lrが検出されなかった場合には、検出部43-1からの信号の入力がないので、信号検知情報T1は無限大(あるいは非常に大きな値)に設定される。
次に、位置データ生成部52は、位置情報(x1,y1,T1)を生成する。検出部43-1の検出面DSにおける配置位置を定義した座標値(x1,y1)と、上記の信号検知情報T1とを関連付けることで、位置情報を生成できる。
その後、位置データ生成部52は、信号を印加するアドレス線を順次変更することにより、上記の工程を他の全ての検出部43-2~43-nに対して実行して、n個の位置情報の集合体(x1、y1、T1)、(x2、y2、T2)、・・・(xn、yn、Tn)を生成する。
その後、位置データ生成部52は、信号を印加するアドレス線を順次変更することにより、上記の工程を他の全ての検出部43-2~43-nに対して実行して、n個の位置情報の集合体(x1、y1、T1)、(x2、y2、T2)、・・・(xn、yn、Tn)を生成する。
さらにその後、位置データ生成部52は、上記のn個の位置情報の集合体(x1、y1、T1)、(x2、y2、T2)、・・・(xn、yn、Tn)のそれぞれを、移動体1が移動する実空間を表す座標系における座標値に変換して、位置データD1、D2を生成する。
具体的には、p番目の位置情報(xp,yp,Tp)を上記の座標値に変換する場合を例にとると、位置データ生成部52は、まず、信号検知情報Tp(時間の単位を有する情報)を、出力部41から測定光Lmを出力した時刻を0として、検出面DSから実空間に存在する物体Bまでの距離zp(=c*Tp/2、c:光速)に変換する。次に、位置データ生成部52は、検出面DSにおいて定義された座標値(xp,yp)を、例えば、距離zpや(レンズ45の)焦点距離などに基づいて決定される拡大率で拡大することにより、測定装置4aを原点とし実空間を表す座標系における物体Bの座標値を(Xp,Yp)と算出する。また、Z軸方向の座標値をZp=zpとして、最終的に、(Xp,Yp,Zp)との座標値を算出できる。
当該座標値の変換を、n個の位置情報の全てに対して実行することにより、位置データD1、D2が得られる。
具体的には、p番目の位置情報(xp,yp,Tp)を上記の座標値に変換する場合を例にとると、位置データ生成部52は、まず、信号検知情報Tp(時間の単位を有する情報)を、出力部41から測定光Lmを出力した時刻を0として、検出面DSから実空間に存在する物体Bまでの距離zp(=c*Tp/2、c:光速)に変換する。次に、位置データ生成部52は、検出面DSにおいて定義された座標値(xp,yp)を、例えば、距離zpや(レンズ45の)焦点距離などに基づいて決定される拡大率で拡大することにより、測定装置4aを原点とし実空間を表す座標系における物体Bの座標値を(Xp,Yp)と算出する。また、Z軸方向の座標値をZp=zpとして、最終的に、(Xp,Yp,Zp)との座標値を算出できる。
当該座標値の変換を、n個の位置情報の全てに対して実行することにより、位置データD1、D2が得られる。
制御部5は、第1変化量算出部53(第1パラメタ算出部の一例)を有する。第1変化量算出部53は、移動体1の移動前において測定装置4aにより測定された物体の座標値である第1座標値と、移動体1の移動後において測定装置4aにより測定された物体の座標値である第2座標値との間の変化に基づいて、移動体1の移動前後の測定装置4aの位置及び姿勢の変化を表す第1変化量M1を算出する。
具体的には、第1変化量算出部53は、移動体1の移動前に生成された第1位置データD1の中から第1座標値を選択し、移動体1の移動後に生成された第2位置データD2の中から第2座標値を選択し、第1座標値と第2座標値との間の変化量を、第1変化量M1として算出する。第1変化量算出部53における第1変化量M1の算出方法については、後ほど詳しく説明する。
具体的には、第1変化量算出部53は、移動体1の移動前に生成された第1位置データD1の中から第1座標値を選択し、移動体1の移動後に生成された第2位置データD2の中から第2座標値を選択し、第1座標値と第2座標値との間の変化量を、第1変化量M1として算出する。第1変化量算出部53における第1変化量M1の算出方法については、後ほど詳しく説明する。
制御部5は、第2変化量算出部54(第2パラメタ算出部の一例)を有する。第2変化量算出部54は、移動体1の移動前後における移動体1の位置及び姿勢の変化に基づいて、移動体1の移動前後の測定装置4aの位置及び姿勢の変化を表す第2変化量M2を算出する。
具体的には、第2変化量算出部54は、移動体1に対して固定された基準位置の移動体1の移動前後における変化と、測定装置4a~4dの取付位置Ps1~Ps4及び姿勢の基準位置に対する相対的な位置関係とに基づいて、第2変化量M2を算出する。
具体的には、第2変化量算出部54は、移動体1に対して固定された基準位置の移動体1の移動前後における変化と、測定装置4a~4dの取付位置Ps1~Ps4及び姿勢の基準位置に対する相対的な位置関係とに基づいて、第2変化量M2を算出する。
本実施形態において、第2変化量M2の算出に用いる測定装置4aの取付位置及び/姿勢(取付角度)の基準位置に対する相対的な関係は、取り付けた時点で決定され、異常がない限り変化しない。また、本実施形態において、第2変化量算出部54は、移動量測定装置6にて測定された移動体1の加速度、速度、及び/又は位置の実測値に基づいて、移動体1の移動前後の移動量を算出する。これにより、移動体1の移動前後の移動量を、実測された物理量に基づいて算出できる。
その他、第2変化量算出部54は、移動体制御部3(後述)へ入力された運転手によるアクセル開度、ブレーキ強度、及び/又はハンドルの操舵角などの移動体1(物体検知システム100)の移動に関する制御量、及び/又は、移動体制御部3が物体Bと移動体1との位置関係に基づいて決定した移動体1の移動に関する制御量に基づいて、移動体1の移動量を算出してもよい。これにより、比較的容易に移動体1の移動量を算出できる。第2変化量算出部54における第2変化量M2の算出方法については、後ほど詳しく説明する。
制御部5は、判定部55を有する。判定部55は、移動体1の移動前後における第1座標値と第2座標値との間の変化(すなわち、移動前後における物体の測定装置4aから見た位置の変化)に基づいて算出される第1変化量と、移動前後における移動体1の位置及び姿勢の変化に基づいて算出される第2変化量とを比較することにより、測定装置4aの異常を判定する。
具体的には、判定部55は、測定装置4aや移動量測定装置6の測定誤差などを考慮して、第1変化量M1と第2変化量M2との差分として算出される値が所定の閾値δ以上であれば、測定装置4aに異常が発生したと判定する。一方、第1変化量M1と第2変化量M2との差分として算出される値が閾値δより小さければ、測定装置4aは正常に動作していると判定する。
具体的には、判定部55は、測定装置4aや移動量測定装置6の測定誤差などを考慮して、第1変化量M1と第2変化量M2との差分として算出される値が所定の閾値δ以上であれば、測定装置4aに異常が発生したと判定する。一方、第1変化量M1と第2変化量M2との差分として算出される値が閾値δより小さければ、測定装置4aは正常に動作していると判定する。
測定装置4aに異常が発生したと判定したら、判定部55は、物体検知システム100のディスプレイ(図示せず)に、測定装置4aに異常が生じた旨(異常警報)を表示してもよい。または、判定部55は、当該異常警報を、物体検知システム100に備わるスピーカーを用いて音声などにより通知してもよい。これにより、移動体1(物体検知システム100)の運転者などに対して、測定装置4aの異常を通知できる。
判定部55における、第1変化量M1と第2変化量M2との差分に基づく測定装置4aの異常判定方法については、後ほど詳しく説明する。
判定部55における、第1変化量M1と第2変化量M2との差分に基づく測定装置4aの異常判定方法については、後ほど詳しく説明する。
制御部5は、実空間データ出力部56を有する。実空間データ出力部56は、実空間データVDを出力する。移動体制御部3(図1)は、実空間データVDを受け取り、移動体1(物体検知システム100)の運転者による制御量(例えば、アクセル開度、ブレーキ強度、及び/又はハンドル操舵角など)を入力し、第2変化量算出部54に出力する。また、移動体制御部3は、移動体1と物体Bとの位置関係に基づいて、移動体1(物体検知システム100)の制御量(アクセル開度、ブレーキ強度、及び/又はハンドル操舵角など)を決定する。
例えば、移動体制御部3は、移動体1の近傍に物体Bが存在していると判断した場合、ブレーキ強度を強める指令、アクセル開度(あるいはエンジン及び/又は電動モータの出力制御機構)を0とする指令、及び/又はハンドル操舵角を物体Bから離れる方向に変化させる指令、などを、動力機能、ブレーキ、及び/又はハンドルなどの走行機能(図示せず)などに出力する。これにより、移動体1(物体検知システム100)が物体Bと衝突することを回避できる。
また、移動体制御部3は、必要に応じて、エンジン及び/又は電動モータ(動力機能)と車輪2a、2bとの間のクラッチを切断する指令を出力してもよい。
例えば、移動体制御部3は、移動体1の近傍に物体Bが存在していると判断した場合、ブレーキ強度を強める指令、アクセル開度(あるいはエンジン及び/又は電動モータの出力制御機構)を0とする指令、及び/又はハンドル操舵角を物体Bから離れる方向に変化させる指令、などを、動力機能、ブレーキ、及び/又はハンドルなどの走行機能(図示せず)などに出力する。これにより、移動体1(物体検知システム100)が物体Bと衝突することを回避できる。
また、移動体制御部3は、必要に応じて、エンジン及び/又は電動モータ(動力機能)と車輪2a、2bとの間のクラッチを切断する指令を出力してもよい。
上記の構成を有することにより、制御部5は、測定装置4aの取付状態や故障といった異常が発生したか否かを判定して、移動体制御部3は、移動体1と物体Bとの位置関係に基づいて移動体1(物体検知システム100)の移動を制御しつつ、測定装置4aに異常がある場合には、運転者などに通知できる。
(4)物体検知システムの動作
(4-1)全体動作
以下、物体検知システム100の動作について説明する。まず、物体検知システム100の全体的な動作について、図4を用いて説明する。図4は、物体検知システムの動作を示すフローチャートである。
物体検知システム100が動作を開始すると、まず、位置データ生成部52が、上記にて説明したようにして、位置データD1、D2を生成し、記憶部51に記憶する(ステップS1)。この時点で、記憶部51は、移動体1の移動前に生成された第1位置データD1(前回生成された位置データ)と、移動体1の移動後に生成された第2位置データD2(今回生成された位置データ)とを記憶している。
また、第2変化量算出部54は、第1位置データD1の取得後から第2位置データD2を取得するまでの間の移動体1の制御量を、移動量測定装置6及び/又は移動体制御部3から取得し、移動量データD3として記憶する。
(4-1)全体動作
以下、物体検知システム100の動作について説明する。まず、物体検知システム100の全体的な動作について、図4を用いて説明する。図4は、物体検知システムの動作を示すフローチャートである。
物体検知システム100が動作を開始すると、まず、位置データ生成部52が、上記にて説明したようにして、位置データD1、D2を生成し、記憶部51に記憶する(ステップS1)。この時点で、記憶部51は、移動体1の移動前に生成された第1位置データD1(前回生成された位置データ)と、移動体1の移動後に生成された第2位置データD2(今回生成された位置データ)とを記憶している。
また、第2変化量算出部54は、第1位置データD1の取得後から第2位置データD2を取得するまでの間の移動体1の制御量を、移動量測定装置6及び/又は移動体制御部3から取得し、移動量データD3として記憶する。
位置データD1、D2を取得後、制御部5は、位置データD1、D2と、移動量データD3と、を用いて、測定装置4aに発生している異常を判定する(ステップS2)。制御部5は、位置データD1、D2に基づいて算出される測定装置4aの位置及び姿勢(取付角度)の変化を表す第1変化量M1と、移動量データD3に基づいて算出される測定装置4aの位置及び姿勢の変化を表す第2変化量M2と、の比較に基づいて、測定装置4aの異常を判定する。ステップS2における、測定装置4aの異常判定処理については、後ほど詳しく説明する。
上記のステップS2を実行した結果、測定装置4aに異常が発生していると判定された場合(ステップS3において「No」の場合)、移動体制御部3は、物体Bと移動体1との位置関係を正確に把握できないと判断する。その結果、物体Bの検知を行うことなく、プロセスはステップS7に進む。
また、測定装置4aに異常が発生していると判定された場合には、判定部55は、その旨を物体検知システム100の運転者に通知する。測定装置4aの異常を認識した移動体1の運転者は、例えば、移動体1の移動を停止し、必要に応じて、測定装置4aの取付位置を調整するなどして、測定装置4aに発生している異常を解消できる。
また、測定装置4aに異常が発生していると判定された場合には、判定部55は、その旨を物体検知システム100の運転者に通知する。測定装置4aの異常を認識した移動体1の運転者は、例えば、移動体1の移動を停止し、必要に応じて、測定装置4aの取付位置を調整するなどして、測定装置4aに発生している異常を解消できる。
また、測定装置4aの異常としては、例えば、測定装置4aの測定光Lmや反射光Lrを透過する窓が汚れていたり、水滴が付着していたり、当該窓に傷が存在したりする場合がある。なぜなら、測定光Lmや反射光Lrを透過する窓が汚れていたり水滴が付着していたり、当該窓に傷があったりすると、測定光Lmや反射光Lrの光路が、正常な光路から変わるからである。
このような場合には、例えば、測定装置4aの窓から汚物や水滴を取り除いたり、異常を生じた測定装置4aを取り替えたりすることにより、測定装置4aを正常に動作させることができる。
このような場合には、例えば、測定装置4aの窓から汚物や水滴を取り除いたり、異常を生じた測定装置4aを取り替えたりすることにより、測定装置4aを正常に動作させることができる。
一方、測定装置4aが正常であると判定された場合(ステップS3において「No」の場合)、移動体制御部3は、物体Bと移動体1との位置関係を正確に把握できると判断する。この場合、移動体制御部3は、移動体1の移動後に生成した第2位置データD2を用いて、移動体1の近傍の物体Bの検知を実行する(ステップS4)。具体的には、移動体制御部3は、第2位置データD2に含まれる座標値の中に、Z座標値が所定の値以下となっているものが存在しているか否かを確認する。
Z座標値が所定の値以下となっている座標値が第2位置データD2中に存在し、移動体1の近傍に物体Bが存在すると判断した場合(ステップS5において「Yes」の場合)、移動体制御部3は、移動体1の近傍の物体Bとの衝突を回避するよう、例えば、ブレーキをかける指令、アクセル開度を0とする指令、及び/又はハンドル操舵角を変更する指令を、動力機能、ブレーキ、及び/又はハンドルなどの走行機能などに出力する(ステップS6)。これにより、移動体1は、近傍に存在する物体Bとの衝突を回避できる。
Z座標値が所定の値以下となっている座標値が第2位置データD2中に存在せず、移動体1の近傍に物体Bが存在しないと判断した場合(ステップS5において「No」の場合)、移動体制御部3が物体Bとの衝突を回避するような制御を移動体1に対して実行することなく、プロセスはステップS7に進む。
必要に応じて物体Bの検知と上記の回避制御とを実行後、制御部5は、物体検知システム100の動作を停止するか否かを判断する(ステップS7)。例えば、移動体1の運転者などにより、物体検知システム100の停止が指令された場合(ステップS7において「Yes」の場合)、制御部5は、物体検知システム100の動作を停止する。
一方、物体検知システム100の停止が指令されていない場合(ステップS7において「No」の場合)、上記のステップS1~S6が継続的に実行される。
一方、物体検知システム100の停止が指令されていない場合(ステップS7において「No」の場合)、上記のステップS1~S6が継続的に実行される。
上記のステップS1~S7を実行することにより、物体検知システム100は、測定装置4aの異常がない場合には、移動体1の近傍に物体Bが存在しているか否かを判定して、近傍に物体Bが存在していれば、当該物体Bとの衝突を回避するよう移動体1を制御できる。
一方、測定装置4aに異常が発生している場合には、物体検知システム100は、移動体1の運転者などに異常警報を通知できる。その結果、運転者は、測定装置4aの異常を認識し、必要に応じて、測定装置4aの調整などを行うことにより、発生している異常を解消できる。
一方、測定装置4aに異常が発生している場合には、物体検知システム100は、移動体1の運転者などに異常警報を通知できる。その結果、運転者は、測定装置4aの異常を認識し、必要に応じて、測定装置4aの調整などを行うことにより、発生している異常を解消できる。
(4-2)測定装置の異常判定処理
次に、上記のステップS2において実行される、測定装置4aの異常判定処理について、図5を用いて説明する。図5は、測定装置の異常判定処理を示すフローチャートである。
以下の説明においては、図6に示すような、水平方向に長い建造物である物体B1と、当該物体B1の手前に配置された台座である物体B2とが存在する実空間を、移動体1が時刻t1から時刻t2の間に移動する場合を例にとって説明する。図6は、実空間の一例を示す図である。
図6に示す例において、時刻t1における移動体1の基準位置Oは、時刻t2において基準位置O’に移動し、移動体1は移動方向(すなわち、姿勢)を所定の角度だけ変化させている。また、以下においては、測定装置4aの異常を判定する場合を例にとって説明する。
次に、上記のステップS2において実行される、測定装置4aの異常判定処理について、図5を用いて説明する。図5は、測定装置の異常判定処理を示すフローチャートである。
以下の説明においては、図6に示すような、水平方向に長い建造物である物体B1と、当該物体B1の手前に配置された台座である物体B2とが存在する実空間を、移動体1が時刻t1から時刻t2の間に移動する場合を例にとって説明する。図6は、実空間の一例を示す図である。
図6に示す例において、時刻t1における移動体1の基準位置Oは、時刻t2において基準位置O’に移動し、移動体1は移動方向(すなわち、姿勢)を所定の角度だけ変化させている。また、以下においては、測定装置4aの異常を判定する場合を例にとって説明する。
異常判定処理が開始されると、まず、第1変化量算出部53が、時刻t1において生成された第1位置データD1から、時刻t1における物体の座標値(第1座標値P1~P4と呼ぶことにする)を取得する(ステップS21)。
具体的には、例えば、時刻t1において図7に示すような第1位置データD1が取得されている場合、第1変化量算出部53は、まず、図7に示す4つの特徴的な点L1~L4を選択する。図7は、移動前の位置データの一例を示す図である。特徴点の候補は位置情報を検出面DS上に設定された二次元座標に展開して、Harris法、KLT法、主曲率法などの点検出手法を用いて抽出できる。ここで、4つの特徴点L1~L4を選択する理由は、後述するように、第1変化量算出部53が、第1変化量M1(第1パラメタSM)を4行4列の行列として算出するためである。
図7に示す例では、建造物である物体B1の上部に取り付けられている2つの構造物が特徴点L1、L2として、台座である物体B2上の2つの構造物が特徴点L3、L4として選択されている。
第1変化量算出部53は、選択した各特徴点の座標値を、第1座標値P1~P4として取得する。例えば、第1変化量算出部53が、第1座標値P1を(X1,Y1,Z1)と、第1座標値P2を(X2,Y2,Z2)と、第1座標値P3を(X3,Y3,Z3)と、第1座標値P4を(X4,Y4,Z4)と、決定したとする。
次に、第1変化量算出部53は、時刻t2において生成された第2位置データD2から、第1位置データD1において選択された特徴点に対応する特徴点を画像認識などにより探索し、当該探索して見つかった対応する特徴点の座標値を、移動体1の移動後の第2座標値P1’~P4’として、上記のステップS21と同様にして取得する(ステップS22)。
例えば、第2座標値P1’を(X1’,Y1’,Z1’)と、第2座標値P2’を(X2’,Y2’,Z2’)と、第2座標値P3’を(X3’,Y3’,Z3’)と、第2座標値P4’を(X4’,Y4’,Z4’)と決定したとする。
例えば、第2座標値P1’を(X1’,Y1’,Z1’)と、第2座標値P2’を(X2’,Y2’,Z2’)と、第2座標値P3’を(X3’,Y3’,Z3’)と、第2座標値P4’を(X4’,Y4’,Z4’)と決定したとする。
第1座標値P1~P4及び第2座標値P1’~P4’を取得後、第1変化量算出部53は、取得した第1座標値P1~P4及び第2座標値P1’~P4’を用いて、移動体1の移動前後における測定装置4aの位置及び姿勢の変化を表す第1変化量M1を算出する(ステップS23)。
第1位置データD1及び第2位置データD2は、それぞれ、測定装置4aに設定された座標系(すなわち、測定装置4aから見た座標系)における物体B1、B2の座標値として表されている。従って、移動体の移動に伴い、時刻t1から時刻t2の間に、測定装置4aの位置が、図6に示すように、取付位置Ps1から取付位置Ps1’へと変化する。また、移動体1の移動に伴って、図6に示すように、測定装置4aの検出面DSの法線方向を表すZ軸がZ’軸へと変化している。
このように、測定装置4aの位置と姿勢が変化することにより、第1位置データD1にて示される物体B1、B2の像(図7及び図8の点線)は、第2位置データD2においては図8の実線で示すように変化する。図8は、移動後の位置データの一例を示す図である。
第1位置データD1及び第2位置データD2は、それぞれ、測定装置4aに設定された座標系(すなわち、測定装置4aから見た座標系)における物体B1、B2の座標値として表されている。従って、移動体の移動に伴い、時刻t1から時刻t2の間に、測定装置4aの位置が、図6に示すように、取付位置Ps1から取付位置Ps1’へと変化する。また、移動体1の移動に伴って、図6に示すように、測定装置4aの検出面DSの法線方向を表すZ軸がZ’軸へと変化している。
このように、測定装置4aの位置と姿勢が変化することにより、第1位置データD1にて示される物体B1、B2の像(図7及び図8の点線)は、第2位置データD2においては図8の実線で示すように変化する。図8は、移動後の位置データの一例を示す図である。
ただし、図6~図8に示すように、測定装置4a(移動体1)の移動の方向は、第1位置データD1及び第2位置データD2に示される像の移動の方向とは逆方向となる。例えば、図6に示すように、移動体1は紙面の右方向へ移動する一方、第2位置データD2に示される物体B1、B2の像は、第1位置データD1に示される物体B1、B2の像よりも紙面の左方向に移動している。
従って、本実施形態の第1変化量算出部53は、第2座標値P1’~P4’の座標値を、第1座標値P1~P4へと移動させる(すなわち、第2位置データD2に示される物体B1、B2の像を、第1位置データD1に示される物体B1、B2の像へと移動及び変形させる座標変換である)第1パラメタSMを算出する。
従って、本実施形態の第1変化量算出部53は、第2座標値P1’~P4’の座標値を、第1座標値P1~P4へと移動させる(すなわち、第2位置データD2に示される物体B1、B2の像を、第1位置データD1に示される物体B1、B2の像へと移動及び変形させる座標変換である)第1パラメタSMを算出する。
本実施形態において、第1変化量算出部53は、3次元空間における同次座標表現を用いて、第1パラメタSMを算出する。これにより、3次元空間の回転運動と並進運動とを1つの行列にて表現できる。
具体的には、まず、上記のステップS22にて取得した第1座標値P1~P4から、それぞれ、(X1,Y1,Z1,1)、(X2,Y2,Z2,1)、(X3,Y3,Z3,1)、(X4,Y4,Z4,1)との4次元座標を生成する。また、第2座標値P1’~P4’から、それぞれ、(X1’,Y1’,Z1’,1)、(X2’,Y2’,Z2’,1)、(X3’,Y3’,Z3’,1)、(X4’,Y4’,Z4’,1)との4次元座標を生成する。
具体的には、まず、上記のステップS22にて取得した第1座標値P1~P4から、それぞれ、(X1,Y1,Z1,1)、(X2,Y2,Z2,1)、(X3,Y3,Z3,1)、(X4,Y4,Z4,1)との4次元座標を生成する。また、第2座標値P1’~P4’から、それぞれ、(X1’,Y1’,Z1’,1)、(X2’,Y2’,Z2’,1)、(X3’,Y3’,Z3’,1)、(X4’,Y4’,Z4’,1)との4次元座標を生成する。
第1パラメタSMは、第2座標値P1’~P4’を、第1座標値P1~P4へと移動させる行列と定義されているので、第1パラメタSMを用いた座標値の移動を示す4つの式、(X1,Y1,Z1,1)T=SM(X1’,Y1’,Z1’,1)T、(X2,Y2,Z2,1)T=SM(X2’,Y2’,Z2’,1)T、(X3,Y3,Z3,1)T=SM(X3’,Y3’,Z3’,1)T、及び、(X4,Y4,Z4,1)T=SM(X4’,Y4’,Z4’,1)Tを定義できる。なお、上記の式の上付き文字「T」は、行列の転置を表す。上記の4つの式を1つの式にまとめると、以下の数1のようになる。
次に、上記の数1において、右辺の第1パラメタSMではない行列の逆行列を両辺に乗算することにより、以下の数2のように第1パラメタSM(4行4列の行列)を、第1変化量M1として算出できる。
数2の右辺には上記のステップS21及びS22において算出された既知の座標値しか含まれていないので、第1変化量算出部53は、数2を用いて、数値として表される具体的な第1パラメタSM(第1変化量M1)を算出できる。なお、数2の右辺の逆行列は、例えば、掃き出し法などの逆行列を算出する方法により算出できる。第1変化量算出部53は、具体的な数値として算出した第1パラメタSMを、第1変化量M1として記憶部51に記憶する。
数2の右辺には上記のステップS21及びS22において算出された既知の座標値しか含まれていないので、第1変化量算出部53は、数2を用いて、数値として表される具体的な第1パラメタSM(第1変化量M1)を算出できる。なお、数2の右辺の逆行列は、例えば、掃き出し法などの逆行列を算出する方法により算出できる。第1変化量算出部53は、具体的な数値として算出した第1パラメタSMを、第1変化量M1として記憶部51に記憶する。
上記のようにして数2を用いて第1パラメタSMを算出し、第1変化量M1として記憶部51に記憶後、第2変化量算出部54が、第2変化量M2を算出する。第2変化量算出部54は、移動体1の移動前後(すなわち、時刻t1から時刻t2の間)に測定装置4aの取付位置や取付角度は変化していないと仮定して、時刻t1から時刻t2の間の移動体1の移動による位置及び姿勢の変化と、測定装置4aの取付位置Ps1、Ps1’及び取付角度の移動体1の基準位置O、O’に対する相対的な関係と、に基づいて、第2変化量M2を算出する。
上記の「測定装置4aの取付位置Ps1、Ps1’及び取付角度の移動体1の基準位置O、O’に対する相対的な関係」は、本実施形態においては、基準位置O、O’を取付位置Ps1、Ps1’へと平行移動したときの移動量として定義されている。なお、正常時の測定装置4aの検出面DSの法線方向は移動体1の直進方向と平行と定めているので、測定装置4aについては、取付角度の移動体1の基準位置O、O’に対する相対的な関係はないものとする。
従って、例えば、移動体1の基準位置Oの座標値(Ox,Oy,Oz,1)を、測定装置4aの取付位置Ps1の座標値(Ps1x,Ps1y,Ps1z,1)へ平行移動させる行列を行列Uとすると、(Ps1x,Ps1y,Ps1z,1)T=U(Ox,Oy,Oz,1)Tとの式が成立する。基準位置O、O’を取付位置Ps1、Ps1’に移動させる行列Uを、第3パラメタUと呼ぶことにする。
従って、例えば、移動体1の基準位置Oの座標値(Ox,Oy,Oz,1)を、測定装置4aの取付位置Ps1の座標値(Ps1x,Ps1y,Ps1z,1)へ平行移動させる行列を行列Uとすると、(Ps1x,Ps1y,Ps1z,1)T=U(Ox,Oy,Oz,1)Tとの式が成立する。基準位置O、O’を取付位置Ps1、Ps1’に移動させる行列Uを、第3パラメタUと呼ぶことにする。
第3パラメタUは、移動体1の基準位置の座標値を(0,0,0)としたときに測定装置4aの取付位置の座標値が(a,b,c)となる場合には、以下の数3のように定義される。例えば、図9に示すように、基準位置が原点(0,0,0)であるときに、測定装置4aの取付位置の座標値が(0,0,c)となる場合には、数3にて示される第3パラメタUのa、bは共に0となる。図9は、測定装置の取付位置の一例を示す図である。
また、第3パラメタUは、移動体1が移動するより前のある時点における、移動体1に対する測定装置4aの位置及び姿勢を表すものとして定義されている。さらに、本実施形態においては、異常がない限り移動体1の移動前後に測定装置4aの取付位置や取付角度は変化していないと仮定している。よって、第3パラメタUは、予め設定された一定値とされている。すなわち、第3パラメタUの要素の値a,b,cは上記のある時点で設定された後は変化しない。よって、第3パラメタUは、予め設定されて記憶部51に記憶されている。
上記のように第3パラメタUを定義後、第2変化量算出部54は、時刻t1と時刻t2との間に移動量データD3として取得した移動量測定装置6からの移動に関する実測値、及び/又は、移動体制御部3から出力される移動体1の制御量とから、移動前後の移動体1の位置及び姿勢の変化を算出する(ステップS24)。
例えば、移動量測定装置6から速度の実測値を入力する場合には、時刻t1と時刻t2との間に取得した速度としての移動量データD3を累積、又は、速度を時間で積分することにより、時刻t1から時刻t2の間に移動した移動体1の移動距離を算出できる。一方、移動量測定装置6から加速度の実測値を移動量データD3として入力した場合には、当該加速度から速度を算出した上で、当該速度を累積又は時間で積分することにより、移動距離を算出できる。
また、移動体制御部3から出力されている移動体1のアクセル開度などにより算出された速度や加速度から、上記のようにして移動距離を算出できる。
例えば、移動量測定装置6から速度の実測値を入力する場合には、時刻t1と時刻t2との間に取得した速度としての移動量データD3を累積、又は、速度を時間で積分することにより、時刻t1から時刻t2の間に移動した移動体1の移動距離を算出できる。一方、移動量測定装置6から加速度の実測値を移動量データD3として入力した場合には、当該加速度から速度を算出した上で、当該速度を累積又は時間で積分することにより、移動距離を算出できる。
また、移動体制御部3から出力されている移動体1のアクセル開度などにより算出された速度や加速度から、上記のようにして移動距離を算出できる。
また、移動量測定装置6や移動体制御部3から移動体1の角度(方向)変化やハンドルの操舵角を移動量データD3として入力した場合には、必要に応じて上記の移動距離を考慮して、時刻t1から時刻t2までの間の移動体1の角度変化を算出できる。
さらに、移動量測定装置6から、時刻t1の移動空間での位置と、時刻t2の移動空間での位置と、を移動量データD3として取得した場合には、これら2つの位置の差分から、移動体1の位置及び姿勢の変化を算出できる。
さらに、移動量測定装置6から、時刻t1の移動空間での位置と、時刻t2の移動空間での位置と、を移動量データD3として取得した場合には、これら2つの位置の差分から、移動体1の位置及び姿勢の変化を算出できる。
移動体1の基準位置の移動量を、時刻t1の基準位置Oを時刻t2の基準位置O’に移動させる行列T(第2パラメタTと呼ぶ)と定義し、算出された移動体1の位置及び姿勢(角度)の変化を第2パラメタTの各要素に反映させると、第2パラメタTは、例えば、以下の数4のように算出される。
上記の第2パラメタTにおいて、4列目の各要素のΔX、ΔY、ΔZは、それぞれ、移動体1の基準位置Oの座標値が、X軸方向にΔX、Y軸方向にΔY、Z軸方向にΔZだけ移動したことに対応する。すなわち、移動前の基準位置Oの座標値を(Ox,Oy,Oz,1)とすると、移動後の基準位置O’の座標値が(Ox+ΔX,Oy+ΔY,Oz+ΔZ,1)と変化したことを意味している。すなわち、4列目の各要素のΔX、ΔY、ΔZが、移動体1の位置の変化に対応する。
一方、第2パラメタTの他の要素は、基準位置O、O’を原点とした座標系の軸回りの回転を表しており、移動体1の姿勢(移動方向)の変化に対応する。上記に示す第2パラメタTは、X軸回りにα、Y軸回りにβ、Z軸回りにγだけ基準位置Oを原点とする座標系を回転させる(すなわち、移動体1を回転させる)行列である。第2パラメタTの回転角度α、β、γについては、基準位置を原点とする座標系におけるX軸、Y軸、Z軸の定義に従って、どちら回り(軸回りに右回転又は左回転)を正の角度とするか適宜決定できる。第2変化量算出部54は、上記のようにして算出された第2パラメタTを、記憶部51に記憶する。
一方、第2パラメタTの他の要素は、基準位置O、O’を原点とした座標系の軸回りの回転を表しており、移動体1の姿勢(移動方向)の変化に対応する。上記に示す第2パラメタTは、X軸回りにα、Y軸回りにβ、Z軸回りにγだけ基準位置Oを原点とする座標系を回転させる(すなわち、移動体1を回転させる)行列である。第2パラメタTの回転角度α、β、γについては、基準位置を原点とする座標系におけるX軸、Y軸、Z軸の定義に従って、どちら回り(軸回りに右回転又は左回転)を正の角度とするか適宜決定できる。第2変化量算出部54は、上記のようにして算出された第2パラメタTを、記憶部51に記憶する。
第2パラメタT及び第3パラメタUが、それぞれ、数3及び数4のように決定された後、第2変化量算出部54は、第2パラメタT及び第3パラメタUを記憶部51から読み出して(ステップS25)、第2変化量M2を算出する(ステップS26)。
第2変化量M2は、例えば、(i)移動前の測定装置4aの取付位置Ps1を、移動前の基準位置Oに移動させる、(ii)移動前の基準位置Oを、移動後の基準位置O’に移動させる、(iii)移動後の基準位置O’を、移動後の測定装置4aの取付位置Ps1’に移動させる、との操作を実行することにより算出できる。具体的には、以下のようにして、第2変化量M2は算出される。
第2変化量M2は、例えば、(i)移動前の測定装置4aの取付位置Ps1を、移動前の基準位置Oに移動させる、(ii)移動前の基準位置Oを、移動後の基準位置O’に移動させる、(iii)移動後の基準位置O’を、移動後の測定装置4aの取付位置Ps1’に移動させる、との操作を実行することにより算出できる。具体的には、以下のようにして、第2変化量M2は算出される。
まず、上記の操作(i)~(iii)を表す式を、第2パラメタT及び第3パラメタUを用いて生成する。
具体的には、操作(i)は、基準位置Oを取付位置Ps1に移動させる(Ps1x,Ps1y,Ps1z,1)T=U(Ox,Oy,Oz,1)Tとは逆の操作となるので、第3パラメタUの逆行列(U-1)を用いて、(Ox,Oy,Oz,1)T=U-1(Ps1x,Ps1y,Ps1z,1)Tと表すことができる。
一方、操作(ii)は、移動後の基準位置O’の座標値を(Ox’,Oy’,Oz’,1)とすると、第2パラメタTを用いて、(Ox’,Oy’,Oz’,1)T=T(Ox,Oy,Oz,1)Tと表すことができる。
さらに、操作(iii)は、移動後の取付位置Ps1’の座標値を(Ps1x’,Ps1y’,Ps1z’,1)とすると、第3パラメタUを用いて、(Ps1x’,Ps1y’,Ps1z’,1)T=U(Ox’,Oy’,Oz’,1)Tと表すことができる。
具体的には、操作(i)は、基準位置Oを取付位置Ps1に移動させる(Ps1x,Ps1y,Ps1z,1)T=U(Ox,Oy,Oz,1)Tとは逆の操作となるので、第3パラメタUの逆行列(U-1)を用いて、(Ox,Oy,Oz,1)T=U-1(Ps1x,Ps1y,Ps1z,1)Tと表すことができる。
一方、操作(ii)は、移動後の基準位置O’の座標値を(Ox’,Oy’,Oz’,1)とすると、第2パラメタTを用いて、(Ox’,Oy’,Oz’,1)T=T(Ox,Oy,Oz,1)Tと表すことができる。
さらに、操作(iii)は、移動後の取付位置Ps1’の座標値を(Ps1x’,Ps1y’,Ps1z’,1)とすると、第3パラメタUを用いて、(Ps1x’,Ps1y’,Ps1z’,1)T=U(Ox’,Oy’,Oz’,1)Tと表すことができる。
次に、上記の操作(i)~(iii)を表す式を用いて、取付位置Ps1を取付位置Ps1’へと移動させる式を算出する。
具体的には、まず、操作(iii)を表す式(Ps1x’,Ps1y’,Ps1z’,1)T=U(Ox’,Oy’,Oz’,1)Tの「(Ox’,Oy’,Oz’,1)T」を、操作(ii)を表す式(Ox’,Oy’,Oz’,1)T=T(Ox,Oy,Oz,1)Tに置き換えることにより、(Ps1x’,Ps1y’,Ps1z’,1)T=UT(Ox,Oy,Oz,1)Tとの式を生成する。
次に、(Ps1x’,Ps1y’,Ps1z’,1)T=UT(Ox,Oy,Oz,1)Tとの式の「(Ox,Oy,Oz,1)T」を、操作(i)を表す式(Ox,Oy,Oz,1)T=U-1(Ps1x,Ps1y,Ps1z,1)Tに置き換えることにより、最終的に、(Ps1x’,Ps1y’,Ps1z’,1)T=UTU-1(Ps1x,Ps1y,Ps1z,1)Tと、取付位置Ps1を取付位置Ps1’へと移動させる操作を表す式が算出される。
具体的には、まず、操作(iii)を表す式(Ps1x’,Ps1y’,Ps1z’,1)T=U(Ox’,Oy’,Oz’,1)Tの「(Ox’,Oy’,Oz’,1)T」を、操作(ii)を表す式(Ox’,Oy’,Oz’,1)T=T(Ox,Oy,Oz,1)Tに置き換えることにより、(Ps1x’,Ps1y’,Ps1z’,1)T=UT(Ox,Oy,Oz,1)Tとの式を生成する。
次に、(Ps1x’,Ps1y’,Ps1z’,1)T=UT(Ox,Oy,Oz,1)Tとの式の「(Ox,Oy,Oz,1)T」を、操作(i)を表す式(Ox,Oy,Oz,1)T=U-1(Ps1x,Ps1y,Ps1z,1)Tに置き換えることにより、最終的に、(Ps1x’,Ps1y’,Ps1z’,1)T=UTU-1(Ps1x,Ps1y,Ps1z,1)Tと、取付位置Ps1を取付位置Ps1’へと移動させる操作を表す式が算出される。
取付位置Ps1を取付位置Ps1’へと移動させる操作を表す式(Ps1x’,Ps1y’,Ps1z’,1)T=UTU-1(Ps1x,Ps1y,Ps1z,1)Tにおいて、「UTU-1」との式で表される行列(4行4列の行列)を、測定装置4aの取付位置Ps1を取付位置Ps1’へと移動させる行列、すなわち、移動体1の移動前後の測定装置4aの移動量を表す第2変化量M2と定義できる。
第2変化量M2である「UTU-1」との行列において、第3パラメタU及びその逆行列U-1は、予め設定された一定値である。また、第2パラメタTは、ステップS24において、移動前後の移動体1の位置及び姿勢の変化から具体的に算出できている。
よって、第2変化量算出部54は、第3パラメタU(U-1)と、ステップS24にてすでに具体的に算出された第2パラメタTとを用いて、「UTU-1」と表される第2変化量M2を具体的に算出できる。第2変化量算出部54は、上記のように算出した第2変化量M2を、記憶部51に記憶する。
よって、第2変化量算出部54は、第3パラメタU(U-1)と、ステップS24にてすでに具体的に算出された第2パラメタTとを用いて、「UTU-1」と表される第2変化量M2を具体的に算出できる。第2変化量算出部54は、上記のように算出した第2変化量M2を、記憶部51に記憶する。
第1変化量M1及び第2変化量M2を算出後、判定部55は、第1変化量M1と第2変化量M2との差分に基づいて、測定装置4aに異常が発生しているか否かを判定する(ステップS27)。
以下、測定装置4aの異常と、第1変化量M1と第2変化量M2の差分との関係について説明する。
測定装置4aの移動体1への取付位置や取付角度が移動前後で変化していない場合や、測定装置4aの内部で検出面DSの取付位置や取付角度が移動前後で変化していない場合、検出面DSと移動体1の基準位置O、O’との位置関係や、検出面DSの法線方向の直進方向に対する角度は移動体1の移動前後で変化しない。
その結果、第2座標値P1’~P4’を第1座標値P1~P4へと移動させる行列として算出される第1変化量M1(第1パラメタSM)は、第2パラメタTと第3パラメタUとを用いて算出される第2変化量M2(行列UTU-1)と一致することとなる。
測定装置4aの移動体1への取付位置や取付角度が移動前後で変化していない場合や、測定装置4aの内部で検出面DSの取付位置や取付角度が移動前後で変化していない場合、検出面DSと移動体1の基準位置O、O’との位置関係や、検出面DSの法線方向の直進方向に対する角度は移動体1の移動前後で変化しない。
その結果、第2座標値P1’~P4’を第1座標値P1~P4へと移動させる行列として算出される第1変化量M1(第1パラメタSM)は、第2パラメタTと第3パラメタUとを用いて算出される第2変化量M2(行列UTU-1)と一致することとなる。
その一方、例えば図10に示すように、移動体1の移動後の測定装置4aの取付位置が取付位置Ps1’’へと変化し、取付角度(検出面DSの法線方向)が直進方向(Z、Z’軸方向)に平行な角度からX軸(X’軸)の正方向(紙面の右方向)に傾いた角度へと変化したとする。図10は、測定装置の取付状態が変化した状態の一例を示す図である。すなわち、測定装置4aの検出面DSの基準位置に対する位置関係や、検出面DSの法線方向の直進方向に対する角度が、移動体1の移動前後で異なったとする。
移動前後で検出面DSの位置関係や法線方向の角度が変化して取得される第2位置データD2’に示される物体B1、B2の像は、図11に示すように、移動前後で取付状態が変化しない場合に取得される第2位置データD2に示される物体B1、B2の像(図11において点線で示す像)とは異なるものとなる。図11は、測定装置の取付状態が変化した後に取得された第2位置データの一例を示す図である。
具体的には、図10に示すように測定装置4aの取付状態が変化した場合に取得される第2位置データD2’に示される物体B1、B2の像は、取付状態が変化しない場合に取得される第2位置データD2に示される物体B1、B2の像よりもさらに紙面の左側にずれたものとなる。
移動前後で検出面DSの位置関係や法線方向の角度が変化して取得される第2位置データD2’に示される物体B1、B2の像は、図11に示すように、移動前後で取付状態が変化しない場合に取得される第2位置データD2に示される物体B1、B2の像(図11において点線で示す像)とは異なるものとなる。図11は、測定装置の取付状態が変化した後に取得された第2位置データの一例を示す図である。
具体的には、図10に示すように測定装置4aの取付状態が変化した場合に取得される第2位置データD2’に示される物体B1、B2の像は、取付状態が変化しない場合に取得される第2位置データD2に示される物体B1、B2の像よりもさらに紙面の左側にずれたものとなる。
その結果、第2位置データD2’から選択された第2座標値P1’’~P4’’を第1座標値P1~P4に移動させる行列として算出される第1パラメタSM’(第1変化量M1’)は、取付状態が変化しない場合に算出される第1パラメタSM(第1変化量M1)とは異なるものとなる。すなわち、移動前後で測定装置4aの取付状態が変化した場合、第1変化量M1’は、第2変化量M2とは一致しなくなる。
また、測定装置4aにおける測定光Lmや反射光Lrを透過させる窓などの光学部材に汚れや傷が存在する場合、これらの汚れや傷は、測定光Lmや反射光Lrの光路を曲げたり、これらの光を散乱(屈折)させたりする。その結果、光学部材の汚れや傷は、測定装置4aの取付位置や取付角度を、光学部材に汚れや傷が付着する前の状態から変化させるのと同等の効果を生じる。
従って、第1変化量M1、M1’と第2変化量M2とが一致するか否かにより、光学部材の汚れなどの測定装置4aの故障や異常も判定できる。
従って、第1変化量M1、M1’と第2変化量M2とが一致するか否かにより、光学部材の汚れなどの測定装置4aの故障や異常も判定できる。
本実施形態において、判定部55は、第1パラメタSM、SM’、第2パラメタT、及び第3パラメタUから生成される、第1変化量M1を表す第1パラメタSM、SM’と、第2変化量M2を表す行列UTU-1と、の差分として算出される値である差分行列SM-UTU-1の行列式(det(SM-UTU-1))を用い、第1変化量M1と第2変化量M2とが一致するか否かを判定(すなわち、測定装置4aに異常があるか否かを判定)する。
行列式det(SM-UTU-1)の値が所定の閾値δよりも小さい場合(ステップS27において「Yes」の場合)、判定部55は、第1変化量M1と第2変化量M2とが一致していると判定し、測定装置4aは正常であると判定する。その後、異常判定処理は終了する。上記の所定の閾値δは、測定装置4aや移動量測定装置6の測定誤差や、測定装置4aの許容される取付位置や取付角度のずれなどを考慮して、その値の大きさを適宜決定できる。
一方、行列式det(SM-UTU-1)の値が所定の閾値δ以上である場合(ステップS26において「No」の場合)、判定部55は、第1変化量M1と第2変化量M2とが一致していないと判定し、測定装置4aに異常が発生していると判定する。
このとき、判定部55は、例えば、制御部5のスピーカーを用いて警告音を鳴らしたり、制御部5などのディスプレイに取付状態が変化した旨を通知するメッセージを表示したり、警告ランプを点灯させたりして、測定装置4aの取付状態が変化したことを、運転者などに通知する(ステップS28)。その後、取付状態の判定プロセスは終了する。
このとき、判定部55は、例えば、制御部5のスピーカーを用いて警告音を鳴らしたり、制御部5などのディスプレイに取付状態が変化した旨を通知するメッセージを表示したり、警告ランプを点灯させたりして、測定装置4aの取付状態が変化したことを、運転者などに通知する(ステップS28)。その後、取付状態の判定プロセスは終了する。
ステップS28を実行中に、判定部55は、行列式det(SM-UTU-1)を用いて、測定装置4aに生じている異常の度合いを算出してもよい。例えば、行列式det(SM-UTU-1)の値の変化と測定装置4aの取付位置や取付角度の値の変化との関係とを予め取得して記憶部51に記憶しておき、行列式det(SM-UTU-1)が所定の閾値δ以上である場合に、判定部55は、当該行列式det(SM-UTU-1)の値の変化と測定装置4aの取付位置や取付角度の値の変化との関係とを用いて、測定装置4aの取付位置や取付角度のある時点からのずれや、測定装置4aの異常の度合い(例えば、光学部材の汚れや傷の付着の度合い)を算出できる。
上記のステップS21~S27を実行することにより、物体検知システム100は、測定装置4aにて物体が観測されなくなるタイミングといった把握が困難な情報ではなく、第1座標値P1~P4及び第2座標値P1’~P4’の測定装置4aによる測定値の変化から算出される測定装置4aの位置及び姿勢の変化(すなわち、第1パラメタSM、SM’、第1変化量M1、M1’)や、移動前後の移動体1の位置及び姿勢の変化(すなわち、第2パラメタT)と第3パラメタUとに基づいて算出される測定装置4aの位置及び姿勢の変化(すなわち、第2変化量M2)といった、物体検知システム100にて困難なく取得される把握しやすい情報に基づいて、測定装置4aの異常を判定している。これにより、測定装置4aの取付状態を、より精度よく把握できる。
上記においては、測定装置4aの異常を判定する方法について説明したが、他の測定装置4b~4dにおいても同様にして異常を判定できる。ただし、他の測定装置4b~4dの異常を判定するに際しては、第2変化量M2(行列UTU-1)を、予め決められている移動体に対する各測定装置4b~4dの位置及び姿勢を表す行列Uを用いて算出する。
2.他の実施形態
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
図5の測定装置4aの異常が発生しているか否かの判定処理を示すフローチャートにおいて、当該フローチャートに示した各処理は、その順番を適宜変更できる。例えば、最初にステップS23~S25にて示した処理を実行して第2変化量M2を算出後に、ステップS21~S22にて示した処理を実行して第1変化量M1、M1’(第1パラメタSM)を算出してもよい。
以上、本発明の一実施形態について説明したが、本発明は上記実施形態に限定されるものではなく、発明の要旨を逸脱しない範囲で種々の変更が可能である。特に、本明細書に書かれた複数の実施形態及び変形例は必要に応じて任意に組み合せ可能である。
図5の測定装置4aの異常が発生しているか否かの判定処理を示すフローチャートにおいて、当該フローチャートに示した各処理は、その順番を適宜変更できる。例えば、最初にステップS23~S25にて示した処理を実行して第2変化量M2を算出後に、ステップS21~S22にて示した処理を実行して第1変化量M1、M1’(第1パラメタSM)を算出してもよい。
(A)位置データについての他の実施形態
上記の第1実施形態において、図7では4つ異なる物体に対してそれぞれ一つずつ特徴点を選択しているが、異なる物体である必要はなく、一つの物体について特徴的な点を4つ選択することも可能である。
上記の第1実施形態において、図7では4つ異なる物体に対してそれぞれ一つずつ特徴点を選択しているが、異なる物体である必要はなく、一つの物体について特徴的な点を4つ選択することも可能である。
(B)異常判定を実行する際の条件についての他の実施形態
上記の第1実施形態において、制御部5は、移動体1の移動中に常に第2パラメタT及び第2変化量M2を求めたうえで測定装置4a~4dの異常判定を実行することとしていた。しかし、これに限られず、例えば、速度一定で、方向の変化がないような場合は、前回の判定で用いた第2パラメタTおよび第2変化量M2を用いることとして、第2パラメタTおよび第2変化量M2の算出を省略してもよい。
上記の第1実施形態において、制御部5は、移動体1の移動中に常に第2パラメタT及び第2変化量M2を求めたうえで測定装置4a~4dの異常判定を実行することとしていた。しかし、これに限られず、例えば、速度一定で、方向の変化がないような場合は、前回の判定で用いた第2パラメタTおよび第2変化量M2を用いることとして、第2パラメタTおよび第2変化量M2の算出を省略してもよい。
(C)判断異常判定を実行する際の条件についての他の実施形態
上記の第1実施形態においては、第1変化量M1と第2変化量M2を用いて異常の判定を行っているが、他の方法により判定を行ってもよい。例えば、第1実施形態の制御部5における第1変化量算出部53及び第2変化量算出部54(図3A)の代わりに、制御部5は、図3Bに示すように、座標予測部57を有していてもよい。図3Bは、他の実施形態における制御部の構成を示す図である。
座標予測部57は、移動前において測定された物体の位置を表す第1座標値と、移動体に対する測定装置の位置情報と、移動前後の基準位置の位置及び姿勢の変化量を用いることにより移動後の物体の位置を予測して(予測座標値)、実際の移動後の物体の位置を表す第2座標値と比較する。この場合、差分としては2つの座標値の距離として計算すればよい。この方法によれば、座標値で比較するため、比較結果を評価しやすく、選択する特徴点の数は4つである必要はない。
上記の第1実施形態においては、第1変化量M1と第2変化量M2を用いて異常の判定を行っているが、他の方法により判定を行ってもよい。例えば、第1実施形態の制御部5における第1変化量算出部53及び第2変化量算出部54(図3A)の代わりに、制御部5は、図3Bに示すように、座標予測部57を有していてもよい。図3Bは、他の実施形態における制御部の構成を示す図である。
座標予測部57は、移動前において測定された物体の位置を表す第1座標値と、移動体に対する測定装置の位置情報と、移動前後の基準位置の位置及び姿勢の変化量を用いることにより移動後の物体の位置を予測して(予測座標値)、実際の移動後の物体の位置を表す第2座標値と比較する。この場合、差分としては2つの座標値の距離として計算すればよい。この方法によれば、座標値で比較するため、比較結果を評価しやすく、選択する特徴点の数は4つである必要はない。
本発明は、車両などの移動体の周囲に存在する物体を検出する物体検知システムにおける、測定装置の異常判定に広く適用できる。
100 物体検知システム
1 移動体
2a、2b、2c、2d 車輪
3 移動体制御部
4a、4b、4c、4d 測定装置
41 出力部
43-1、43-2、43-n検出部
45 レンズ
5 制御部
51 記憶部
52 位置データ生成部
53 第1変化量算出部
54 第2変化量算出部
55 判定部
56 実空間データ出力部
57 座標予測部
6 移動量測定装置
B、B1、B2 物体
D1 第1位置データ
D2、D2' 第2位置データ
D3 移動量データ
L1、L2、L3、L4 特徴点
P1、P2、P3、P4 第1座標値
P1'、P2'、P3'、P4' 第2座標値
P1'’、P2'’、P3'’、P4'’ 第2座標値
Ps1、Ps2、Ps3、Ps4 、Ps1’ 取付位置
M1、M1' 第1変化量
M2 第2変化量
DS 検出面
Lm 測定光
Lr 反射光
O、O'基準位置
SM、SM’ 第1パラメタ
T 第2パラメタ
U 第3パラメタ
SM-UTU-1 差分行列
δ 所定の閾値
1 移動体
2a、2b、2c、2d 車輪
3 移動体制御部
4a、4b、4c、4d 測定装置
41 出力部
43-1、43-2、43-n検出部
45 レンズ
5 制御部
51 記憶部
52 位置データ生成部
53 第1変化量算出部
54 第2変化量算出部
55 判定部
56 実空間データ出力部
57 座標予測部
6 移動量測定装置
B、B1、B2 物体
D1 第1位置データ
D2、D2' 第2位置データ
D3 移動量データ
L1、L2、L3、L4 特徴点
P1、P2、P3、P4 第1座標値
P1'、P2'、P3'、P4' 第2座標値
P1'’、P2'’、P3'’、P4'’ 第2座標値
Ps1、Ps2、Ps3、Ps4 、Ps1’ 取付位置
M1、M1' 第1変化量
M2 第2変化量
DS 検出面
Lm 測定光
Lr 反射光
O、O'基準位置
SM、SM’ 第1パラメタ
T 第2パラメタ
U 第3パラメタ
SM-UTU-1 差分行列
δ 所定の閾値
Claims (13)
- 移動可能な物体である移動体と、
前記移動体に取り付けられ、前記移動体と共に移動する測定装置であり、前記移動体の周囲に存在する物体の前記測定装置から見た位置を表す座標値を測定する測定装置と、
前記移動体の移動前において測定された前記物体の位置を表す第1座標値、前記移動体に対する前記測定装置の位置情報、及び移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の前記移動体から見た前記物体の位置を表す予測座標値を算出する座標予測部と、
前記予測座標値と、前記移動体の移動後において測定された前記物体の位置を表す第2座標値とを比較することにより、前記測定装置の異常を判定する判定部と、
を備える、物体検知システム。 - 前記移動体の移動を制御する移動体制御部をさらに備え、
移動前後における前記移動体の位置及び姿勢の変化は、前記移動体制御部における前記移動体の制御量に基づいて算出される、
請求項1に記載の物体検知システム。 - 前記移動体の移動前後の移動を表す量を測定する移動量測定装置をさらに備える、請求項1又は2に記載の物体検知システム。
- 前記移動体の移動前後の位置の変化は、前記移動体の移動前後における、前記移動体に対して設けられた基準位置の変化として表される、
請求項1~3のいずれかに記載の物体検知システム。 - 取付位置と前記基準位置との位置関係は一定である、請求項4に記載の物体検知システム。
- 移動可能な物体である移動体と、
前記移動体に取り付けられ前記移動体と共に移動する測定装置であり、前記移動体の周囲に存在する物体の前記測定装置から見た位置を表す座標値を測定する測定装置と、
前記移動体の移動前において測定された前記物体の位置を表す第1座標値と前記移動体の移動後において測定された前記物体の位置を表す第2座標値との間の変化に基づいて、前記測定装置の位置及び姿勢の変化を表す第1パラメタを算出する第1パラメタ算出部と、
移動前後における前記移動体の位置及び姿勢の変化を表す第2パラメタを算出する第2パラメタ算出部と、
前記移動体が移動するより前のある時点における、前記移動体に対する前記測定装置の位置及び姿勢を表す第3パラメタを記憶する記憶部と、
前記第1パラメタ、前記第2パラメタ、及び前記第3パラメタに基づいて、前記測定装置の取付位置及び姿勢の前記ある時点に対する変化を判定する判定部と、
を備える、物体検知システム。 - 前記判定部は、前記測定装置の取付位置及び姿勢の前記ある時点に対する変化の有無または変化の度合いを算出する、請求項6に記載の物体検知システム。
- 前記判定部は、前記第1パラメタ、前記第2パラメタ、及び前記第3パラメタから生成される行列の行列式の値に基づいて、前記変化の有無または前記変化の度合いを算出する、
請求項7に記載の物体検知システム。 - 移動前後の前記移動体の位置の変化は、前記移動体の移動前後における、前記移動体に対して固定された基準位置の変化として表される、
請求項6~8のいずれかに記載の物体検知システム。 - 移動可能な物体である移動体に取り付けられた測定装置の異常判定方法であって、
前記移動体の移動前に、前記移動体の周囲に存在する物体の前記測定装置から見た位置を表す第1座標値を前記測定装置に測定させるステップと、
前記移動体の移動後に、前記物体の前記測定装置から見た位置を表す第2座標値を前記測定装置に測定させるステップと、
前記第1座標値、前記移動体に対する前記測定装置の位置情報、及び移動前後の基準位置の位置及び姿勢の変化量に基づき、移動後の前記移動体から見た前記物体の位置を表す予測座標値を算出するステップと、
前記予測座標値と前記第2座標値とを比較することにより、前記測定装置の異常を判定するステップと、
を含む、異常判定方法。 - 移動可能な物体である移動体に取り付けられた測定装置の取付状態の異常判定方法であって、
前記移動体の移動前に、前記移動体の周囲に存在する物体の前記測定装置から見た位置を表す第1座標値を前記測定装置に測定させるステップと、
前記移動体の移動後に、前記物体の前記測定装置から見た位置を表す第2座標値を前記測定装置に測定させるステップと、
前記第1座標値と前記第2座標値との間の変化に基づいて、前記測定装置の位置及び姿勢の変化を表す第1パラメタを算出するステップと、
前記移動体の移動前後における前記移動体の位置及び姿勢の変化を表す第2パラメタを算出するステップと、
前記移動体が移動するより前のある時点において記憶部に記憶され前記移動体に対する前記測定装置の位置及び姿勢を表す第3パラメタを、前記記憶部から読み出すステップと、
前記第1パラメタ、前記第2パラメタ、及び前記第3パラメタに基づいて、前記測定装置の取付位置及び姿勢の前記ある時点に対する変化を判定するステップと、
を含む、異常判定方法。 - 請求項10に記載の前記異常判定方法を、コンピュータに実行させるプログラム。
- 請求項11に記載の前記異常判定方法を、コンピュータに実行させるプログラム。
Applications Claiming Priority (2)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2016-050816 | 2016-03-15 | ||
JP2016050816A JP2017166897A (ja) | 2016-03-15 | 2016-03-15 | 物体検知システム、異常判定方法、及びプログラム |
Publications (1)
Publication Number | Publication Date |
---|---|
WO2017158951A1 true WO2017158951A1 (ja) | 2017-09-21 |
Family
ID=59852089
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
PCT/JP2016/086235 WO2017158951A1 (ja) | 2016-03-15 | 2016-12-06 | 物体検知システム、異常判定方法、及びプログラム |
Country Status (2)
Country | Link |
---|---|
JP (1) | JP2017166897A (ja) |
WO (1) | WO2017158951A1 (ja) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2021140864A1 (ja) * | 2020-01-10 | 2021-07-15 | 株式会社デンソー | 検出器の姿勢・位置検出システムおよび検出器の姿勢・位置検出方法 |
JP2022024741A (ja) * | 2020-07-28 | 2022-02-09 | 株式会社Soken | 車両制御装置、車両制御方法 |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108458718B (zh) * | 2017-12-21 | 2022-05-27 | 杭州后博科技有限公司 | 一种结合地理特征的行进异常判断系统及方法 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH1114748A (ja) * | 1997-06-25 | 1999-01-22 | Honda Motor Co Ltd | 車両の物体検知装置 |
JP2000284389A (ja) * | 1999-03-31 | 2000-10-13 | Fuji Heavy Ind Ltd | ステレオカメラの自己診断装置 |
JP2004317507A (ja) * | 2003-04-04 | 2004-11-11 | Omron Corp | 監視装置の軸調整方法 |
JP2007163258A (ja) * | 2005-12-13 | 2007-06-28 | Alpine Electronics Inc | 車載センサの補正装置および方法 |
JP2007248056A (ja) * | 2006-03-13 | 2007-09-27 | Omron Corp | 車載用レーダ装置 |
JP2010025632A (ja) * | 2008-07-16 | 2010-02-04 | Nippon Soken Inc | 路面監視装置,サスペンション制御装置 |
JP2013195783A (ja) * | 2012-03-21 | 2013-09-30 | Ricoh Co Ltd | 位置ずれ検出装置、車両及び位置ずれ検出方法 |
-
2016
- 2016-03-15 JP JP2016050816A patent/JP2017166897A/ja active Pending
- 2016-12-06 WO PCT/JP2016/086235 patent/WO2017158951A1/ja active Application Filing
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH1114748A (ja) * | 1997-06-25 | 1999-01-22 | Honda Motor Co Ltd | 車両の物体検知装置 |
JP2000284389A (ja) * | 1999-03-31 | 2000-10-13 | Fuji Heavy Ind Ltd | ステレオカメラの自己診断装置 |
JP2004317507A (ja) * | 2003-04-04 | 2004-11-11 | Omron Corp | 監視装置の軸調整方法 |
JP2007163258A (ja) * | 2005-12-13 | 2007-06-28 | Alpine Electronics Inc | 車載センサの補正装置および方法 |
JP2007248056A (ja) * | 2006-03-13 | 2007-09-27 | Omron Corp | 車載用レーダ装置 |
JP2010025632A (ja) * | 2008-07-16 | 2010-02-04 | Nippon Soken Inc | 路面監視装置,サスペンション制御装置 |
JP2013195783A (ja) * | 2012-03-21 | 2013-09-30 | Ricoh Co Ltd | 位置ずれ検出装置、車両及び位置ずれ検出方法 |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2021140864A1 (ja) * | 2020-01-10 | 2021-07-15 | 株式会社デンソー | 検出器の姿勢・位置検出システムおよび検出器の姿勢・位置検出方法 |
JP2022024741A (ja) * | 2020-07-28 | 2022-02-09 | 株式会社Soken | 車両制御装置、車両制御方法 |
JP7466396B2 (ja) | 2020-07-28 | 2024-04-12 | 株式会社Soken | 車両制御装置 |
US12037015B2 (en) | 2020-07-28 | 2024-07-16 | Denso Corporation | Vehicle control device and vehicle control method |
Also Published As
Publication number | Publication date |
---|---|
JP2017166897A (ja) | 2017-09-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7052174B2 (ja) | 将来経路を推定するシステム及び方法 | |
JP6397934B2 (ja) | 走行制御装置 | |
US10508912B2 (en) | Road surface shape measuring device, measuring method, and non-transitory computer-readable medium | |
JP6224370B2 (ja) | 車両用コントローラ、車両システム | |
JP4760272B2 (ja) | 車両周辺監視装置及びセンサユニット | |
US20160327948A1 (en) | Misrecognition determination device | |
JP6720510B2 (ja) | 移動体システム | |
WO2019181284A1 (ja) | 情報処理装置、移動装置、および方法、並びにプログラム | |
JP6666289B2 (ja) | 移動体用空間情報算出装置及び衝突回避システム | |
JP2009180536A (ja) | 画像処理装置、画像処理方法、およびプログラム | |
WO2019073920A1 (ja) | 情報処理装置、移動装置、および方法、並びにプログラム | |
JP6985089B2 (ja) | 立体物接地判定装置 | |
EP3432259B1 (en) | Object detection device, object detection method, and program | |
WO2017158951A1 (ja) | 物体検知システム、異常判定方法、及びプログラム | |
JP5152898B2 (ja) | 障害認識装置及びこれを有する自律走行移動体並びにその制御方法 | |
JP6672915B2 (ja) | 物体検出装置、物体検出方法、及び、プログラム | |
JP5287344B2 (ja) | 駐車支援装置及び障害物検知方法 | |
JP2004120661A (ja) | 移動体周辺監視装置 | |
WO2021246170A1 (ja) | 情報処理装置、情報処理システム、および方法、並びにプログラム | |
JP6465919B2 (ja) | 障害物検知システム | |
JP2014035561A (ja) | 車両走行支援装置及び車両走行支援方法 | |
JP4923766B2 (ja) | 車両用制動力制御装置 | |
JP2021135838A (ja) | 駐車支援装置、及び駐車支援システム | |
WO2019176278A1 (ja) | 情報処理装置、情報処理方法、プログラム、及び移動体 | |
WO2021106510A1 (ja) | 障害物認識装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
NENP | Non-entry into the national phase |
Ref country code: DE |
|
121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 16894568 Country of ref document: EP Kind code of ref document: A1 |
|
122 | Ep: pct application non-entry in european phase |
Ref document number: 16894568 Country of ref document: EP Kind code of ref document: A1 |