WO2024024656A1 - 信号処理装置および信号処理方法、ならびに、情報処理装置 - Google Patents

信号処理装置および信号処理方法、ならびに、情報処理装置 Download PDF

Info

Publication number
WO2024024656A1
WO2024024656A1 PCT/JP2023/026725 JP2023026725W WO2024024656A1 WO 2024024656 A1 WO2024024656 A1 WO 2024024656A1 JP 2023026725 W JP2023026725 W JP 2023026725W WO 2024024656 A1 WO2024024656 A1 WO 2024024656A1
Authority
WO
WIPO (PCT)
Prior art keywords
unit
sensor
point group
velocity
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2023/026725
Other languages
English (en)
French (fr)
Inventor
和俊 北野
祐介 川村
恒介 高橋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sony Semiconductor Solutions Corp
Original Assignee
Sony Semiconductor Solutions Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Sony Semiconductor Solutions Corp filed Critical Sony Semiconductor Solutions Corp
Priority to US18/878,655 priority Critical patent/US20260003075A1/en
Priority to DE112023003273.4T priority patent/DE112023003273T5/de
Priority to JP2024537663A priority patent/JPWO2024024656A1/ja
Priority to CN202380055837.3A priority patent/CN119630988A/zh
Publication of WO2024024656A1 publication Critical patent/WO2024024656A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • G01S17/32—Systems determining position data of a target for measuring distance only using transmission of continuous waves, whether amplitude-, frequency-, or phase-modulated, or unmodulated
    • G01S17/34—Systems determining position data of a target for measuring distance only using transmission of continuous waves, whether amplitude-, frequency-, or phase-modulated, or unmodulated using transmission of continuous, frequency-modulated waves while heterodyning the received signal, or a signal derived therefrom, with a locally-generated signal related to the contemporaneously transmitted signal
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/08—Systems determining position data of a target for measuring distance only
    • G01S17/32—Systems determining position data of a target for measuring distance only using transmission of continuous waves, whether amplitude-, frequency-, or phase-modulated, or unmodulated
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/50—Systems of measurement based on relative movement of target
    • G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/4808—Evaluating distance, position or velocity data
    • 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/491—Details of non-pulse systems
    • G01S7/4912—Receivers
    • G01S7/4915—Time delay measurement, e.g. operational details for pixel components; Phase measurement

Definitions

  • the present disclosure relates to a signal processing device, a signal processing method, and an information processing device.
  • FMCW-LiDAR Frequency Modulated Continuous Wave-Laser Imaging Detection and Ranging
  • FMCW-LiDAR performs coherent detection on a received signal that is a combination of laser light emitted as a chirp light whose pulse frequency is changed linearly over time and reflected light of the emitted laser light.
  • the distance is measured by FMCW-LiDAR uses the Doppler effect to measure speed and distance at the same time.
  • the present disclosure aims to provide a signal processing device, a signal processing method, and an information processing device that make it possible to use measurement results by FMCW-LiDAR more generally.
  • a signal processing device includes a receiving unit that receives speed point group data including a plurality of points each having speed information and time information from a first sensor, and at least one point included in the speed point group data. a correction unit that corrects at least one attribute value regarding a point based on an estimated value at a predetermined time; and a correction time information indicating the predetermined time is added to the attribute value corrected by the correction unit.
  • a transmitting unit that transmits data.
  • FIG. 1 is a block diagram schematically showing a configuration example of a signal processing system according to an embodiment.
  • FIG. 1 is a block diagram showing in more detail the configuration of an example of a signal processing system according to an embodiment.
  • FIG. 1 is a block diagram showing the configuration of an example of a photodetection distance measurement sensor according to an embodiment.
  • FIG. 2 is a schematic diagram schematically showing an example of a scanning pattern of a transmission optical signal according to an embodiment.
  • FIG. 7 is a schematic diagram schematically showing another example of a scanning pattern of a transmitted optical signal.
  • FIG. 7 is a schematic diagram schematically showing another example of a scanning pattern of a transmitted optical signal.
  • FIG. 3 is a schematic diagram schematically showing a speed point group correction process according to the embodiment.
  • FIG. 1 is a block diagram schematically showing a configuration example of a signal processing system according to an embodiment.
  • FIG. 1 is a block diagram showing in more detail the configuration of an example of a signal processing system according to an
  • FIG. 1 is a block diagram showing a hardware configuration of an example of a signal processing system applicable to the embodiment.
  • FIG. 2 is a schematic diagram showing an example of a data format defined in MIPI-CSI-2 that is applicable to the embodiment.
  • FIG. 2 is a schematic diagram showing another example of a data format defined in MIPI-CSI-2 that is applicable to the embodiment.
  • FIG. 2 is a schematic diagram illustrating an example of transmitting a point cloud in a data format defined by MIPI-CSI-2, which is applicable to the embodiment.
  • FIG. 2 is a schematic diagram showing an example in which Ethernet is applied as an interface for data transmission between each unit, which is applicable to the embodiment.
  • FIG. 2 is a diagram illustrating an example of the architecture of a signal processing unit according to an embodiment.
  • FIG. 2 is a flowchart of an example of a transmission optical signal detection process in a photodetection and ranging sensor that can be applied to the embodiment.
  • FIG. FIG. 3 is a schematic diagram for explaining a transmission optical signal transmitted by an optical transmitter/receiver, which is applicable to the embodiment.
  • FIG. 3 is a schematic diagram for explaining determination processing for a received optical signal according to an embodiment.
  • FIG. 3 is a schematic diagram showing an example of data output when the photodetection and ranging sensor performs scanning according to a raster scanning pattern according to the embodiment.
  • FIG. 2 is a schematic diagram for explaining a frame definition applicable to the embodiment.
  • FIG. 3 is a schematic diagram for explaining the output timing of each data by the sensor unit according to the embodiment.
  • FIG. 3 is a schematic diagram for explaining the output timing of each data by the sensor unit according to the embodiment.
  • FIG. 2 is a schematic diagram showing an example of a data format of FMCW-LiDAR data and IMU data output from a sensor unit according to an embodiment.
  • FIG. 2 is a schematic diagram showing an example of output data from the photodetection and ranging sensor according to the embodiment.
  • FIG. 3 is a schematic diagram showing the definition of the coordinates of the injection point, which is applicable to the embodiment.
  • FIG. 2 is a schematic diagram showing each emission point and an object as seen from a photodetection and ranging sensor.
  • FIG. 3 is a schematic diagram showing the overall flow of processing related to point cloud correction in the signal processing unit according to the embodiment.
  • FIG. 7 is a flowchart of an example showing processing related to point cloud correction in the signal processing unit according to the embodiment.
  • 7 is a flowchart of an example showing correction processing of a velocity point group frame of a stationary body in the signal processing unit according to the embodiment.
  • FIG. 3 is a schematic diagram for defining a coordinate system and each variable.
  • FIG. 2 is a block diagram showing the configuration of an example of a signal processing system according to a first modification example of the implementation. It is a block diagram showing the composition of an example of the signal processing system concerning the 2nd modification of implementation.
  • FIG. 1 is a block diagram schematically showing a configuration example of a signal processing system according to an embodiment.
  • the signal processing system 1 includes a sensor section 10, a signal processing section 20, and an information processing section 30.
  • the sensor section 10 includes an IMU (Inertial Measurement Unit) 100, a light detection distance measurement sensor 110, and an image sensor 120.
  • the IMU 100 includes, for example, a 3-axis acceleration sensor, a 3-axis angular velocity sensor, and a 3-axis geomagnetic sensor, and outputs inertial measurement data (hereinafter referred to as IMU data as appropriate) from these sensors.
  • the photodetection ranging sensor 110 is a sensor that measures distance using light, and in the embodiment, it is an FMCW-LiDAR (Frequency Modulated Continuous Wave-Laser Imaging Detection) that measures distance using a laser beam that is continuously modulated in frequency. and Ranging).
  • the image sensor 120 is a so-called camera, and images a subject and outputs image data including, for example, information of R (red), G (green), and B (blue).
  • the signal processing unit 20 is configured to process IMU data output from the IMU 100 of the sensor unit 10, FMCW-LiDAR data output from the photodetection and ranging sensor 110, and image data output from the image sensor 120. input.
  • the signal processing unit 20 performs signal processing based on the input IMU data, FMCW-LiDAR data, and image data.
  • the signal processing unit 20 generates information such as map information, velocity point groups of a stationary object and a moving object, motion meta information indicating the movement of the moving object, and image meta information regarding images in the region of interest by signal processing based on each data. do.
  • the signal processing section 20 outputs each piece of generated information to the information processing section 30.
  • the information processing section 30 performs predetermined processing according to each piece of information output from the signal processing section 20.
  • the processing that the information processing section 30 performs in response to each piece of information output from the signal processing section 20 is not particularly limited.
  • the information processing unit 30 may control the drive of the robot based on map information or the like.
  • the information processing unit 30 may perform notification based on motion meta information or image meta information.
  • the information processing section 30 may send instructions regarding signal processing to the signal processing section 20.
  • the signal processing section 20 may control processing of the output of the sensor section 10 in accordance with this instruction. Further, the signal processing section 20 may control the operation of each sensor (IMU 100, light detection and ranging sensor 110, and image sensor 120) in the sensor section 10 according to this instruction.
  • a general computer configuration including a central processing unit (Central Processing Unit), memory, storage device, etc. may be applied.
  • the signal processing system 1 can output various data based on the measurement results by FMCW-LiDAR, and can make the measurement results by FMCW-LiDAR more versatile. .
  • FIG. 2 is a block diagram showing in more detail the configuration of an example of the signal processing system 1 according to the embodiment.
  • the sensor section 10 includes an IMU 100, a photodetection distance sensor 110 using FMCW-LiDAR, an image sensor 120, and a synchronization signal generation section 130.
  • the synchronization signal generation unit 130 generates a synchronization signal.
  • the synchronization signal may be a pulse with a predetermined period.
  • the synchronization signal generation unit 130 supplies the generated synchronization signal to the IMU 100, the photodetection and ranging sensor 110, and the image sensor 120.
  • the operation and data output timing of the IMU 100, the photodetection and ranging sensor 110, and the image sensor 120 are controlled in synchronization with the supplied synchronization signal, respectively.
  • the IMU 100 includes a 3-axis acceleration sensor, a 3-axis angular velocity sensor, and a 3-axis geomagnetic sensor.
  • the IMU 100 outputs sensor data from these three-axis acceleration sensors, three-axis angular velocity sensors, and three-axis geomagnetic sensors as IMU data.
  • the IMU 100 may output IMU data with a time stamp indicating the time when the IMU data was acquired.
  • the image sensor 120 is a so-called camera, and images a subject and outputs image data including, for example, R (red), G (green), and B (blue) information.
  • the image sensor 120 includes a pixel array in which pixels that output pixel signals as electrical signals corresponding to received light are arranged in a matrix, and a drive circuit that drives each pixel of the pixel array.
  • the image sensor 120 converts each pixel signal output as an analog signal from each pixel of the pixel array into digital pixel data and outputs the converted data.
  • One frame of image data is composed of pixel data based on the output of each pixel included in the effective area of the pixel array.
  • the sensor unit 10 includes an interface unit that controls data input and output of the IMU 100, the photodetection and ranging sensor 110, and the image sensor 120.
  • the present invention is not limited to this, and the IMU 100, the photodetection and ranging sensor 110, and the image sensor 120 may each include separate interface sections.
  • the photodetection distance sensor 110 is a sensor that measures distance using light, and in the embodiment, FMCW-LiDAR is applied.
  • FMCW-LiDAR uses chirped light, in which the pulse frequency is changed linearly over time, as the emitted laser light.
  • distance measurement is performed by coherent detection on a received signal that is a combination of part of the laser light emitted as chirp light, or local light synchronized therewith, and reflected light of the emitted laser light.
  • FMCW-LiDAR With FMCW-LiDAR, by using the Doppler effect, it is possible to measure distance and velocity (Doppler velocity) at the same time. Therefore, by using FMCW-LiDAR, it becomes easy to quickly determine the position of objects with speed, such as people or other moving objects. Furthermore, since coherent detection is less susceptible to interference from other light sources, it is possible to avoid crosstalk, and furthermore, it is less affected by noise from high-intensity light sources such as sunlight. Furthermore, since FMCW-LiDAR is an active type, it is possible to perform measurements even in low-light environments such as darkness.
  • the emission direction of the laser beam is scanned in the horizontal direction over a predetermined angle range, and further scanned in the vertical direction over a predetermined angle range, and the laser beam is emitted at each predetermined angle of the scan. Measurement is performed. Therefore, the measurement results by FMCW-LiDAR are obtained as point information for each scanning angle.
  • the information on each measured point may include, as attribute information (attribute value), the distance based on the distance measurement result, the Doppler velocity, and the intensity of the received reflected light.
  • the attribute information may include the intensity of each orthogonal polarization component of the reflected light.
  • a set of points having three-dimensional or two-dimensional spatial coordinates is called a point group, and a point group in which each point includes velocity information (Doppler velocity) is called a velocity point group.
  • the photodetection and ranging sensor 110 collects information indicating the distance, Doppler velocity, and intensity, a timestamp indicating the time when these information was acquired, and information indicating the emission direction of the laser beam (horizontal scanning angle, vertical scanning angle, etc.). ) and is output as FMCW-LiDAR data. That is, the FMCW-LiDAR data includes a velocity point cloud.
  • FIG. 3 is a block diagram showing the configuration of an example of the photodetection and ranging sensor 110 according to the embodiment.
  • the photodetection and ranging sensor 110 includes an optical scanning section 111, an optical transmitting/receiving section 112, a received signal processing section 113, an optical scanning control section 114, and a transmitting section 115.
  • the optical transmitter/receiver 112 includes an optical transmitter that generates a transmit optical signal to be transmitted from the optical scanner 111 (described later), a transmit light controller that controls transmit light generated by the optical transmitter, and an optical scanner 111. an optical receiver that receives (receives) a received optical signal from the optical receiver.
  • the optical transmitting unit includes, for example, a light source such as a laser diode for emitting laser light as transmission light, an optical system for emitting the light emitted by the light source, and driving the light source. and a laser output modulation device.
  • the optical transmitter causes the light source to emit light in response to the optical transmission control signal supplied from the transmission light controller, and transmits a transmission optical signal using chirp light whose frequency changes linearly within a predetermined frequency range over time. eject.
  • the transmitted optical signal is sent to the optical scanning section 111 and is also sent as local light to the optical receiving section.
  • the transmission light control section In the optical transmission section, the transmission light control section generates a signal whose frequency changes linearly (increases and decreases) within a predetermined frequency range over time. Such a signal whose frequency changes linearly within a predetermined frequency range over time is called a chirp signal.
  • the transmission light control section generates an optical transmission control signal, which is a modulation synchronization timing signal, which is input to the laser output modulation device included in the optical transmission section, based on this chirp signal.
  • the transmission light control section generates an optical transmission control signal as a signal synchronized with the synchronization signal supplied from the synchronization signal generation section 130.
  • the transmission light control section passes the generated optical transmission control signal to the optical transmission section and a received signal processing section 113, which will be described later.
  • the optical receiving unit includes, for example, a light receiving unit that receives (receives) the received optical signal from the optical scanning unit 111, and a drive circuit that drives the light receiving unit.
  • a configuration in which a condensing lens and a light receiving element such as a photodiode are combined can be applied to the light receiving section.
  • the light receiving section further includes a light combining section that combines the received light received from the scanning section and the local light sent from the optical transmitting section.
  • the received light is the reflected light of the transmitted light from the target object
  • the received light will be a signal delayed from the local light according to the distance to the target, and the composite signal that combines the received light and the local light will be , a constant frequency signal (beat signal).
  • the optical transmitting/receiving section 112 passes this signal to the received signal processing section 113 as a received waveform signal.
  • the received signal processing unit 113 performs predetermined signal processing, such as fast Fourier transform, on the received signal passed from the optical transmitting/receiving unit 112 in synchronization with the synchronization signal supplied from the synchronization signal generation unit 130. Through this signal processing, the received signal processing unit 113 obtains the distance to the object, the Doppler velocity of the object, and the intensity of the received optical signal. The received signal processing unit 113 adds a timestamp generated in synchronization with a synchronization signal to the acquired distance, velocity, and Doppler velocity, and passes them to the transmitting unit 115.
  • the time stamp here is a measurement-related time stamp indicating the timing at which the transmission optical signal was transmitted by the optical transmitter/receiver 112, and is generated and added for each measurement.
  • the optical scanning control section 114 generates a scanning control signal for controlling the scanning of the transmitted optical signal in the optical scanning section 111. At this time, the optical scanning control section 114 generates a scanning control signal so that the scanning of the transmitted optical signal is synchronized with the synchronization signal supplied from the synchronization signal generation section 130.
  • the optical scanning control section 114 may generate a scanning control signal for scanning a predetermined scanning range, or may generate a scanning control signal according to scanning control information transmitted from a control communication section 230, which will be described later. Good too.
  • the optical scanning control unit 114 receives an angle detection signal indicating the scanning angle of the transmitted optical signal from the optical scanning unit 111.
  • the optical scanning control unit 114 passes information indicating the scanning angle to the transmitting unit 115 based on the received angle detection signal.
  • the optical scanning section 111 transmits the transmission optical signal sent from the optical transmission/reception section 112 at an angle according to a scanning pattern according to a scanning control signal supplied from the optical scanning control section 114, and also transmits the transmission optical signal sent from the optical transmission/reception section 112 at an angle according to a scanning pattern according to a scanning control signal supplied from the optical scanning control section 114. It receives light and outputs it as a received optical signal.
  • a two-axis mirror scanning device for example, can be applied as a scanning mechanism for the transmitted optical signal.
  • the scan control signal is, for example, a drive voltage signal applied to each axis of the two-axis mirror scanning device.
  • the optical scanning unit 111 detects the horizontal and vertical angles at which the transmission optical signal is transmitted, and outputs an angle detection signal indicating the detected angle.
  • the transmitting unit 115 sends the time stamp, distance, Doppler velocity, and intensity passed from the received signal processing unit 113 and the scanning angle passed from the optical scanning control unit 114 to the signal processing unit 20 as FMCW-LiDAR data.
  • the FMCW-LiDAR data includes a time stamp for each measurement point in the photodetection and ranging sensor 110.
  • FIG. 4 is a schematic diagram schematically showing an example of a scanning pattern of a transmitted optical signal according to the embodiment.
  • FIG. 4 shows an example of a raster scan pattern among the scan patterns.
  • the optical scanning unit 111 performs scanning within a predetermined angular range 45 according to the scanning line 40 that is folded back at both ends of the angular range 45 in the horizontal direction.
  • the scanning line 40 corresponds to one locus scanned between the left end and the right end of the angular range 45.
  • the optical scanning unit 111 scans between the upper end and the lower end of the angular range 45 according to the scanning line 40 in accordance with the scanning control signal.
  • the optical scanning unit 111 sequentially and discretely changes the emission point 41 of the chirp light as the transmitted optical signal along the scanning line 40 at a constant time interval (point rate), for example, according to the scanning control signal.
  • the injection point 41 within the angular range 45 constitutes one frame in FMCW-LiDAR.
  • the scanning time from the upper end to the lower end of the angle range 45 is one frame time.
  • the transmission optical signals at each emission point 41 are sequentially emitted at predetermined time intervals within one frame according to the scanning line 40. Therefore, the measurement data at each injection point 41 is acquired at different times for each injection point 41 according to the scanning order.
  • the scanning speed by the two-axis mirror scanning device becomes slow. Therefore, the injection points 41 are not arranged in a grid in the angular range 45.
  • the respective emission points 41 for example, at the emission points 41a and 41b (shown filled out) corresponding to the positions where the objects 50a and 50b exist, the emitted transmission optical signal is reflected by the objects 50a and 50b, and the reflected light is , which is returned as a received optical signal.
  • the respective emission points 41 at the emission points 41 corresponding to positions where the objects 50a and 50b are not present, reflected light is not obtained and no received optical signal is obtained.
  • the optical transmitter/receiver 112 may emit the transmission optical signal to one emitting point 41 one or more times. Further, the optical transmitter/receiver 112 emits a transmission optical signal using chirp light whose frequency changes continuously over time at each emission point 41, and the optical scanner 111 continuously changes the scanning angle. Therefore, the transmitted optical signal is irradiated onto the object in an elliptical shape along the scanning line 40, for example.
  • the scanning pattern of the transmitted optical signal is not limited to the raster scanning pattern described above.
  • 5A and 5B are schematic diagrams schematically showing another example of a scanning pattern of a transmitted optical signal.
  • FIG. 5A shows an example of a multi-layer scanning pattern among the scanning patterns.
  • multiple beams are rotated 360 degrees to simultaneously scan multiple lines. More specifically, in multi-layer scanning, the emission point 41 of the transmitted optical signal is sequentially moved at regular time intervals along the scanning lines 42 1 , 42 2 , ..., 42 N of each of the plurality of beams. , vary discretely. In multi-layer scanning, the scanning lines 42 1 , 42 2 , .
  • FIG. 5B shows an example of a dot scanning type scanning pattern among the scanning patterns.
  • the beam scanning trajectory is fixed at each injection point 41 and is therefore discrete rather than continuous.
  • a transmitted optical signal is emitted, and after the reception of the reflected optical signal is completed, the beam is switched to the next emission point 41.
  • section (a) shows an example in which beam switching is performed sequentially according to a prescribed sequence, as indicated by an arrow 47 in the figure.
  • section (b) shows an example in which beam switching is performed in an arbitrary order, for example, by scanning control settings from the signal processing unit 20, as indicated by an arrow 48 in the figure.
  • a predetermined region of the entire scanning region may be scanned by scanning control settings, or a plurality of predetermined regions may be set within one frame. Furthermore, the predetermined area may be determined based on the detected position of a moving object or a stationary object.
  • each component included in the signal processing unit 20 includes a part (referred to as a library part) that provides individual functions, and a part that uses the functions provided by the part to perform the desired processing. It can be roughly divided into two parts (referred to as application parts).
  • the library section includes, for example, a receiving section 200, a sensor position and orientation estimation section 210, a moving object/stationary object separation section 211, a sensor speed estimation section 212, a stationary object point group correction section 213, and a moving object point group.
  • a correction unit 214 is included.
  • the application section includes, for example, a map conversion section 220, a moving object state estimation section 221, a 3D/2D conversion section 222, a ROI (Region of Interest) extraction section 223, a synthesis section 224, a motion recognition section 225, An image recognition unit 226 is included.
  • the configurations included in the library unit and the application unit are not limited to the above-mentioned example.
  • the moving body state estimating unit 221, or the moving body state estimating unit 221 and the 3D/2D converting unit 222 may be included in the library unit.
  • the transmitting unit 201 may be included in either the library unit or the application unit, or may be included in neither.
  • the receiving section 200 may also be included in the application section, either the library section or the application section, or it may be included in neither.
  • the receiving section 200 receives each data output from the sensor section 10 and passes the received data to each section of the signal processing section 20. More specifically, the receiving unit 200 passes the IMU data output from the IMU 100 of the sensor unit 10 to the sensor position and orientation estimation unit 210. The receiving unit 200 also passes the FMCW-LiDAR data output from the photodetection and ranging sensor 110 of the sensor unit 10 to the moving object/stationary object separation unit 211. Furthermore, the receiving unit 200 passes the image data output from the image sensor 120 of the sensor unit 10 to the ROI extracting unit 223.
  • the receiving unit 200 functions as a receiving unit that receives velocity point group data including a plurality of points each having velocity information and time information.
  • the sensor position and orientation estimation unit 210 estimates the position, orientation, and angular velocity of the IMU 100 based on the IMU data passed from the reception unit 200. For example, the sensor position and orientation estimation unit 210 performs sensor fusion processing such as a Kalman filter using sensor data from a 3-axis acceleration sensor, a 3-axis angular velocity sensor, and a 3-axis geomagnetic sensor included in the IMU data. By doing so, the current position, attitude, and angular velocity of the IMU 100 are estimated.
  • sensor fusion processing such as a Kalman filter using sensor data from a 3-axis acceleration sensor, a 3-axis angular velocity sensor, and a 3-axis geomagnetic sensor included in the IMU data.
  • the sensor position and orientation estimating unit 210 acquires calibration data such as the positional relationship of the reference coordinate system of the photodetection and ranging sensor 110 with respect to the reference coordinate system of the IMU 100 and the internal parameters of each sensor in advance, and uses the data at the time of estimation.
  • the sensor position and orientation estimation unit 210 passes data on the estimated sensor position, orientation, and angular velocity to the sensor velocity estimation unit 212, the map conversion unit 220, and the transmission unit 201.
  • the moving object/stationary object separation section 211 receives FMCW-LiDAR data from the receiving section 200, and also receives a sensor speed indicating the speed of the photodetection and ranging sensor 110 estimated by a sensor speed estimating section 212, which will be described later.
  • the moving object/stationary object separation unit 211 performs speed discrimination based on the Doppler speed included in the passed FMCW-LiDAR data and the sensor speed, and separates the speed point group indicated by the FMCW-LiDAR data from the speed point group due to the moving object. It is separated into a velocity point group due to a stationary body.
  • the moving object/stationary object separation unit 211 may extract a moving object velocity point group and a stationary object velocity point group from the FMCW-LiDAR data corresponding to one frame of scanning.
  • the moving object/stationary object separation unit 211 subtracts the optical axis direction component of the sensor speed from the Doppler velocity of each measurement point included in the FMCW-LiDAR data, and calculates the influence of the movement of the sensor unit 10 from the speed information of the speed point group. Then, calculate the corrected Doppler velocity of each measurement point as seen from the stationary body's coordinate system. That is, the corrected Doppler velocity is the Doppler velocity included in the FMCW-LiDAR data, excluding the influence of the movement of the sensor unit 10.
  • the moving object/stationary object separation unit 211 performs a threshold value judgment on the magnitude of this corrected Doppler velocity, and selects a certain spatial range (corresponding to the size of the target object) among the measurement points whose magnitude is equal to or greater than the threshold value.
  • a velocity point group based on localized measurement points (referred to as a localized velocity point group) is extracted as a moving body velocity point group.
  • the moving object/stationary object separation unit 211 may extract a plurality of moving object velocity point groups from one frame of FMCW-LiDAR data.
  • the moving object/stationary object separation unit 211 performs a threshold value determination on the magnitude of the corrected Doppler velocity for the measurement points based on the FMCW-LiDAR data. As a result of this threshold value determination, the moving object/stationary object separation unit 211 extracts a set of measurement points whose corrected Doppler velocity is equal to or less than the threshold value as a stationary object velocity point group.
  • the moving object/stationary object separation unit 211 may provide two types of threshold values, a threshold value v tha and a threshold value v thb ( ⁇ v tha ), in this threshold determination.
  • the moving object/stationary object separation unit 211 may extract measurement points whose corrected Doppler velocities are equal to or greater than a threshold value v tha as a moving object velocity point group, and measurement points whose corrected Doppler velocity is equal to or less than a threshold value v thb as a stationary object velocity point group. This means that there is an intermediate point group that does not belong to either the moving body velocity point group or the stationary body velocity point group. For the purpose of extracting measurement points that are clearly stationary, it is appropriate to provide two types of threshold values in this way.
  • a point cloud frame is composed of a velocity point group obtained from one frame of FMCW-LiDAR data.
  • the moving body/stationary body separation unit 211 passes the extracted stationary body velocity point group to the sensor velocity estimation unit 212 and the stationary body point group correction unit 213. Further, the moving object/stationary object separation section 211 passes the extracted moving object velocity point group to the moving object point group correction section 214 .
  • the sensor speed estimation unit 212 estimates the speed of the sensor unit 10 based on the stationary body point group passed from the moving body/stationary body separation unit 211 and the sensor position, attitude, and angular velocity passed from the sensor position and posture estimation unit 210. Estimate the sensor speed that indicates. A specific example of the sensor speed estimation process by the sensor speed estimator 212 will be described later.
  • the sensor speed estimation unit 212 passes the estimated sensor speed to the stationary body point group correction unit 213 and the moving body/stationary body separation unit 211 , and inputs it to the moving body/stationary body separation unit 211 . Further, the sensor speed estimating unit 212 passes the estimated sensor speed to the transmitting unit 201.
  • the stationary body point cloud correction unit 213 corrects the stationary body velocity point group using the sensor position, orientation, and angular velocity passed from the sensor position and posture estimation unit 210 and the sensor velocity passed from the sensor velocity estimation unit 212. I do.
  • the moving body point cloud correction unit 214 receives the sensor position, orientation, and angular velocity passed from the sensor position and orientation estimation unit 210, the sensor velocity passed from the sensor velocity estimation unit 212, and the sensor velocity passed from the moving body state estimation unit 221, which will be described later.
  • the moving object velocity point group is corrected using information indicating the state of the moving object.
  • the acquisition time of measurement data at each measurement point within a frame varies depending on the type of scanning pattern and the position of the measurement point within the frame.
  • the stationary body point cloud correction unit 213 and the moving body point cloud correction unit 214 estimate the data of each point when each point of the velocity point group in the frame is acquired at the same predetermined time, and adjust the velocity according to the estimation result. Correct the point cloud.
  • FIG. 6 is a schematic diagram schematically showing a speed point group correction process according to the embodiment.
  • section (a) shows an example of the output timing of FMCW-LiDAR data output from the photodetection and ranging sensor 110.
  • FMCW-LiDAR data is output at regular time intervals from frame start time t fst1 to next frame start time t fst2 .
  • Each point of the velocity point group based on FMCW-LiDAR data has time information corresponding to the output timing of each data.
  • the data corresponding to the injection point 41 corresponding to the position where the objects 50a, 50b (see FIG. 4), etc. are present is shown by a solid line (with diagonal lines). Further, data corresponding to the injection point 41 corresponding to a position where objects 50a, 50b, etc. are not present is shown by a dotted line. The data indicated by the dotted line indicates that no object is actually detected from the received optical signal.
  • section (b) schematically shows the state in which the FMCW-LiDAR data output from the photodetection and ranging sensor 110 is corrected by the stationary object point cloud correction section 213 and the moving object point cloud correction section 214.
  • the stationary body point cloud correction unit 213 and the moving body point cloud correction unit 214 calculate each FMCW-LiDAR data from frame start time t fst1 to frame start time t fst2 based on the sensor position, orientation, angular velocity, and sensor velocity, for example.
  • a value at a reference time for correction, for example, the frame start time tfst2 is estimated.
  • the stationary body point group correction unit 213 and the moving body point group correction unit 214 respectively correct the stationary body velocity point group and the moving body velocity point group based on the estimation results.
  • the stationary body point group correction unit 213 and the moving body point group correction unit 214 correct the time information of each point included in the stationary body velocity point group and the moving body velocity point group to the reference time. This eliminates distortion of the point cloud caused by self-motion or moving body motion within the frame time, and in the embodiment, the measurement results by FMCW-LiDAR can be used more generally.
  • the stationary body point group correction unit 213 and the moving body point group correction unit 214 output corrected stationary body velocity point groups and moving body velocity point groups, respectively.
  • the stationary body point group correction unit 213 and the moving body point group correction unit 214 calculate the correction reference time (in the example of FIG. 6, the frame start A timestamp indicating the time t fst2 ) is added and output.
  • the timestamp here is a value for each frame.
  • the stationary body point cloud correction unit 213 and the moving body point cloud correction unit 214 correct at least one attribute value regarding at least one point included in the velocity point cloud data based on the estimated value at a predetermined time. function as a department.
  • the stationary body point group correction unit 213 and the moving body point group correction unit 214 respectively pass the corrected stationary body velocity point group and the moving body velocity point group to the transmitting unit 201.
  • each point included in the stationary body velocity point group and the moving body velocity point group within one frame can be treated as points acquired at the same time, and the stationary body velocity points in the subsequent stage can be treated as points included in the stationary body velocity point group and the moving body velocity point group in one frame. It becomes possible to reduce the processing load on the group and the moving body velocity point group.
  • the velocity point group is corrected based on values estimated based on the sensor position, orientation, and angular velocity, and the sensor velocity. Therefore, in the embodiment, a velocity point group in which each point has time information of the same time can be obtained using one frame of FMCW-LiDAR data.
  • the method of acquiring a velocity point group in which each point has time information of the same time is not limited to the above method.
  • the stationary body point cloud correction unit 213 passes the corrected stationary body velocity point group to the transmission unit 201 and the map conversion unit 220.
  • the map conversion unit 220 estimates and generates map information based on the sensor position, orientation, and angular velocity passed from the sensor position and orientation estimation unit 210 and the stationary body velocity point group passed from the stationary body point cloud correction unit 213. do.
  • the map information includes the self-location of the sensor unit 10 and a map of the surrounding environment of the sensor unit 10.
  • the map conversion unit 220 may apply SLAM (Simultaneous Localization and Mapping) technology to map creation.
  • the map converter 220 passes the generated map information to the transmitter 201.
  • the moving object point cloud correction section 214 passes the corrected moving object velocity point group to the transmitting section 201 and also to the moving object state estimation section 221 and the combining section 224.
  • the moving object state estimation unit 221 separates each moving object included in one frame, for example, based on the corrected moving object speed point group.
  • the moving object state estimation unit 221 estimates the state of each separated moving object, including the position, orientation, speed, and angle of the moving object, based on the corresponding moving object velocity point group.
  • the moving object state estimation section 221 passes moving object state information indicating the estimated state of the moving object to the transmitting section 201 and also to the moving object point group correction section 214 and the 3D/2D conversion section 222.
  • the 3D/2D conversion unit 222 converts the moving body velocity point group, which is 3D (Three-Dimensional) information, into 2D (Two-Dimensional) data corresponding to image data output from the image sensor 120. More specifically, the 3D/2D conversion unit 222 converts the 3D coordinates of the moving body velocity point group into 2D coordinates based on the coordinate system of image data by the image sensor 120. The 3D/2D conversion unit 222 acquires calibration data such as the positional relationship of the reference coordinate system of the light detection and ranging sensor 110 with respect to the reference coordinate system of the image sensor 120 and the internal parameters of each sensor in advance, and uses the data at the time of conversion. do.
  • the 3D/2D conversion unit 222 passes the point group whose coordinates of the moving body velocity point group have been converted into 2D coordinates to the ROI extraction unit 223.
  • the ROI extraction unit 223 extracts a region of interest (ROI) from the image data passed from the image sensor 120 via the reception unit 200, based on the point group based on 2D coordinates passed from the 3D/2D conversion unit 222. More specifically, the ROI extraction unit 223 may extract, for example, a region corresponding to a point group based on 2D coordinates passed from the 3D/2D conversion unit 222 in the image data as a region of interest. Note that if the positional relationship between the photodetection and ranging sensor 110 and the image sensor 120 is known, the ROI extraction unit 223 can instantaneously calculate the correspondence between the image data and the point cloud based on 2D coordinates. It is.
  • ROI region of interest
  • the ROI extraction unit 223 passes the extracted image data of the region of interest to the transmission unit 201 and also to the synthesis unit 224 and the image recognition unit 226.
  • the synthesis unit 224 synthesizes the moving body velocity point group passed from the moving body point group correction unit 214 and the image data of the region of interest passed from the ROI extraction unit 223.
  • the synthesizing unit 224 may synthesize the moving body velocity point group with the image data of the region of interest.
  • the synthesizing unit 224 may associate velocity information of each point included in the moving body velocity point group with each pixel data in the image data of the region of interest. This image obtained by combining speed information with respect to each pixel of the image data of the region of interest is called a composite image.
  • the composition unit 224 is not limited to this, and the composition unit 224 may compose the image data of the region of interest with the moving body velocity point group.
  • the synthesizing unit 224 may associate each point of the moving body velocity point group with corresponding pixel data information in the region of interest.
  • a point group obtained by combining each pixel data of the region of interest with each point of the moving body velocity point group is called a composite point group.
  • the composition unit 224 passes the composite image or composite point group to the transmission unit 201 and also to the motion recognition unit 225.
  • the motion recognition unit 225 recognizes the motion of the moving object based on the composite image or composite point group passed from the composition unit 224.
  • the motion recognition unit 225 can acquire velocity distribution information based on velocity information of each point included in the composite image or composite point group. By using this velocity distribution information, the motion recognition unit 225 can estimate the motion of the moving object more precisely.
  • the motion recognition unit 225 may perform motion recognition using a learning model trained by machine learning based on known velocity distribution information.
  • the motion recognition unit 225 recognizes the motion of a moving object and outputs meta information regarding the motion (walking, running, etc.).
  • the motion meta information output from the motion recognition section 225 is passed to the transmission section 201.
  • the image recognition unit 226 performs image recognition based on the image data of the region of interest passed from the ROI extraction unit 223, and outputs image meta information (person, Mr. ⁇ , vehicle, etc.). .
  • the image recognition unit 226 may perform image recognition using a learning model that has been trained by machine learning based on image data of known images.
  • the image meta information output from the image recognition unit 226 is passed to the transmission unit 201.
  • the transmitter 201 transmits the sensor position, orientation, and angular velocity passed from the sensor position and orientation estimation unit 210 and the sensor velocity passed from the sensor velocity estimation unit 212 to the information processing unit 30. Further, the transmitting unit 201 transmits the map information passed from the map conversion unit 220 and the corrected stationary body velocity point group passed from the stationary body point cloud correction unit 213 to the information processing unit 30. Further, the transmitting unit 201 transmits the moving body velocity point group passed from the moving body point group correction unit 214 and the moving body state information passed from the moving body state estimation unit 221 to the information processing unit 30.
  • the transmitting unit 201 receives the composite image or composite point group passed from the composition unit 224, the ROI image passed from the ROI extraction unit 223, the motion meta information passed from the motion recognition unit 225, and the image recognition unit 201.
  • the image meta information passed from the section 226 is transmitted to the information processing section 30.
  • the transmitting unit 201 adds a time stamp to each of the above-mentioned information and point clouds and transmits them to the information processing unit 30 .
  • the time stamp that the transmitting unit 201 adds to each information and point group may be time information in units of one frame of scanning by the photodetection and ranging sensor 110.
  • information indicating the frame start time t fst2 may be added as a timestamp.
  • the transmitting unit 201 functions as a transmitting unit that adds corrected time information indicating a predetermined time to the attribute value corrected by the correcting unit and transmits the attribute value.
  • the transmitting section 201 may selectively transmit information and point clouds to the information processing section 30 in response to a request from the information processing section 30. Further, the transmitting unit 201 may transmit each of the above-mentioned information and point clouds to a different destination than the information processing unit 30.
  • FIG. 7 is a block diagram showing the hardware configuration of an example of the signal processing system 1 applicable to the embodiment.
  • the sensor unit 10 includes a sensor group 1000 and firmware 1010.
  • Sensor group 1000 includes IMU 100 , photodetection and ranging sensor 110 , and image sensor 120 .
  • the firmware 1010 may be a program for controlling the operation of the sensor group 1000.
  • the firmware 1010 may control, for example, an interface (not shown) as hardware that controls data input/output of each sensor included in the sensor group 1000 or the operation of the synchronization signal generation unit 130.
  • the interface may include a memory in which firmware 1010 is stored in advance, and a processor that operates according to the firmware 1010 stored in the memory.
  • the signal processing section 20 includes a processor 2000, and the above-mentioned library section 2010 and application section 2020 are configured by the processor 2000 operating according to a program stored in a memory (not shown).
  • the processor 2000 may be, for example, an ISP (Image Signal Processor).
  • the processor 2000 is not limited to this, and may be a DSP (Digital Signal Processor) or a CPU (Central Processing Unit).
  • the configuration is not limited to this, and it is also possible to configure some or all of the units included in the library unit 2010 and the application unit 2020 described above using hardware circuits that operate in cooperation with each other.
  • the information processing unit 30 includes a processor 3000 provided by, for example, an ISP, and a library unit 3010 and an application unit 3020 are configured by the processor 3000 operating according to a program stored in a memory (not shown).
  • the library section 3010 is a collection of programs that provide individual functions in the information processing section 30, and the application section 3020 is a collection of programs that execute target processing using the functions provided by the library section 3010. .
  • the library unit 3010 may include a function of outputting a control command for driving the robot.
  • the application section 3020 may include a function of determining the robot's motion based on the map information, motion meta information, image meta information, etc. passed from the signal processing section 20.
  • the library unit 3010 may include a function of outputting a control command for controlling the operation of the surveillance camera.
  • the application unit 3020 may include a function of making a judgment based on an image from a surveillance camera, motion meta information passed from the signal processing unit 20, image meta information, and the like.
  • the configuration is not limited to this, but it is also possible to configure part or all of the units included in the library unit 3010 and the application unit 3020 by hardware circuits that operate in cooperation with each other. Further, the information processing section 30 may be a general computer.
  • the output of each sensor included in the sensor group 1000 (image data, FMCW-LiDAR data, IMU data) is output as RAW data in a predetermined format under the control of the firmware 1010, and is output by an interface (not shown).
  • the signal is transmitted to the signal processing section 20.
  • the signal processing unit 20 receives the RAW data transmitted from the sensor unit 10 using the receiving unit 200 and passes it to the library unit 2021.
  • the library unit 2010 performs the above-described processing on the RAW data passed from the receiving unit 200 and passes the processed data to the application unit 2020.
  • the application unit 2020 performs the above-described processing on the data passed from the library unit 2010, and the transmission unit 201 adds a frame-based time stamp to each piece of information and point cloud generated by this processing. Output. In the example of FIG. 7, the output of the signal processing section 20 is transmitted to the information processing section 30 as SMART data.
  • the information processing unit 30 receives the SMART data transmitted from the signal processing unit 20 and passes it to the library unit 3010.
  • the library unit 3010 performs processing related to individual functions on the passed SMART data and passes it to the application unit 3020.
  • the application unit 3020 performs predetermined processing on the data passed from the library unit 3010 and generates output data.
  • the data generated by the application unit 3020 may be output from the information processing unit 30, for example.
  • the information processing unit 30 may generate a request to the signal processing unit 20 using, for example, the application unit 3020 and transmit the generated request to the signal processing unit 20.
  • the application unit 3020 may generate and transmit to the signal processing unit 20 a request specifying each piece of information that can be transmitted by the signal processing unit 20 by the transmitting unit 201, necessary information from the point cloud, and the point cloud. .
  • the signal processing unit 20 may generate a request to the sensor unit 10 using, for example, the application unit 2020, and may transmit the generated request to the sensor unit 10.
  • the application unit 2020 may generate and transmit a request to the photodetection and ranging sensor 110 to limit the scanning range to the region of interest.
  • MIPI Mobile Industry Processor Interface
  • MIPI-CSI Cara Serial Interface-2
  • FIG. 8 is a schematic diagram showing an example of a data format defined in MIPI-CSI-2 that is applicable to the embodiment.
  • FIG. 8 shows an example of transmitting one frame of image data.
  • a case of data transmission from the sensor section 10 to the signal processing section 20 will be described.
  • the upper left of the diagram is the start position of data transmission, and data is transmitted in order from left to right on the diagram, and further in order from top to bottom on the diagram. .
  • a field FS Frarame Start
  • a field FE Fre End
  • Data transmission starts from the leftmost data, and data transmission for that row sequentially progresses toward the rightmost data.
  • data transmission starts again sequentially from the left end data toward the right end one row lower in the figure.
  • blank areas indicate that there is no data to be transmitted.
  • the field PH is a field in which a packet header is transmitted
  • the field PF is a field in which a packet footer is transmitted.
  • the field Image Data is a field to which image data is transmitted.
  • the image data of the field Image Data is sandwiched between the field PH and the field PF for each row, and is sequentially transmitted from the left side to the right side. Image data output from the image sensor 120 may be transmitted using this field Image Data.
  • the field Embedded Data is a field for transmitting data other than image data. For example, if optional data is defined, the data is generally sent in the field Embedded Data.
  • the data of the field Embedded Data is sandwiched between the field PH and the field PF and is transmitted sequentially from the left side to the right side, similarly to the image data.
  • IMU data output from the IMU 100 of the sensor unit 10 may be transmitted using this field Embedded Data.
  • Information indicating the type of data (IMU data, etc.) transmitted by the field Embedded Data may be transmitted by the field PH.
  • FIG. 9 is a schematic diagram showing another example of a data format defined in MIPI-CSI-2 that is applicable to the embodiment.
  • section (a) is an example in which the field Embedded Data is transmitted after the field Image Data, similar to FIG. 8.
  • Section (b) is an example in which the transmission of the field Image Data is temporarily interrupted and the field Embedded Data is transmitted in the middle of the field Image Data.
  • section (c) is an example in which the field Embedded Data is transmitted before and after the field Image Data.
  • the field Embedded Data may be transmitted before the field Image Data.
  • FIG. 10 is a schematic diagram showing an example of transmitting a point cloud in a data format defined by MIPI-CSI-2, which is applicable to the embodiment.
  • field Embedded Data is similarly transmitted between fields PH and PF.
  • IMU data is transmitted in the field Embedded Data.
  • point cloud data is transmitted by the field Point Cloud Data, which is sandwiched between fields PH and PF.
  • a field FE indicating the end of the frame is transmitted.
  • the field Image Data, the field Embedded Data, and the field Point Cloud Data are transmitted in this order, but the order in which each data is transmitted is not limited to this example. Furthermore, the field Image Data and the field Point Cloud Data may be divided and transmitted within one frame.
  • the interface for data transmission between the sensor section 10 and the signal processing section 20 and between the signal processing section 20 and the information processing section 30 is MIPI, but this is not limited to this example.
  • other interfaces may be applied as interfaces related to these data transmissions.
  • a serial interface mainly used for internal data communication of a device such as a receiver/transmitter
  • a communication interface such as a USB (Universal Serial Bus), or an Ethernet (registered trademark)
  • an interface mainly used for external communication of devices such as USB, Ethernet, or Wi-Fi (Wireless Fidelity) (registered trademark) may be applied between the signal processing unit 20a and the information processing unit 30.
  • Wi-Fi Wireless Fidelity
  • FIG. 12 is a diagram showing an example of the architecture of the signal processing unit 20 according to the embodiment.
  • the signal processing unit 20 has a structure in which an OS (Operating System) 2030 operates on a processor 2000 such as a hardware such as an ISP, and a library unit 2010 and an application unit 2020 operate on the OS 2030.
  • OS Operating System
  • the application section 2020 includes an API (Application Programming Interface) calling section 2040
  • the library section 2010 includes an API processing section 2041.
  • the API calling unit 2040 in accordance with a request from the application unit 2020, calls a function of the library unit 2010 according to the request.
  • the API processing unit 2041 returns a response by the function in the library unit 2010 to the API calling unit 2040 in response to the function calling by the API calling unit 2040.
  • the API calling unit 2040 requests the library unit 2010 for a moving body velocity point group.
  • the API processing unit 2041 calls the function of the moving object point cloud correction unit 214 in response to this request, and returns the moving object velocity point cloud output from the moving object point cloud correction unit 214 to the API calling unit 2040.
  • the API calling unit 2040 passes the moving body velocity point group returned from the API processing unit 2041 to the moving body state estimation unit 221.
  • FIG. 13 is a flowchart of an example of a transmission optical signal detection process in the photodetection and ranging sensor 110, which is applicable to the embodiment.
  • step S10 the light detection and ranging sensor 110 transmits (emits) a transmission optical signal using a laser beam whose frequency is continuously modulated in synchronization with the synchronization signal by the optical transmitting/receiving unit 112, and the received light is reflected from the object and returned. Receive a signal.
  • FIG. 14 is a schematic diagram for explaining a transmission optical signal transmitted by the optical transceiver 112, which is applicable to the embodiment.
  • the upper diagram in FIG. 14 shows the relationship between the optical frequency of the transmitted optical signal and time, and the lower diagram shows the signal transmission start time t st in the upper diagram.
  • the vertical axis represents the optical frequency of the transmitted optical signal
  • the horizontal axis represents time.
  • the optical transmitter/receiver 112 linearly increases and decreases the optical frequency of the laser beam, for example, from a signal transmission start time t st1 to a next signal transmission start time t st2 , and generates chirp light. do.
  • the optical transmitting/receiving section 112 emits the chirp light resulting from the combination of rising and falling optical frequencies as a transmission optical signal from the optical scanning section 111 toward a predetermined emission point.
  • chirp light is generated by linearly increasing and decreasing the optical frequency of the transmitted optical signal according to the above-mentioned period, and is emitted from the optical scanning unit 111 as a transmitted optical signal.
  • the transmitted optical signal is thus formed by chirped light with a set of rising and falling optical frequencies that are transmitted successively.
  • the light detection and ranging sensor 110 uses the received signal processing unit 113 to calculate a first peak value and a first peak value from the first received spectrum signal in the received received optical signal. Estimate the signal spectrum frequency (peak frequency) at the peak value. Further, the light detection and ranging sensor 110 uses the received signal processing unit 113 to calculate a second peak value and a signal spectrum frequency (peak frequency) at the peak value from the second received spectrum signal in the received received optical signal. presume.
  • the first received spectrum signal is a signal corresponding to a transmitted optical signal in a period in which the optical frequency increases among the received optical signals.
  • the second received spectrum signal is a signal corresponding to the transmitted optical signal in a period in which the optical frequency decreases among the received optical signals.
  • the photodetection and ranging sensor 110 determines in the received signal processing unit 113 whether the first peak value and the second peak value estimated in step S11 are greater than or equal to the threshold th.
  • FIG. 15 is a schematic diagram for explaining determination processing for received optical signals according to the embodiment.
  • section (a) is a diagram showing determination processing for the first received spectrum signal
  • section (b) is a diagram showing determination processing for the second received spectrum signal.
  • the vertical axis shows the intensity of the received optical signal
  • the horizontal axis shows the received spectrum frequency.
  • the intensity of the first received spectrum signal exceeds the threshold th at the signal spectrum frequency f pk1 and the first peak value V pk1 is obtained.
  • the signal spectrum frequency f pk1 at which the intensity of the first received spectrum signal takes the first peak value V pk1 is defined as the first peak frequency.
  • the intensity of the second received spectrum signal exceeds the threshold th at the signal spectrum frequency f pk2 , and the second peak value V pk2 is obtained.
  • the signal spectrum frequency f pk2 at which the intensity of the second received spectrum signal takes the second peak value V pk2 is defined as the second peak frequency.
  • step S12 determines that at least one of the first peak value V pk1 and the second peak value V pk2 is less than the threshold th (step S12, "No"), the process moves to step S15.
  • the received signal processing unit 113 determines that the first peak value V pk1 and the second peak value V pk2 are each greater than or equal to the threshold th (step S12, "Yes"), the process proceeds to step S13.
  • step S13 the received signal processing unit 113 selects the first peak value V pk1 and the second peak value V pk2 , and the signal spectrum frequency f pk1 that takes the first peak value V pk1 and the second peak value V pk2 . and f pk2 , the distance to the object, the Doppler velocity with respect to the object, and the intensity of the received optical signal are calculated. Furthermore, the received signal processing unit 113 calculates the signal transmission start time t st1 based on the synchronization signal.
  • the emission point 41 corresponding to the received optical signal in which the first peak value V pk1 and the second peak value V pk2 are each greater than or equal to the threshold value th is defined as the detection point where the object is detected.
  • the received signal processing unit 113 outputs the distance, Doppler velocity, and intensity calculated in step S13. Further, the received signal processing unit 113 outputs the signal transmission start time calculated in step S13 as a timestamp for each measurement (for each injection point).
  • the transmitter 115 receives the distance, Doppler velocity and intensity output from the received signal processor 113, and the time stamp.
  • the transmitter 115 adds a time stamp to the received distance, Doppler velocity, and intensity, and outputs them as FMCW-LiDAR data from the photodetection and ranging sensor 110.
  • the photodetection distance measurement sensor 110 determines whether the measurement has ended. For example, the photodetection and distance measurement sensor 110 may determine whether the measurement has been completed according to a control signal (not shown) input to the sensor unit 10 from the outside. When the photodetection and ranging sensor 110 determines that the measurement has ended, it ends the series of processes according to the flowchart of FIG. 14 .
  • step S15 determines in step S15 that the measurement has not been completed. If the photodetection distance measurement sensor 110 determines in step S15 that the measurement has not been completed, the process returns to step S10 and executes the process for the next injection point.
  • step S12 in the flowchart of FIG. 13 will be explained using a specific example.
  • FIG. 16 is a schematic diagram showing an example of data output when the photodetection and ranging sensor 110 scans in a raster scanning pattern according to the embodiment.
  • section (a) corresponds to FIG. 4 described above, in which the vertical axis indicates the vertical angle of scanning, and the horizontal axis indicates the horizontal angle of scanning.
  • Section (b) shows an example in which FMCW-LiDAR data output from the photodetection and ranging sensor 110 corresponding to each emission point 41 is arranged in time series.
  • the emission points 41a and 41b shown filled in in the figure are emission points corresponding to the positions where the objects 50a and 50b (see FIG. 4) exist, for example, and the reflected light reflected by the objects 50a and 50b is received. It is returned as an optical signal.
  • the light detection and ranging sensor 110 scans between the upper end and the lower end of the angular range 45 within a predetermined angular range 45 by the optical scanning unit 111, following the scanning line 40 that is folded back at both ends of the angular range 45 in the horizontal direction. .
  • Each injection point 41, 41a and 41b within this angular range 45 constitutes a point cloud frame.
  • the output data of each emission point 41 is output at predetermined time intervals, for example, according to the emission timing of the transmission optical signal.
  • the light detection and ranging sensor 110 receives the transmitted optical signal. Either the corresponding received optical signal is not received, or it can be assumed that the received received optical signal is noise.
  • step S12 the process moves to step S13.
  • step S12 the process moves to step S15.
  • the processes of steps S13 and S14 in the flowchart of FIG. 13 are executed at the emission points 41a and 41b corresponding to the positions where the objects 50a and 50b are present.
  • the processes of step S13 and step S14 are canceled, and the next injection point Processing for is executed.
  • FIG. 17 is a schematic diagram for explaining frame definitions applicable to the embodiment.
  • section (a) shows an example of a frame based on image data output from the image sensor 120.
  • Sections (b) and (c) in FIG. 17 each show examples of frames based on FMCW-LiDAR data output from the photodetection and ranging sensor 110.
  • section (b) shows an example of a raster scan pattern by raster scan
  • section (c) shows an example of a dot scan pattern by dot scan.
  • Raster scanning is realized, for example, by a mechanical mirror scanner. Further, dot scanning is realized by a beam steering device such as an OPA (Optical Phase Array) or a light beam switching element.
  • OPA Optical Phase Array
  • a frame 60a is composed of a set of pixels 61 including information of each color of R (red), G (green), and B (blue).
  • the size of the frame 60a is expressed by the number of pixels 61 in the width direction and height direction.
  • Each pixel 61 is arranged in a matrix, corresponding to each pixel included in the effective pixel area of the pixel array.
  • a frame (point cloud frame) 60b is composed of the injection points 41 within a predetermined angular range 45.
  • scanning is performed according to the scanning lines 40 that are folded back at both ends of the angular range 45 in the horizontal direction.
  • the position of each emission point 41 on the scanning line 40 is represented by the horizontal and vertical scanning angles (horizontal angle ⁇ hi , vertical angle ⁇ vi ) of the optical signal transmitted by the optical scanning unit 111.
  • a transmission optical signal is formed by continuously transmitting chirp light with a set of increasing and decreasing optical frequencies.
  • Each emission point 41 shown in section ( b ) of FIG. 17 has a scanning angle (horizontal angle ⁇ h i , vertical angle ⁇ v i ).
  • each injection point 43 within a predetermined scanning range 46 constitutes a frame (point group frame) 60c.
  • each emission point 43 completes the chirp light. That is, a transmission optical signal is transmitted for each emission point 43. Therefore, the position of each injection point 43 may be represented by xy coordinates within the scanning range 46.
  • FIGS. 18A and 18B are schematic diagrams for explaining the output timing of each data by the sensor unit 10 according to the embodiment.
  • the image sensor 120 is exposed using a global shutter method in which all pixels are exposed simultaneously.
  • FIG. 18A shows an example of measurement timing by each sensor of the sensor unit 10 according to the embodiment.
  • the IMU 100, the photodetection distance sensor 110, and the image sensor 120 each perform a measurement operation in synchronization with a synchronization signal Sync supplied from the synchronization signal generation unit 130, and output data. It is assumed that the synchronization signal Sync is output from the synchronization signal generation unit 130 at a cycle corresponding to one frame cycle of image data.
  • the image sensor 120 starts exposure at exposure start time t exst1 in synchronization with the synchronization signal Sync, and is exposed during the exposure period t ex . Exposure of the next frame is started at exposure start time t exst2 after one frame period of image data.
  • the frame period of image data is approximately 10 ms (milliseconds) to 100 ms.
  • one frame of image data resulting from exposure starting from the exposure start time t exst1 is output from the end of the exposure period t ex to the next exposure start time t exst2 .
  • the photodetection and ranging sensor 110 starts scanning one frame at frame start times t fst1 , t fst2 , . . . which are synchronized with the exposure start times t exst1 , t exst2 , . . . , in response to the synchronization signal Sync, for example. be done.
  • FIG. 18B schematically shows the relationship between the exposure start time t exst in the image sensor 120 and the frame start time t fst in the photodetection and ranging sensor 110. As shown in FIG.
  • the emission interval of the transmitted optical signals at each emission point 41 is, for example, about 1 ⁇ s (microsecond) to several 100 ⁇ s.
  • FMCW-LiDAR data is output according to the injection timing of each injection point 41.
  • the emission point 41 indicated by a dotted line in the figure is an emission point where the transmitted optical signal is not reflected by an object or the like and the received optical signal is not received, or where the intensity of the received optical signal is less than the threshold th. It shows.
  • an emission point 41 indicated by a solid line and shaded in the figure indicates an emission point where a transmitted optical signal is reflected by an object, for example, and a received optical signal with an intensity equal to or higher than the threshold value th is received. Therefore, FMCW-LiDAR data is actually output only at the timing corresponding to the injection point 41 indicated by a solid line and shaded in the figure.
  • the IMU 100 outputs IMU data in synchronization with the beginning of the image data and FMCW-LiDAR data frames in response to the synchronization signal Sync.
  • the IMU 100 may output IMU data in accordance with, for example, a signal obtained by multiplying the synchronization signal Sync during a period from one synchronization signal Sync to the next synchronization signal Sync.
  • the IMU data output interval in the IMU 100 is, for example, about 1 ms to 1000 ms.
  • FIG. 19 is a schematic diagram showing an example of the data structure of FMCW-LiDAR data and IMU data output from the sensor unit 10 according to the embodiment.
  • section (a) shows an example of the data structure of FMCW-LiDAR data
  • section (b) shows an example of the data structure of IMU data. Both sections (a) and (b) represent time vertically on the diagram.
  • each injection point is shown as point #1, point #2, . . . , point #N.
  • each injection point will be described as point #1, point #2, . . . , point #N as appropriate.
  • point #i will be used as a representative point, and each subscript number in the diagram will be explained using i as its representative (Section (b) as well).
  • the header includes at least scan type information.
  • the scan type information may be information indicating a scanning method, such as raster scanning, multilayer scanning, or dot scanning.
  • the data at point #i includes a time stamp Ts i and a scan angle (horizontal angle ⁇ h i , vertical angle ⁇ v i ) as information regarding scanning, and distance d i as measurement data obtained based on the received optical signal. , Doppler velocity w i , and intensity Lp i .
  • the time stamp Ts i indicates the time when the transmission optical signal at point #i was transmitted.
  • each point #i may be a point where the intensity of the above-mentioned received optical signal becomes a value equal to or greater than the threshold value th in the first and second received spectrum signals.
  • the time stamp Ts i is added to the data of each point #i, the data of each point i included in one frame of FMCW-LiDAR data is data based on the received optical signal corresponding to which emission point. It is possible to easily know whether
  • the processing in the signal processing unit 20 for the data of each point #i may differ depending on the scanning method.
  • the header transmitted at the beginning of the frame includes scan type information indicating the scanning method. Therefore, the signal processing unit 20 can perform processing on this FMCW-LiDAR data according to the scanning method.
  • the IMU data includes a timestamp Ts-IMU i , a 3-axis acceleration a i , a 3-axis angular velocity ⁇ i and a 3-axis geomagnetic field ge i at each measurement timing in the IMU 100.
  • the first data of the frame to which the timestamp Ts-IMU 1 is added is synchronized with the data at the first point #1 in the FMCW-LiDAR data shown in section (a), for example.
  • the IMU 100 performs measurement at regular time intervals. Therefore, for example, by synchronizing the first data of a frame with the data at point #1 of the FMCW-LiDAR data, the measurement timing of each data can be estimated, and each time stamp Ts-IMU i can be omitted. .
  • Point cloud correction processing according to embodiment Next, point cloud correction processing according to the embodiment will be described.
  • the point cloud correction process according to the embodiment is executed in the stationary object point cloud correction section 213 and the moving object point group correction section 214 shown in FIG.
  • FIG. 20 is a schematic diagram showing an example of output data from the photodetection and ranging sensor 110 according to the embodiment.
  • the photodetection and ranging sensor 110 outputs a detection time t i of a point i as a detection point, a position vector r i of the point i, and a Doppler velocity w i of the point i.
  • point i refers to an emission point at which the intensities of the first and second received spectrum signals in the received optical signal are each greater than or equal to the threshold th.
  • FIG. 21 is a schematic diagram showing the definition of the coordinates of the injection point, which is applicable to the embodiment.
  • a vertical angle ⁇ v in the vertical direction and a horizontal angle ⁇ h in the horizontal direction are defined with the center of the angular range 45 related to scanning as the origin.
  • the injection point 41 at the upper left corner of the angle range 45 is designated as point #1, and points #1, #2, ... are designated from left to right, and the injection point 41 at the lower right corner of the angle range 45 is designated as point #N.
  • the injection point 41 at a position corresponding to the object 52 to be measured is defined as a point i.
  • FIG. 22 is a schematic diagram showing each emission point 41 and object 52 as seen from the photodetection and ranging sensor 110.
  • characters with a dot immediately above them to indicate time differentiation are indicated by a dot immediately preceding them, such as "x".
  • a character with a ⁇ '' (tilde) placed immediately above it to indicate an angular velocity tilde matrix is shown with a ⁇ '' placed immediately before the character, such as ⁇ x.''
  • the position of the photodetection and ranging sensor 110 is defined as a position Os
  • a sensor coordinate system is defined by coordinate axes xs , ys , and zs , with the position Os as the origin.
  • each injection point 41 is shown as a point equidistant from the photodetection distance measurement sensor 110 for convenience.
  • the vector ⁇ v i indicates the motion of the object 52
  • the vector ⁇ w i indicates the Doppler velocity of the object 52 with respect to the sensor position O s
  • a vector ⁇ r i directed from the position O s to the point i indicates the distance from the photodetection and ranging sensor 110 to the point i on the object 52 .
  • the information that we ultimately want to obtain is the vector ⁇ v i .
  • FIG. 23 is a schematic diagram showing the overall flow of processing related to point cloud correction in the signal processing unit 20 according to the embodiment.
  • sensor position/orientation estimation processing 400 stationary object/moving object discrimination processing 401, and sensor speed estimation processing 402 are performed by the sensor position/orientation estimation section 210, moving object/stationary object separation section 211, and sensor speed estimating section 212 of FIG. 2, respectively.
  • the process will be as follows.
  • the point group correction processes 403 and 404 are the processes of the stationary object point group correction section 213 and the moving object point group correction section 214 in FIG. 2, respectively.
  • the moving object detection process 405 is a process of the moving object state estimation unit 221 in FIG. 2 . Note that the processing of the map conversion unit 220 in FIG. 2 is shown as SLAM processing 410 in FIG.
  • the IMU 100 outputs acceleration_a s , angular velocity_ ⁇ s , and geomagnetism_ ⁇ s as IMU data.
  • the sensor position and orientation estimation process 400 uses the acceleration_as , angular velocity_ ⁇ s, and geomagnetism_ ⁇ s output from the IMU 100, and the sensor position_ps ,j and sensor orientation Rs, obtained by the SLAM process 410 . Based on j , sensor angular velocity ⁇ s,j and sensor velocity _v s,j , sensor position _p s , sensor attitude R s and sensor angular velocity ⁇ s are estimated.
  • the sensor position and orientation estimation process 400 passes the estimated sensor position p s to the SLAM process 410 . Further, the sensor position and orientation estimation processing 400 passes the estimated sensor orientation R s and sensor angular velocity ⁇ s to the sensor velocity estimation processing 402 .
  • the photodetection and ranging sensor 110 stores, as FMCW-LiDAR data, a detection time t i , a distance_r s i (t i ) as a position at the detection time t i , and a Doppler velocity w i (t i ) and output.
  • Each piece of data output from the photodetection and ranging sensor 110 is passed to a stationary object/moving object discrimination process 401 .
  • the stationary body moving object discrimination process 401 determines the position of the stationary body at the detection time ti based on each data passed from the photodetection and ranging sensor 110 and the sensor speed_vs estimated in the sensor speed estimation process 402.
  • the distance ⁇ _r s i (t i ) ⁇ i static
  • the Doppler velocity ⁇ w i (t i ) ⁇ i static
  • curly brackets " ⁇ " and “ ⁇ ” indicate that the enclosed part is a set.
  • the sensor speed estimation process 402 passes the calculated sensor speed_v s to the stationary object/moving object discrimination process 401 , the point cloud correction processes 403 and 404 , and the SLAM process 410 .
  • Point cloud correction processing 404 uses sensor orientation R s and sensor angular velocity ⁇ s passed from sensor position and orientation estimation processing 400, and distance ⁇ _r s i (t i ) ⁇ i passed from stationary object moving object discrimination processing 401.
  • Dynamic Dynamic
  • Doppler velocity ⁇ w i (t i ) ⁇ i dynamic
  • stationary object detection time ⁇ t i ⁇ i dynamic
  • sensor velocity_v s passed from sensor velocity estimation processing 402
  • Each value output from the moving object detection process 405 is passed to, for example, the transmitter 201 (see FIG. 2). Furthermore, the moving object detection processing 405 sends the calculated moving object position ⁇ _p m ⁇ j , moving object posture ⁇ R m ⁇ j and ⁇ _v m ⁇ j , and moving object angular velocity ⁇ m ⁇ j to the point cloud correction processing 404. hand over.
  • FIG. 24 is a flowchart of an example of processing related to point cloud correction in the signal processing unit 20 according to the embodiment. The processing in FIG. 24 is executed for each frame of measurement processing by the photodetection and ranging sensor 110.
  • step S200 the signal processing unit 20 starts acquiring frames based on the FMCW-LiDAR data output from the photodetection and ranging sensor 110.
  • the signal processing unit 20 acquires the distance and Doppler velocity of the detection point where the received optical signal was detected, the detection time, and the scanning angle by the stationary object/moving object discrimination process 401.
  • the signal processing unit 20 uses the stationary object and moving object discrimination processing 401 to obtain the sensor speed estimated by the sensor speed estimation processing 402 in the previous processing.
  • the signal processing unit 20 uses the sensor velocity acquired in step S202 to perform coordinate transformation on the Doppler velocity of the detection point by the stationary object/moving object discrimination process 401, and calculates the corrected Doppler velocity.
  • the signal processing unit 20 determines whether the absolute value of the corrected Doppler velocity of the detection point calculated in step S203 is less than or equal to a threshold value by the stationary/moving object discrimination process 401.
  • step S204 determines that the absolute value of the corrected Doppler velocity of the detection point is equal to or less than the threshold value in the stationary body/moving body discrimination process 401 (step S204, "Yes")
  • the signal processing unit 20 changes the processing to the process of the stationary body point group. The process proceeds to step S210.
  • step S210 the signal processing unit 20 adds the detection point to the velocity point cloud frame of the stationary body through point cloud correction processing 403 that corrects the stationary body point group.
  • the signal processing unit 20 uses point cloud correction processing 403 to obtain the sensor orientation and angular velocity estimated in sensor position and orientation estimation processing 400.
  • the signal processing unit 20 estimates the sensor velocity using the velocity point group frame of the stationary zone, the sensor attitude, and the angular velocity through sensor velocity estimation processing 402.
  • the signal processing unit 20 uses the sensor velocity estimated in step S212, the sensor attitude and angular velocity acquired in step S211, and the time difference from the previous processing by point cloud correction processing 403. , correct the velocity point cloud frame of a stationary body.
  • the signal processing unit 20 determines whether the point group correction processing 403 has completed processing for all points in the frame.
  • the signal processing unit 20 determines that the process is completed by the point cloud correction process 403 (step S214, "Yes")
  • the signal processing unit 20 moves the process to step S230.
  • the signal processing unit 20 returns the processing to step S201 and performs the next correction in the frame based on the FMCW-LiDAR data. Execute processing for the detected points.
  • step S204 determines in the above-described step S204 that the absolute value of the corrected Doppler velocity of the detection point exceeds the threshold by the stationary object/moving object discrimination process 401 (step S204, "No"), the signal processing unit 20 performs the processing on the moving object point.
  • the process moves to step S220 related to group processing.
  • step S220 the signal processing unit 20 adds the detection point to the velocity point group frame of the moving object through point cloud correction processing 404 that corrects the moving object point group.
  • step S221 the signal processing unit 20 uses point cloud correction processing 404 to obtain the sensor orientation and angular velocity estimated in sensor position and orientation estimation processing 400.
  • the signal processing unit 20 performs clustering on the velocity point group of the moving body by point cloud correction processing 404, and divides the velocity point group of the moving body into local velocity point groups. That is, the signal processing unit 20 uses the point cloud correction process 404 to divide the velocity point group included in the velocity point cloud frame of the moving object into velocity point groups for each moving object.
  • the signal processing unit 20 performs point cloud correction processing 404 using the sensor velocity, sensor orientation, and angular velocity, the position, orientation, velocity, and angular velocity of the moving object, and the time difference from the previous processing. , correct the point cloud cluster frame (local velocity point group) of each moving object in the velocity point cloud frame.
  • step S224 the signal processing unit 20 determines whether the point group correction processing 404 has completed processing for all points in the frame.
  • the signal processing unit 20 determines that the point cloud correction processing 404 has completed the processing (step S224, "Yes")
  • step S224 the signal processing unit 20 moves the processing to step S230.
  • step S224 the signal processing unit 20 determines that the processing is not completed by the point cloud correction processing 404 (step S224, "No")
  • step S224 determines that the processing is not completed by the point cloud correction processing 404 (step S224, "No")
  • the signal processing unit 20 returns the processing to step S201, and processes the next detection point in the frame. Execute.
  • step S230 the signal processing unit 20 generates a stationary body velocity point cloud frame based on the stationary body point group corrected in the point cloud correction process 403 and a moving body velocity point cloud frame based on the moving body point group corrected in the point cloud correction process 404. and outputs.
  • the output stationary body velocity point cloud frame is passed to, for example, SLAM processing 410.
  • the outputted moving object velocity point group frame is passed to, for example, moving object detection processing 405.
  • FIG. 25 is a flowchart of an example showing correction processing of a velocity point group frame of a stationary body in the signal processing unit 20 according to the embodiment.
  • the flowchart in FIG. 25 shows in more detail the process of step S213 in the flowchart in FIG. 24 described above.
  • step S250 the signal processing unit 20 uses the point cloud correction process 403 to obtain the sensor speed estimated by the sensor speed estimation process 402.
  • the signal processing unit 20 acquires the coordinates of each point included in the velocity point cloud frame of the stationary body by point cloud correction processing 403.
  • the signal processing unit 20 uses point cloud correction processing 403 to obtain the sensor orientation and acceleration estimated in sensor position and orientation estimation processing 400.
  • the signal processing unit 20 uses the point cloud correction processing 403 to calculate the instantaneous velocity vector of each point included in the velocity point cloud frame of the stationary body based on the values acquired in steps S250 to S252. Estimate.
  • the signal processing unit 20 uses the point group correction process 403 to calculate the minute position change of each point from the product of the difference time from the previous process and the instantaneous velocity vector of each point.
  • the signal processing unit 20 performs point cloud correction processing 403 to shift the position of each point based on the calculated minute position change of each point, and corrects the velocity point cloud frame of the stationary body.
  • FIG. 26 is a schematic diagram for defining the coordinate system and variables used in the following explanation.
  • the position of the photodetection and ranging sensor 110 (hereinafter referred to as sensor ) is taken as the position Os
  • the sensor coordinate system is defined by coordinate axes xs , ys , and zs that are orthogonal to each other, with the position Os as the origin. be done.
  • the angular velocity and velocity of the sensor be angular velocity ⁇ ⁇ s and velocity ⁇ v s , respectively.
  • a rigid body coordinate system is defined by axes x m , y m , and z m that are orthogonal to each other, with a position O m on the rigid body as the origin. Let the angular velocity and velocity of the rigid body be ⁇ m and ⁇ v m , respectively.
  • Point i which is the detection position on the rigid body, is the position indicated by the vector ⁇ u i from the position O m , and the point i is the velocity ⁇ v i and the Doppler velocity ⁇ w i with respect to the photodetection and ranging sensor 110. shall have it. It is assumed that point i is located at a distance of ⁇ r i when viewed from the sensor.
  • a world coordinate system is defined by mutually orthogonal axes x, y, and z, with an arbitrary position O in space as the origin.
  • position O m which is the origin of the world coordinate system
  • point i is located at position ⁇ p i .
  • position O s which is the origin of the sensor coordinate system is assumed to be located at the position ⁇ p s .
  • equation (2) the angular velocity tilde matrix ⁇ s indicating the angular velocity of the rigid body is expressed as shown in equation (4) below based on the angular velocity vector _ ⁇ of equation (3) below.
  • equation (2) if the rigid body coordinate system is stationary, the following equation (5) holds true.
  • Equation (8) is transformed as shown in Equation (9) below to organize the relationship between each value and obtain the relationship between the Doppler velocity wi and the velocity vector of point i, _p i .
  • Equation (10) can be expressed as the following equation (11) using the sensor attitude R s .
  • Equation (12) -(r s i /r i ) represents a beam direction vector, which can be expressed using a matrix as shown in Equation (12) below.
  • equation (14) the following equation (15) representing the sensor speed_v s can be obtained.
  • the sensor speed_vs is estimated using this equation (15).
  • the matrix with the value e i indicates the matrix of the beam direction vector in the stationary body and world coordinate system.
  • the velocity vector _v i at point i is expressed by the following equation (16).
  • Equation (16) is transformed to obtain the following equation (17).
  • Equation (17) is further modified to obtain the following equation (18).
  • the left side indicates the position vector of each detected point (point i) after correction in the sensor coordinate system. Furthermore, the last term on the right side indicates the position vector in the sensor coordinate system of each detection point before correction.
  • the measurement difference time ⁇ t i and the correction coefficient at the point i are calculated. By adding a position correction value using a correction term based on ⁇ s i , the position of each point in the stationary body point group is corrected.
  • the coordinate system is the same as that in FIG. 26 except that the stationary body (rigid body) in FIG. 26 described above is replaced with a moving body (rigid body), so the description thereof will be omitted here.
  • the velocity vector ⁇ _p i of the rigid body coordinate system at point i is expressed by the following equation (23). Note that the velocity vector ⁇ _p i of the sensor coordinate system at point i is the same as the above-mentioned equation (2), so the explanation here will be omitted.
  • Equation (24) is obtained by transforming equation (23) using the above equation (6) based on the vector constant equation and the velocity vector _p i of the sensor coordinate system at point i in equation (2). It will be done.
  • Equation (24) is further transformed as shown in Equation (25) below, and the relationship between each value is rearranged.
  • a threshold value determination is performed on the corrected Doppler velocity w i ′ to determine whether the point i is a moving body or a stationary body. For example, a moving object determination threshold value v th1 for determining a moving object and a stationary object determination threshold value v th2 for determining a stationary object are set.
  • v th1 for determining a moving object
  • v th2 for determining a stationary object
  • the absolute value of the corrected Doppler velocity w i ′ is less than the stationary object determination threshold v th2 (
  • the absolute value of the corrected Doppler velocity w i ' exceeds the moving object determination threshold v th1 (
  • the point i is a moving object.
  • Equation (27) is transformed using the sensor attitude R s to obtain the following equation (28).
  • equation (29) is expressed as the following equation (31) using a determinant.
  • Equation (32) is transformed using the value ⁇ i of equation (33) and the value e i defined using equation (14), and the following equation (34) representing the moving body velocity_v m is obtained. obtain.
  • the velocity vector _v i of point i in the sensor coordinate system is expressed by the following equation (35).
  • Equation (35) and Equation (36) are made equal, and Equation (35) is transformed using sensor orientation R s to obtain the following Equation (37).
  • Expression (38) is converted into a discrete equation, and the measurement difference time ⁇ t i is applied to obtain the following expression (39).
  • Equation (40) the correction coefficient ⁇ s i is defined as shown in Equation (40) below.
  • the left side indicates the position vector of each detected point (point i) after correction in the sensor coordinate system. Furthermore, the last term on the right side indicates the position vector in the sensor coordinate system of each detection point before correction.
  • the measurement difference time ⁇ t i and the correction coefficient at the point i are calculated. By adding a position correction value using a correction term based on ⁇ s i , the position of each point in the moving object point group is corrected.
  • a first modification of the embodiment according to the present disclosure is an example in which a configuration for controlling scanning of the photodetection and ranging sensor 110 is added to the signal processing system 1 according to the embodiment shown in FIG.
  • FIG. 27 is a block diagram showing the configuration of an example of a signal processing system according to the first modified example of implementation.
  • the signal processing system 1a includes a control communication section 230, a scanning control section 231, and a parameter setting section 232 for the signal processing section 20a in the signal processing system 1 shown in FIG. and have been added.
  • the control communication unit 230 generates scan control information for controlling scanning in the photodetection and ranging sensor 110 based on the coordinate reference information and ROI designation information transmitted from the information processing unit 30.
  • control communication unit 230 passes the received coordinate reference information to the parameter setting unit 232.
  • the parameter setting unit 232 sets, for example, initial values related to scanning of the photodetection and ranging sensor 110 based on the passed coordinate reference information.
  • the control communication unit 230 passes the initial value set by the parameter setting unit 232 to the light detection and ranging sensor 110 as scanning control information.
  • the photodetection and ranging sensor 110 initializes the scanning mechanism and the like according to the scanning control information passed from the control communication unit 230.
  • the control communication unit 230 passes the received ROI designation information to the scanning control unit 231.
  • the ROI designation information may include, for example, coordinate information set as a region of interest in the image data.
  • the scan control unit 231 converts the coordinate information into horizontal and vertical angle range information for controlling the scanning range of the photodetection and ranging sensor 110 based on the passed ROI designation information, and performs control communication. Hand it over to department 230.
  • the control communication unit 230 passes information on the horizontal and vertical angle ranges passed from the scan control unit 231 to the light detection and ranging sensor 110 as scan control information.
  • the photodetection and ranging sensor 110 controls the scanning range according to the passed scanning control information.
  • the photodetection and distance measurement sensor 110 can be controlled to scan a specified area of interest in a limited and high density manner, making it possible to achieve more accurate motion recognition and image recognition in the area of interest. be.
  • the ROI specification information is not limited to this, and the ROI specification information may be passed from the ROI extraction unit 223. That is, the ROI extraction unit 223 extracts information indicating the region of interest extracted from the image data passed from the image sensor 120 via the reception unit 200 based on the point group based on 2D coordinates passed from the 3D/2D conversion unit 222. , is passed to the control communication unit 230 as ROI designation information.
  • the control communication unit 230 passes the ROI designation information passed from the ROI extraction unit 223 to the scanning control unit 231.
  • a second modification of the embodiment according to the present disclosure is an example in which the image sensor 120 and the configuration that performs processing related to image data are omitted from the signal processing system 1 according to the embodiment shown in FIG.
  • FIG. 28 is a block diagram showing the configuration of an example of a signal processing system according to a second modification of the embodiment.
  • the image sensor 120 is omitted from the sensor section 10 shown in FIG. 2 in the sensor section 10a.
  • the signal processing system 1b includes a configuration related to processing of image data from the signal processing unit 20 shown in FIG. , the synthesis section 224, the motion recognition section 225, and the image recognition section 226 are omitted.
  • the stationary point group correction unit 213 and the moving object point group correction unit 214 always target the entire frame in FMCW-LiDAR for correction processing, but this is limited to this example. Not done. For example, whether or not to perform correction by the stationary point cloud correction unit 213 and the moving object point cloud correction unit 214 may be set for the entire frame, or for one or more areas set within the frame. May be set.
  • each point may be set for each point (detection point) included in the frame whether or not to perform correction by the stationary object point group correction section 213 and the moving object point group correction section 214.
  • the determination for each point as to whether or not to perform correction may be made based on at least one attribute information of each point in the frame or the attribute value of a point in a frame before the frame.
  • the signal processing unit 20 determines whether or not to perform the correction by the stationary point group correction unit 213 and the moving object point group correction unit 214, and in which unit (area, point by point, entire frame) the correction is to be performed. It can be specified from outside. For example, the information processing section 30 may make this designation to the signal processing section 20. Further, in the first modification of the embodiment described above, the information processing section 30 may transmit designation information indicating this designation to the control communication section 230 in the signal processing section 20a. The control communication section 230 may control the operations of the stationary point group correction section 213 and the moving object point group correction section 214 based on this designation information.
  • each data such as the stationary body velocity point group and the moving body velocity point group output from the transmitter 201 according to the above-mentioned correction unit.
  • Information may be added.
  • the present technology can also have the following configuration.
  • a receiving unit that receives speed point cloud data including a plurality of points each having speed information and time information from the first sensor; a correction unit that corrects at least one attribute value regarding at least one point included in the speed point group data based on an estimated value at a predetermined time; a transmitting unit that adds and transmits corrected time information indicating the predetermined time to the attribute value corrected by the correcting unit; Equipped with Signal processing device.
  • the time information indicates a time at which each of the plurality of points was acquired by the first sensor.
  • the signal processing device according to (1) above.
  • At least one of the predetermined times is given for each frame of a detection operation by the first sensor.
  • the receiving section includes: further receiving inertial measurement data from a second sensor;
  • the correction unit is calculating the estimated value based on the speed information, the time information, and the inertial measurement data;
  • the signal processing device according to any one of (1) to (3) above.
  • the correction unit is At least a moving body velocity point group that is a velocity point group due to a moving body is subjected to the correction;
  • the signal processing device according to any one of (1) to (4) above.
  • the correction unit is The moving body velocity point group is a first correction target by the correction unit, and the stationary body velocity point group, which is a velocity point group due to a stationary body, is a second correction target by the correction unit.
  • the signal processing device according to (5) above.
  • a map generation unit that generates map information based on the stationary body velocity point group corrected by the correction unit and inertial measurement data received from the second sensor; further comprising, The signal processing device according to (6) above.
  • an attention area extraction unit that extracts an attention area;
  • the receiving section includes: further receiving image data from a third sensor;
  • the region of interest extraction unit includes: extracting the region of interest from the image data based on a region including the moving object estimated based on the moving object velocity point group corrected by the correction unit;
  • the signal processing device according to any one of (5) to (7) above.
  • the receiving section includes: further receiving type information indicating the type of detection operation by the first sensor from the first sensor; The signal processing device according to any one of (1) to (10) above.
  • the execution unit includes: a receiving unit that receives a speed point group including a plurality of points each having speed information and time information from the first sensor; a correction unit that corrects at least one attribute value regarding at least one point included in the speed point group based on an estimated value at a predetermined time; an interface unit that receives the request from the application unit; including; The interface section includes: in response to the request, passing the velocity point group corrected by the correction unit to the application unit; Information processing device. (14) The time information indicates a time at which each of the plurality of points was acquired by the first sensor. The information processing device according to (13) above.
  • the correction unit is Calculating the estimated value based on the speed information, the time information, and inertial measurement data received from the second sensor by the receiving unit; The information processing device according to any one of (13) to (15) above.
  • the correction unit is At least a moving body velocity point group that is a velocity point group due to a moving body is subjected to the correction; The information processing device according to any one of (13) to (16) above.
  • the correction unit is The moving body velocity point group is a first correction target by the correction unit, and the stationary body velocity point group, which is a velocity point group due to a stationary body, is a second correction target by the correction unit.
  • the information processing device according to (17) above.
  • an application section that executes predetermined processing; an interface unit that passes a request related to the predetermined processing to an execution unit that executes a predetermined function; Equipped with The application section includes: Regarding at least one point included in a speed point group that is passed from the execution unit in response to the request and that includes a plurality of points each having velocity information and time information that the execution unit received from the first sensor.
  • the application section includes: a stationary body velocity point group, which is a velocity point group of a stationary body corrected based on the estimated value, received via the interface unit, and inertial measurement data received by the execution unit from the second sensor; a map generation unit that generates map information based on the including, The information processing device according to (19) above.
  • the application section includes: The execution unit receives the area of interest for the image data received from the third sensor via the interface unit, and converts the area of interest to the image data received from the third sensor into a group of velocity points due to the moving object corrected based on the estimated value received via the interface unit. a region of interest extraction unit that extracts an area including the moving object, which is estimated based on a certain moving object speed point group; including, The information processing device according to (19) or (20) above.
  • the application section includes: a motion recognition unit that recognizes the motion of the moving body based on composite data obtained by synthesizing the moving body velocity point group and image data of the region of interest among the image data; including, The information processing device according to (21) above.
  • the application section includes: an image recognition unit that performs image recognition processing based on image data of the region of interest among the image data; including, The information processing device according to (21) or (22) above.

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本開示は、ＦＭＣＷ－ＬｉＤＡＲによる計測結果をより汎用的に利用可能とする信号処理装置を提供することを目的とする。　本開示に係る信号処理装置（２０）は、第１のセンサ（ＦＭＣＷ－ＬｉＤＡＲによる光検出測距センサ１１０）から、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群データを受信する受信部（２００）と、前記速度点群データに含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部（２１３，２１４）と、前記補正部により補正された前記属性値に対して前記所定の時刻を示す補正時刻情報を付加して送信する送信部（２０１）と、を備える。

Description

信号処理装置および信号処理方法、ならびに、情報処理装置
　本開示は、信号処理装置および信号処理方法、ならびに、情報処理装置に関する。
　光を用いて測距を行う方法の一つとして、ＦＭＣＷ－ＬｉＤＡＲ（Frequency　Modulated　Continuous　Wave-Laser　Imaging　Detection　and　Ranging)と呼ばれる技術が知られている。ＦＭＣＷ－ＬｉＤＡＲでは、時間の経過に応じてパルスの周波数を例えば直線的に変化させたチャープ光として射出するレーザ光と、射出されたレーザ光の反射光とを合成した受信信号に対し、コヒーレント検出により測距を行う。ＦＭＣＷ－ＬｉＤＡＲでは、ドップラー効果を利用することで、測距と同時に速度を計測することができる。
米国特許出願公開第２０２０／０２９２７０６号明細書 米国特許出願公開第２０２０／００４００８２号明細書 国際公開第２０１９／２３９５６６号
　ＦＭＣＷ－ＬｉＤＡＲでは、一般的には、順次走査するレーザ光の照射方向ごとに計測結果を取得するため、原理的に、計測結果の取得時刻が各計測点で異なってしまう。そのため、既存技術では、点群全体あるいはその一部について同一時刻における計測結果を得たい、という要求に応えることができず、計測結果の汎用化が困難であった。
　本開示は、ＦＭＣＷ－ＬｉＤＡＲによる計測結果をより汎用的に利用可能とする信号処理装置および信号処理方法、ならびに、情報処理装置を提供することを目的とする。
　本開示に係る信号処理装置は、第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群データを受信する受信部と、前記速度点群データに含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部と、前記補正部により補正された前記属性値に対して前記所定の時刻を示す補正時刻情報を付加して送信する送信部と、を備える。
実施形態に係る信号処理システムの構成例を概略的に示すブロック図である。 実施形態に係る信号処理システムの一例の構成をより詳細に示すブロック図である。 実施形態に係る光検出測距センサの一例の構成を示すブロック図である。 実施形態に係る、送信光信号の走査パターンの一例を概略的に示す模式図である。 送信光信号の走査パターンの別の例を概略的に示す模式図である。 送信光信号の走査パターンの別の例を概略的に示す模式図である。 実施形態に係る速度点群の補正処理を概略的に示す模式図である。 実施形態に適用可能な信号処理システムの一例のハードウェア構成を示すブロック図である。 実施形態に適用可能な、ＭＩＰＩ－ＣＳＩ－２に規定されるデータフォーマットの例を示す模式図である。 実施形態に適用可能な、ＭＩＰＩ－ＣＳＩ－２に規定されるデータフォーマットの別の例を示す模式図である。 実施形態に適用可能な、ＭＩＰＩ－ＣＳＩ－２に規定されるデータフォーマットで点群を送信する例を示す模式図である。 実施形態に適用可能な、各部間のデータ送信に係るインタフェースとしてイーサネットを適用した例を示す模式図である。 実施形態に係る信号処理部のアーキテクチャの例を示す図である。 実施形態に適用可能な、光検出測距センサにおける送信光信号検出処理を示す一例のフローチャートである。 実施形態に適用可能な、光送受信部により送信される送信光信号を説明するための模式図である。 実施形態に係る、受信光信号に対する判定処理を説明するための模式図である。 実施形態に係る、光検出測距センサがラスタ走査パターンにより走査を行った場合のデータ出力の例を示す模式図である。 実施形態に適用可能なフレーム定義について説明するための模式図である。 実施形態に係る、センサ部による各データの出力タイミングについて説明するための模式図である。 実施形態に係る、センサ部による各データの出力タイミングについて説明するための模式図である。 実施形態に係る、センサ部から出力されるＦＭＣＷ－ＬｉＤＡＲデータおよびＩＭＵデータのデータフォーマットの例を示す模式図である。 実施形態に係る光検出測距センサによる出力データの例を示す模式図である。 実施形態に適用可能な、射出ポイントの座標の定義を示す模式図である。 光検出測距センサから見た各射出ポイントおよび物体を示す模式図である。 実施形態に係る信号処理部における、点群補正に係る処理の全体的な流れを示す模式図である。 実施形態に係る信号処理部における、点群補正に係る処理を示す一例のフローチャートである。 実施形態に係る信号処理部における、静止体の速度点群フレームの補正処理を示す一例のフローチャートである。 座標系および各変数を定義するための模式図である。 実施の第１の変形例に係る信号処理システムの一例の構成を示すブロック図である。 実施の第２の変形例に係る信号処理システムの一例の構成を示すブロック図である。
　以下、本開示の実施形態について、図面に基づいて詳細に説明する。なお、以下の実施形態において、同一の部位には同一の符号を付することにより、重複する説明を省略する。
　以下、本開示の実施形態について、下記の順序に従って説明する。
１．本開示に係る実施形態の概要
２．本開示に係る実施形態のより具体的な説明
　２－１．実施形態に係る構成について
　　２－１－１．センサ部について
　　　２－１－１－１．光検出測距センサについて
　　２－１－２．信号処理部について
　　　２－１－２－１．ライブラリ部について
　　　２－１－２－２．アプリケーション部について
　　２－２－１．システム構成について
　２－２．実施形態に係る処理について
　　２－２－１．実施形態に適用可能な計測技術
　　２－２－２．実施形態に係るデータ構造例
　　２－２－３．実施形態に係る点群補正処理
３．本開示に係る実施形態の第１の変形例
４．本開示に係る実施形態の第２の変形例
５．本開示に係る他の実施形態
（１．本開示に係る実施形態の概要）
　先ず、本開示に係る実施形態について、概略的に説明する。
　図１は、実施形態に係る信号処理システムの構成例を概略的に示すブロック図である。図１において、信号処理システム１は、センサ部１０と、信号処理部２０と、情報処理部３０と、を含む。
　センサ部１０は、ＩＭＵ(Inertial　Measurement　Unit)１００と、光検出測距センサ１１０と、イメージセンサ１２０と、を含む。ＩＭＵ１００は、例えば３軸の加速度センサ、３軸の角速度センサおよび３軸の地磁気センサを含み、これらのセンサによる慣性計測データ（以下、適宜、ＩＭＵデータと呼ぶ）を出力する。光検出測距センサ１１０は、光を用いて測距を行うセンサであって、実施形態では、周波数連続変調したレーザ光を用いて測距を行うＦＭＣＷ－ＬｉＤＡＲ(Frequency　Modulated　Continuous　Wave-Laser　Imaging　Detection　and　Ranging)を適用する。イメージセンサ１２０は、所謂カメラであり、被写体を撮像して、例えばＲ（赤色）、Ｇ（緑色）およびＢ（青色）の情報を含む画像データを出力する。
　図１において、信号処理部２０は、センサ部１０のＩＭＵ１００から出力されたＩＭＵデータと、光検出測距センサ１１０から出力されたＦＭＣＷ－ＬｉＤＡＲデータと、イメージセンサ１２０から出力された画像データとが入力される。信号処理部２０は、入力されたこれらＩＭＵデータ、ＦＭＣＷ－ＬｉＤＡＲデータおよび画像データに基づき信号処理を行う。信号処理部２０は、各データに基づく信号処理により、例えば地図情報、静止体および動体それぞれの速度点群、動体の動作を示す動作メタ情報、注目領域における画像に関する画像メタ情報といった各情報を生成する。信号処理部２０は、生成した各情報を情報処理部３０に出力する。
　情報処理部３０は、信号処理部２０から出力された各情報に応じて、所定の処理を行う。情報処理部３０が信号処理部２０から出力された各情報に応じて行う処理は、特に限定されない。例えば、情報処理部３０は、この信号処理システム１が自律動作するロボット（移動体）に適用される場合、地図情報などに基づきロボットの駆動制御を行うものであってよい。また例えば、情報処理部３０は、この信号処理システム１が監視カメラの制御システムに適用される場合、動作メタ情報や画像メタ情報に基づく通知などを行うものであってよい。
　さらに、情報処理部３０は、信号処理部２０に対して信号処理に関する指示を送ってよい。信号処理部２０は、この指示に応じて、センサ部１０の出力に対する処理を制御してよい。また、信号処理部２０は、この指示に応じて、センサ部１０における各センサ（ＩＭＵ１００、光検出測距センサ１１０およびイメージセンサ１２０の動作を制御してよい。なお、情報処理部３０は、ＣＰＵ(Central　Processing　Unit)、メモリ、ストレージ装置などを含む、一般的なコンピュータとしての構成を適用してよい。
　このように、実施形態に係る信号処理システム１は、ＦＭＣＷ－ＬｉＤＡＲによる計測結果に基づき様々なデータを出力することができ、ＦＭＣＷ－ＬｉＤＡＲによる計測結果をより汎用的に利用可能とすることができる。
（２．本開示に係る実施形態のより具体的な説明）
　次に、本開示に係る実施形態について、より具体的に説明する。
（２－１．実施形態に係る構成について）
　先ず、実施形態に係る構成について、より具体的に説明する。図２は、実施形態に係る信号処理システム１の一例の構成をより詳細に示すブロック図である。
（２－１－１．センサ部について）
　図２において、センサ部１０は、ＩＭＵ１００と、ＦＭＣＷ－ＬｉＤＡＲによる光検出測距センサ１１０と、イメージセンサ１２０とを含むと共に、同期信号生成部１３０を含む。
　同期信号生成部１３０は、同期信号を生成する。同期信号は、所定周期のパルスであってよい。同期信号生成部１３０は、生成した同期信号を、ＩＭＵ１００、光検出測距センサ１１０およびイメージセンサ１２０に供給する。ＩＭＵ１００、光検出測距センサ１１０およびイメージセンサ１２０は、それぞれ、供給された同期信号に同期して、動作およびデータ出力タイミングが制御される。
　ＩＭＵ１００は、３軸の加速度センサ、３軸の角速度センサおよび３軸の地磁気センサを含む。ＩＭＵ１００は、これら３軸の加速度センサ、３軸の角速度センサおよび３軸の地磁気センサによるセンサデータを、ＩＭＵデータとして出力する。ＩＭＵ１００は、ＩＭＵデータを、ＩＭＵデータを取得した時刻を示すタイムスタンプを付加して出力してよい。
　イメージセンサ１２０は、所謂カメラであり、被写体を撮像して、例えばＲ（赤色）、Ｇ（緑色）およびＢ（青色）の情報を含む画像データを出力する。イメージセンサ１２０は、受光した光に応じた電気信号としての画素信号を出力する画素が行列状の配列で配置された画素アレイと、画素アレイの各画素を駆動する駆動回路とを含む。イメージセンサ１２０は、画素アレイの各画素からアナログ方式の信号として出力された各画素信号を、それぞれデジタル方式の画素データに変換して出力する。画素アレイの有効領域に含まれる各画素の出力に基づく画素データにより、１フレームの画像データが構成される。
　なお、図示は省略するが、センサ部１０は、ＩＭＵ１００、光検出測距センサ１１０およびイメージセンサ１２０のデータの入出力を制御するインタフェース部を含む。これに限らず、ＩＭＵ１００、光検出測距センサ１１０およびイメージセンサ１２０は、それぞれ別体としての個別のインタフェース部を備えていてもよい。
（２－１－１－１．光検出測距センサについて）
　光検出測距センサ１１０は、光を用いて測距を行うセンサであり、実施形態では、ＦＭＣＷ－ＬｉＤＡＲを適用する。ＦＭＣＷ－ＬｉＤＡＲは、射出するレーザ光として、時間の経過に応じてパルスの周波数を例えば直線的に変化させたチャープ光を用いる。ＦＭＣＷ－ＬｉＤＡＲでは、チャープ光として射出するレーザ光の一部、若しくは、それと同期した局発光と、射出されたレーザ光の反射光とを合成した受信信号に対し、コヒーレント検出により測距を行う。
　ＦＭＣＷ－ＬｉＤＡＲでは、ドップラー効果を利用することで、測距と同時に速度（ドップラー速度）を計測することができる。そのため、ＦＭＣＷ－ＬｉＤＡＲを用いることで、人やその他の移動物体などの、速度を持った物体の位置を迅速に把握することが容易となる。また、コヒーレント検出は、他の光源からの干渉を受けにくいため、混信を避けることが可能であり、さらには、太陽光などの高照度の光源によるノイズの影響が少ない。また、ＦＭＣＷ－ＬｉＤＡＲは、アクティブ型であるため、暗闇などの低照度環境でも計測を行うことが可能である。
　また、ＦＭＣＷ－ＬｉＤＡＲでは、一般的に、レーザ光の射出方向を例えば水平方向に所定角度範囲で走査させ、さらに垂直方向に所定角度範囲で走査させ、スキャンの所定角度ごとにレーザ光を射出させて計測を行う。そのため、ＦＭＣＷ－ＬｉＤＡＲによる計測結果は、走査角度ごとの点の情報として取得される。計測された各点の情報は、属性情報（属性値）として、測距結果による距離と、ドップラー速度と、受信した反射光の強度とを含んでよい。当該属性情報は、受信装置の構成によっては、反射光の直交偏光成分ごとの強度を含むことができる。また、３次元若しくは２次元の空間座標を持つ点の集合を点群と呼び、各点が速度情報（ドップラー速度）を含む点群を速度点群と呼ぶ。
　光検出測距センサ１１０は、これら距離、ドップラー速度および強度を示す情報を、これらの情報が取得された時刻を示すタイムスタンプと、レーザ光の射出方向を示す情報（水平走査角度、垂直走査角度）と、に関連付けて、ＦＭＣＷ－ＬｉＤＡＲデータとして出力する。すなわち、ＦＭＣＷ－ＬｉＤＡＲデータは、速度点群を含む。
　図３は、実施形態に係る光検出測距センサ１１０の一例の構成を示すブロック図である。図３において、光検出測距センサ１１０は、光走査部１１１と、光送受信部１１２と、受信信号処理部１１３と、光走査制御部１１４と、送信部１１５と、を含む。
　光送受信部１１２は、後述する光走査部１１１から送信する送信光信号を生成する光送信部と、光送信部により生成される送信光を制御するための送信光制御部と、光走査部１１１からの受信光信号を受信（受光）する光受信部と、を含む。
　光送受信部１１２において光送信部は、例えば、送信光であるレーザ光を発光するためのレーザダイオードなどの光源と、光源で発光された光を射出するための光学系と、当該光源を駆動するレーザ出力変調装置とを含む。光送信部は、送信光制御部から供給された光送信制御信号に応じて光源を発光させ、時間の経過に応じて所定周波数範囲内で周波数が直線的に変化するチャープ光による送信光信号を射出する。送信光信号は、光走査部１１１に送られると共に、局発光として光受信部に送られる。
　光送信部において、送信光制御部は、時間の経過に応じて、周波数が所定周波数範囲内で直線的に変化（増加および減少）する信号を生成する。このような、時間の経過に応じて所定周波数範囲内で周波数が直線的に変化する信号を、チャープ信号と呼ぶ。送信光制御部は、このチャープ信号に基づき、光送信部が含むレーザ出力変調装置に入力される変調同期タイミング信号である、光送信制御信号を生成する。送信光制御部は、光送信制御信号を、同期信号生成部１３０から供給された同期信号に同期した信号として生成する。送信光制御部は、生成した光送信制御信号を、光送信部と後述する受信信号処理部１１３とに渡す。
　光送受信部１１２において光受信部は、例えば、光走査部１１１からの受信光信号を受信（受光）する受光部と、当該受光部を駆動する駆動回路とを含む。受光部は、例えば、集光レンズとフォトダイオードなどの受光素子とを組み合わせた構成を適用することができる。光受信部は、走査部から受信した受信光と、光送信部から送られた局発光とを合成する光合成部をさらに含む。受信光が送信光の対象物からの反射光であれば、受信光は、局発光に対して対象物との距離に応じて遅延した信号となり、受信光と局発光とを合成した合成信号は、一定周波数の信号（ビート信号）となる。光送受信部１１２は、この信号を受信波形信号として受信信号処理部１１３に渡す。
　受信信号処理部１１３は、光送受信部１１２から渡された受信信号に対して、同期信号生成部１３０から供給された同期信号に同期して、例えば高速フーリエ変換といった所定の信号処理を行う。受信信号処理部１１３は、この信号処理により、対象物までの距離と、対象物のドップラー速度と、受信光信号の強度と、を取得する。受信信号処理部１１３は、これら取得した距離、速度およびドップラー速度を、同期信号に同期して生成されるタイムスタンプを付加して、送信部１１５に渡す。ここでのタイムスタンプは、光送受信部１１２により送信光信号が送信されたタイミングを示す、計測に係るタイムスタンプであって、計測ごとに生成、付加される。
　光走査制御部１１４は、光走査部１１１における送信光信号の走査を制御するための走査制御信号を生成する。このとき、光走査制御部１１４は、送信光信号の走査が同期信号生成部１３０から供給された同期信号に同期するように、走査制御信号を生成する。光走査制御部１１４は、予め定められた走査範囲に対する走査を行う走査制御信号を生成してもよいし、後述する制御通信部２３０から送信された走査制御情報に従い、走査制御信号を生成してもよい。
　また、光走査制御部１１４は、光走査部１１１から、送信光信号の走査角を示す角度検出信号を受信する。光走査制御部１１４は、受信した角度検出信号に基づき、走査角を示す情報を送信部１１５に渡す。
　光走査部１１１は、光送受信部１１２から送られる送信光信号を、光走査制御部１１４から供給される走査制御信号に応じた走査パターンに従った角度で送信すると共に、当該角度から入射される光を受信して受信光信号として出力する。光走査部１１１において、送信光信号の走査機構として、例えば２軸ミラースキャン装置を適用することができる。この場合、走査制御信号は、例えば、当該２軸ミラースキャン装置の各軸に印加される駆動電圧信号となる。
　また、光走査部１１１は、送信光信号を送信する水平方向および垂直方向の角度を検出し、検出された角度を示す角度検出信号を出力する。
　送信部１１５は、受信信号処理部１１３から渡されたタイムスタンプ、距離、ドップラー速度および強度と、光走査制御部１１４から渡された走査角とを、ＦＭＣＷ－ＬｉＤＡＲデータとして、信号処理部２０に送信する。すなわち、ＦＭＣＷ－ＬｉＤＡＲデータは、光検出測距センサ１１０における計測点ごとにタイムスタンプを含む。
　図４は、実施形態に係る、送信光信号の走査パターンの一例を概略的に示す模式図である。図４は、走査パターンのうち、ラスタ走査パターンの例を示している。光走査部１１１は、所定の角度範囲４５内において、角度範囲４５の水平方向の両端で折り返す走査線４０に従い走査を行う。走査線４０は、角度範囲４５の左端と右端との間を走査した１本の軌跡に対応する。光走査部１１１は、走査制御信号に応じて、走査線４０に従い角度範囲４５の上端と下端との間を走査する。
　光走査部１１１は、走査制御信号に従い、送信光信号としてのチャープ光の射出ポイント４１を、例えば一定の時間間隔（ポイントレート）で走査線４０に沿って順次、離散的に変化させる。角度範囲４５内の射出ポイント４１により、ＦＭＣＷ－ＬｉＤＡＲにおける１フレームが構成される。角度範囲４５の上端から下端までの走査の時間が１フレーム時間となる。また、各射出ポイント４１における送信光信号は、走査線４０に従い１フレーム内において所定の時間間隔で順次に射出される。そのため、各射出ポイント４１における計測データは、走査の順序に従い射出ポイント４１ごとに異なる時刻に取得される。
　なお、走査線４０の角度範囲４５の左端および右端の折り返し点付近では、２軸ミラースキャン装置による走査速度が遅くなる。そのため、各射出ポイント４１は、角度範囲４５において格子状に並ばないことになる。
　各射出ポイント４１のうち、例えば物体５０ａ、５０ｂが存在する位置に相当する射出ポイント４１ａおよび４１ｂ（塗り潰して示す）は、射出された送信光信号が物体５０ａおよび５０ｂで反射され、その反射光が、受信光信号として返される。一方、各射出ポイント４１のうち、物体５０ａおよび５０ｂが存在しない位置に相当する射出ポイント４１では、反射光が得られず、受信光信号も得られない。
　なお、光送受信部１１２は、１つの射出ポイント４１に対して、１または複数回、送信光信号を射出してよい。また、光送受信部１１２は、各射出ポイント４１において、周波数が時系列で連続変化するチャープ光による送信光信号を射出すると共に、光走査部１１１では、走査角度を連続して変化させる。そのため、送信光信号は、例えば走査線４０に沿った楕円状の形状で対象物に照射される。
　送信光信号の走査パターンは、上述のラスタ走査パターンに限られない。図５Ａおよび図５Ｂは、送信光信号の走査パターンの別の例を概略的に示す模式図である。
　図５Ａは、走査パターンのうち、マルチレイヤ型の走査パターンの例を示している。マルチレイヤ型では、複数のビームを３６０°回転させて、複数のラインの走査を同時に行う。より具体的には、マルチレイヤ型の走査では、送信光信号の射出ポイント４１を、複数のビームそれぞれによる走査線４２1、４２2、…、４２Nに沿って、それぞれ一定の時間間隔で順次、離散的に変化させる。マルチレイヤ型の走査では、走査線４２1、４２2、…、４２Nの折り返しが発生しないため、各射出ポイント４１を格子状に並べることが容易である。
　図５Ｂは、走査パターンのうち、ドット走査型の走査パターンの例を示している。ドット走査型では、ビーム走査軌跡は、各射出ポイント４１で固定されるため、連続的ではなく離散的である。各射出ポイント４１で、送信光信号を射出し、反射光信号の受信が完了した後、次の射出ポイント４１にビームを切り替える。図５Ｂにおいて、セクション（ａ）は、図中に矢印４７で示されるように、規定のシーケンスに従い、順次、ビーム切り替えが実行される例を示している。また、セクション（ｂ）は、図中に矢印４８で示されるように、例えば信号処理部２０からの走査制御設定によって、任意の順番でビーム切替が実行される例を示している。セクション（ｂ）の例において、走査制御設定により、走査領域全体のうち、所定の領域のみを走査してもよく、また、所定の領域は、１フレーム内に複数設定してもよい。さらに、所定の領域は、検出された動体や静止体の位置に基づいて、決定されてもよい。
（２－１－２．信号処理部について）
　説明は図２に戻り、信号処理部２０に含まれる各構成は、個別の機能を提供する部分（ライブラリ部と呼ぶ）と、当該部分により提供される機能を利用して目的の処理を実行する部分（アプリケーション部と呼ぶ）と、に大別することができる。
　図２において、ライブラリ部は、例えば受信部２００と、センサ位置姿勢推定部２１０と、動体・静止体分離部２１１と、センサ速度推定部２１２と、静止体点群補正部２１３と、動体点群補正部２１４と、を含む。また、アプリケーション部は、例えば地図変換部２２０と、動体状態推定部２２１と、３Ｄ／２Ｄ変換部２２２と、ＲＯＩ(Region　of　Interest)抽出部２２３と、合成部２２４と、動作認識部２２５と、画像認識部２２６と、を含む。
　なお、信号処理部２０において、ライブラリ部およびアプリケーション部に含まれる構成は、上述の例に限定されない。例えば、動体状態推定部２２１、あるいは、動体状態推定部２２１および３Ｄ／２Ｄ変換部２２２をライブラリ部に含めてもよい。また、送信部２０１は、ライブラリ部およびアプリケーション部の何れかに含めてもよいし、何れにも含まれないものとしてもよい。受信部２００についても、アプリケーション部に含めてもよいし、ライブラリ部およびアプリケーション部の何れかに含めてもよいし、何れにも含まれないものとしてもよい。
（２－１－２－１．ライブラリ部について）
　先ず、信号処理部２０のライブラリ部について、説明する。
　ライブラリ部において、受信部２００は、センサ部１０から出力された各データを受信し、受信したデータを信号処理部２０の各部に渡す。より具体的には、受信部２００は、センサ部１０のＩＭＵ１００から出力されたＩＭＵデータを、センサ位置姿勢推定部２１０に渡す。受信部２００は、また、センサ部１０の光検出測距センサ１１０から出力されたＦＭＣＷ－ＬｉＤＡＲデータを、動体・静止体分離部２１１に渡す。さらに、受信部２００は、センサ部１０のイメージセンサ１２０から出力された画像データを、ＲＯＩ抽出部２２３に渡す。
　このように、受信部２００は、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群データを受信する受信部として機能する。
　センサ位置姿勢推定部２１０は、受信部２００から渡されたＩＭＵデータに基づき、ＩＭＵ１００の位置、姿勢および角速度を推定する。例えば、センサ位置姿勢推定部２１０は、ＩＭＵデータに含まれる、３軸の加速度センサ、３軸の角速度センサおよび３軸の地磁気センサによる各センサデータを用いて、例えばカルマンフィルタなどのセンサフュージョン処理を行うことで、現在のＩＭＵ１００の位置、姿勢および角速度を推定する。センサ位置姿勢推定部２１０は、予め、ＩＭＵ１００の基準座標系に対する光検出測距センサ１１０の基準座標系の位置関係、各センサの内部パラメータといったキャリブレーションデータを取得しておき、推定時に利用する。センサ位置姿勢推定部２１０は、推測したセンサ位置、姿勢および角速度のデータを、センサ速度推定部２１２と、地図変換部２２０と、送信部２０１と、に渡す。
　動体・静止体分離部２１１は、受信部２００からＦＭＣＷ－ＬｉＤＡＲデータが渡されると共に、後述するセンサ速度推定部２１２により推定された光検出測距センサ１１０の速度を示すセンサ速度が渡される。動体・静止体分離部２１１は、渡されたＦＭＣＷ－ＬｉＤＡＲデータに含まれるドップラー速度と、センサ速度とに基づく速度弁別により、ＦＭＣＷ－ＬｉＤＡＲデータが示す速度点群を、動体による速度点群と、静止体による速度点群とに分離する。動体・静止体分離部２１１は、動体速度点群と静止体速度点群とを、１フレーム分の走査に応じたＦＭＣＷ－ＬｉＤＡＲデータから抽出してよい。
　例えば、動体・静止体分離部２１１は、ＦＭＣＷ－ＬｉＤＡＲデータに含まれる各計測点のドップラー速度からセンサ速度の光軸方向成分を差し引いて速度点群の速度情報からセンサ部１０の動きによる影響を排除し、静止体の座標系から見た、各計測点の補正ドップラー速度を求める。すなわち、補正ドップラー速度は、ＦＭＣＷ－ＬｉＤＡＲデータに含まれるドップラー速度からセンサ部１０の動きによる影響を排除したものである。動体・静止体分離部２１１は、この補正ドップラー速度の大きさに対して閾値判定を行い、当該大きさが閾値以上の計測点のうち、一定の空間範囲（対象物体の大きさに相当）に局在する計測点による速度点群（局在速度点群と呼ぶ）を、動体速度点群として抽出する。動体・静止体分離部２１１は、１フレームのＦＭＣＷ－ＬｉＤＡＲデータから複数の動体速度点群を抽出してよい。
　また、動体・静止体分離部２１１は、ＦＭＣＷ－ＬｉＤＡＲデータによる計測点について、補正ドップラー速度の大きさに対して閾値判定を行う。動体・静止体分離部２１１は、この閾値判定の結果、補正ドップラー速度の大きさが閾値以下の計測点の集合を、静止体速度点群として抽出する。
　ここで、動体・静止体分離部２１１は、この閾値判定において、閾値ｖtha、閾値ｖthb（＜ｖtha）の２種類の閾値を設けてよい。動体・静止体分離部２１１は、補正ドップラー速度の大きさが閾値ｖtha以上の計測点を動体速度点群、閾値ｖthb以下の計測点を静止体速度点群として抽出してよい。これは、動体速度点群および静止体速度点群の何れにも属さない中間の点群が存在することを意味している。明確に静止している計測点を抽出する目的では、このように、２種類の閾値を設けることが妥当である。
　なお、１フレームのＦＭＣＷ－ＬｉＤＡＲデータから求めた速度点群により、点群フレームが構成される。
　動体・静止体分離部２１１は、抽出した静止体速度点群を、センサ速度推定部２１２および静止体点群補正部２１３に渡す。また、動体・静止体分離部２１１は、抽出した動体速度点群を、動体点群補正部２１４に渡す。
　センサ速度推定部２１２は、動体・静止体分離部２１１から渡された静止体点群と、センサ位置姿勢推定部２１０から渡されたセンサ位置、姿勢および角速度と、に基づき、センサ部１０の速度を示すセンサ速度を推定する。センサ速度推定部２１２によるセンサ速度の推定処理の具体例については、後述する。
　センサ速度推定部２１２は、推定したセンサ速度を、静止体点群補正部２１３および動体・静止体分離部２１１に渡すと共に、動体・静止体分離部２１１に入力する。また、センサ速度推定部２１２は、推定したセンサ速度を、送信部２０１に渡す。
　静止体点群補正部２１３は、センサ位置姿勢推定部２１０から渡されたセンサ位置、姿勢および角速度と、センサ速度推定部２１２から渡されたセンサ速度とを用いて、静止体速度点群の補正を行う。動体点群補正部２１４は、センサ位置姿勢推定部２１０から渡されたセンサ位置、姿勢および角速度と、センサ速度推定部２１２から渡されたセンサ速度と、後述する動体状態推定部２２１から渡された動体の状態を示す情報とを用いて、動体速度点群の補正を行う。
　上述したように、ＦＭＣＷ－ＬｉＤＡＲにおいては、フレーム内の各計測点における計測データの取得時刻が、走査パターンの種別に依存して、計測点のフレーム内の位置に応じて異なる。静止体点群補正部２１３および動体点群補正部２１４は、フレーム内の速度点群の各点が同一の所定時刻に取得された場合の各点のデータを推測し、推測結果に応じて速度点群を補正する。
　図６は、実施形態に係る速度点群の補正処理を概略的に示す模式図である。図６において、セクション（ａ）は、光検出測距センサ１１０から出力されるＦＭＣＷ－ＬｉＤＡＲデータの出力タイミングの例を示している。この図に示すように、ＦＭＣＷ－ＬｉＤＡＲデータは、フレーム開始時刻ｔfst1から、次のフレーム開始時刻ｔfst2まで、一定の時間間隔で出力される。ＦＭＣＷ－ＬｉＤＡＲデータに基づく速度点群の各点は、各データの出力タイミングに応じた時間情報を持っている。
　なお、図において、物体５０ａ、５０ｂ（図４参照）などが存在する位置に相当する射出ポイント４１に対応するデータを、実線（斜線付き）にて示している。また、物体５０ａ、５０ｂなどが存在しない位置に相当する射出ポイント４１に対応するデータを点線にて示している。点線にて示されるデータは、実際には、受信光信号からは、物体としては何も検出されないことを示している。
　図６において、セクション（ｂ）は、光検出測距センサ１１０から出力されたＦＭＣＷ－ＬｉＤＡＲデータを、静止体点群補正部２１３および動体点群補正部２１４により補正した状態を概略的に示している。静止体点群補正部２１３および動体点群補正部２１４は、例えば、センサ位置、姿勢および角速度と、センサ速度とに基づき、フレーム開始時刻ｔfst1からフレーム開始時刻ｔfst2までの各ＦＭＣＷ－ＬｉＤＡＲデータの補正の基準となる時刻、例えばフレーム開始時刻ｔfst2における値を推測する。静止体点群補正部２１３および動体点群補正部２１４は、それぞれ、この推測結果に基づき静止体速度点群および動体速度点群を補正する。
　より具体的には、静止体点群補正部２１３および動体点群補正部２１４は、静止体速度点群および動体速度点群に含まれる各点の時間情報を、基準時刻に補正する。これにより、フレーム時間内の自己運動や動体運動に起因する点群の歪みが解消され、実施形態では、ＦＭＣＷ－ＬｉＤＡＲによる計測結果をより汎用的に利用可能とすることができる。
　静止体点群補正部２１３および動体点群補正部２１４は、それぞれ補正された静止体速度点群および動体速度点群を出力する。このとき、静止体点群補正部２１３および動体点群補正部２１４は、補正された静止体速度点群および動体速度点群に対して、補正の基準となる時刻（図６の例ではフレーム開始時刻ｔfst2）を示すタイムスタンプを付加して出力する。ここでのタイムスタンプは、フレームごとの値となる。
　このように、静止体点群補正部２１３および動体点群補正部２１４は、速度点群データに含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部として機能する。
　静止体点群補正部２１３および動体点群補正部２１４は、補正しタイムスタンプを付加した静止体速度点群および動体速度点群を、それぞれ送信部２０１に渡される。
　静止体点群補正部２１３および動体点群補正部２１４による、センサ位置、姿勢および角速度と、センサ速度とに基づく補正処理の具体例については、後述する。
　実施形態では、この補正処理により、１フレーム内の静止体速度点群および動体速度点群に含まれる各点を、同一の時刻に取得された点として扱うことができ、後段における静止体速度点群および動体速度点群に対する処理の負荷を軽減することが可能となる。
　また、実施形態では、センサ位置、姿勢および角速度と、センサ速度とに基づき推測した値に基づき速度点群の補正を行っている。そのため、実施形態では、各点が同一時刻の時間情報を持った速度点群を、１フレーム分のＦＭＣＷ－ＬｉＤＡＲデータにより取得することができる。
　なお、各点が同一時刻の時間情報を持った速度点群を取得する方法は、上述の方法に限定されない。例えば、複数フレームのＦＭＣＷ－ＬｉＤＡＲデータを用いて、複数フレーム間で射出ポイント４１が対応するデータに基づく線形補間により、各点が同一時刻の時間情報を持った速度点群を取得することも可能である。
（２－１－２－２．アプリケーション部について）
　次に、信号処理部２０のアプリケーション部について、説明する。
　図２において、静止体点群補正部２１３は、補正した静止体速度点群を、送信部２０１と、地図変換部２２０とに渡す。
　地図変換部２２０は、センサ位置姿勢推定部２１０から渡されたセンサ位置、姿勢および角速度と、静止体点群補正部２１３から渡された静止体速度点群とに基づき、地図情報を推測、生成する。地図情報は、センサ部１０の自己位置と、センサ部１０の周囲環境の地図とを含む。地図変換部２２０は、地図作成に、ＳＬＡＭ(Simultaneous　Localization　and　Mapping)の技術を適用してよい。地図変換部２２０は、生成した地図情報を送信部２０１に渡す。
　動体点群補正部２１４は、補正した動体速度点群を、送信部２０１に渡すと共に、動体状態推定部２２１および合成部２２４に渡す。
　動体状態推定部２２１は、例えば、補正された動体速度点群に基づき、１フレームに含まれる各動体を分離する。動体状態推定部２２１は、分離された各動体について、対応する動体速度点群に基づき、その動体の位置、姿勢、速度および角度を含む動体の状態を推定する。動体状態推定部２２１は、推定した動体の状態を示す動体状態情報を、送信部２０１に渡すと共に、動体点群補正部２１４および３Ｄ／２Ｄ変換部２２２に渡す。
　３Ｄ／２Ｄ変換部２２２は、３Ｄ(Three-Dimensional)情報である動体速度点群を、イメージセンサ１２０から出力される画像データに対応する２Ｄ(Two-Dimensional)データに変換する。より具体的には、３Ｄ／２Ｄ変換部２２２は、動体速度点群の３Ｄ座標を、イメージセンサ１２０による画像データの座標系による２Ｄ座標に変換する。３Ｄ／２Ｄ変換部２２２は、予め、イメージセンサ１２０の基準座標系に対する光検出測距センサ１１０の基準座標系の位置関係、各センサの内部パラメータといったキャリブレーションデータを取得しておき、変換時に利用する。これにより、動体速度点群に示される各動体の、画像データにおける位置を知ることができる。３Ｄ／２Ｄ変換部２２２は、動体速度点群の座標が２Ｄ座標に変換された点群をＲＯＩ抽出部２２３に渡す。
　ＲＯＩ抽出部２２３は、３Ｄ／２Ｄ変換部２２２から渡された２Ｄ座標による点群に基づき、イメージセンサ１２０から受信部２００を介して渡された画像データから、注目領域（ＲＯＩ）を抽出する。より具体的には、ＲＯＩ抽出部２２３は、例えば、画像データにおいて、３Ｄ／２Ｄ変換部２２２から渡された２Ｄ座標による点群に対応する領域を、注目領域として抽出してよい。なお、ＲＯＩ抽出部２２３は、光検出測距センサ１１０とイメージセンサ１２０との位置関係が既知であれば、画像データと２Ｄ座標による点群との対応関係を、瞬時的に計算することが可能である。
　ＲＯＩ抽出部２２３は、抽出した注目領域の画像データを、送信部２０１に渡すと共に、合成部２２４および画像認識部２２６に渡す。
　合成部２２４は、動体点群補正部２１４から渡された動体速度点群と、ＲＯＩ抽出部２２３から渡された注目領域の画像データとを合成する。合成部２２４は、例えば、注目領域の画像データに対して動体速度点群を合成してよい。この場合、合成部２２４は、注目領域の画像データにおける各画素データに対して、動体速度点群に含まれる各点の速度情報を対応付けてよい。この、注目領域の画像データの各画素に対して速度情報を合成した画像を、合成画像と呼ぶ。
　これに限らず、合成部２２４は、動体速度点群に対して注目領域の画像データを合成してよい。この場合、合成部２２４は、動体速度点群の各点に対して、注目領域において対応する画素データの情報を対応付けてよい。この、動体速度点群の各点に対して注目領域の各画素データを合成した点群を、合成点群と呼ぶ。
　合成部２２４は、合成画像または合成点群を、送信部２０１に渡すと共に、動作認識部２２５に渡す。
　動作認識部２２５は、合成部２２４から渡された合成画像または合成点群に基づき、動体の動作を認識する。動作認識部２２５は、合成画像または合成点群に含まれる、各点の速度情報に基づき速度の分布情報を取得することができる。動作認識部２２５は、この速度の分布情報を用いることで、動体の動きをより精密に推定することが可能である。例えば、動作認識部２２５は、既知の速度の分布情報に基づき機械学習により学習済みの学習モデルを用いて、動作認識を行ってよい。動作認識部２２５は、動体の動作を認識して、動作に関するメタ情報（歩いている、走っている、など）を出力する。動作認識部２２５から出力された動作メタ情報は、送信部２０１に渡される。
　画像認識部２２６は、ＲＯＩ抽出部２２３から渡された注目領域の画像データに基づき画像認識を行い、画像メタ情報（人である、○○氏である、車両である、…など）を出力する。例えば、画像認識部２２６は、既知の画像による画像データに基づき機械学習により学習済みの学習モデルを用いて、画像認識を行ってよい。画像認識部２２６から出力された画像メタ情報は、送信部２０１に渡される。
　送信部２０１は、センサ位置姿勢推定部２１０から渡されたセンサ位置、姿勢および角速度と、センサ速度推定部２１２から渡されたセンサ速度とを、情報処理部３０に送信する。また、送信部２０１は、地図変換部２２０から渡された地図情報と、静止体点群補正部２１３から渡された、補正された静止体速度点群とを、情報処理部３０に送信する。さらに、送信部２０１は、動体点群補正部２１４から渡された動体速度点群と、動体状態推定部２２１から渡された動体状態情報とを、情報処理部３０に送信する。さらにまた、送信部２０１は、合成部２２４から渡された合成画像または合成点群と、ＲＯＩ抽出部２２３から渡されたＲＯＩ画像と、動作認識部２２５から渡された動作メタ情報と、画像認識部２２６から渡された画像メタ情報とを、情報処理部３０に送信する。
　送信部２０１は、上述した各情報、点群に対して、タイムスタンプを付加して情報処理部３０に送信する。ここで、送信部２０１が各情報、点群に付加するタイムスタンプは、光検出測距センサ１１０の走査の１フレームを単位とする時刻情報であってよい。具体的な例として、上述した図６を参照し、光検出測距センサ１１０の走査のフレーム開始時刻ｔfst1から次のフレーム開始時刻ｔfst2の期間に取得された各情報、点群に対して、速度点群の補正の基準となるフレーム開始時刻ｔfst2を示す情報を、タイムスタンプとして付加してよい。
　このように、送信部２０１は、補正部により補正された属性値に対して所定の時刻を示す補正時刻情報を付加して送信する送信部として機能する。
　送信部２０１は、上述した各情報、点群のうち、情報処理部３０からの要求に応じた情報や点群を選択的に情報処理部３０に送信してよい。また、送信部２０１は、上述した各情報、点群を、情報処理部３０とは異なる送信先に送信してもよい。
（２－２－１．システム構成について）
　次に、実施形態に適用可能なシステム構成について説明する。
　図７は、実施形態に適用可能な信号処理システム１の一例のハードウェア構成を示すブロック図である。
　図７において、センサ部１０は、センサ群１０００と、ファームウェア１０１０とを含む。センサ群１０００は、ＩＭＵ１００と、光検出測距センサ１１０と、イメージセンサ１２０とを含む。
　ファームウェア１０１０は、センサ群１０００の動作を制御するためのプログラムであってよい。ファームウェア１０１０は、例えば、センサ群１０００に含まれる各センサのデータの入出力を制御するハードウェアとしてのインタフェース（図示しない）や、同期信号生成部１３０の動作を制御してよい。例えば、当該インタフェースは、ファームウェア１０１０が予め記憶されるメモリと、メモリに記憶されたファームウェア１０１０に従い動作するプロセッサと、を含んでよい。
　信号処理部２０は、プロセッサ２０００を含み、プロセッサ２０００が図示されないメモリに記憶されるプログラムに従い動作することで、上述したライブラリ部２０１０およびアプリケーション部２０２０が構成される。プロセッサ２０００は、例えばＩＳＰ(Image　Signal　Processor)であってよい。これに限らず、プロセッサ２０００は、ＤＳＰ(Digital　Signal　Processor)やＣＰＵ(Central　Processing　Unit)であってもよい。
　また、この構成に限らず、上述した、ライブラリ部２０１０およびアプリケーション部２０２０に含まれる各部の一部または全部を、互いに協働して動作するハードウェア回路により構成することも可能である。
　情報処理部３０は、信号処理部２０と同様に、例えばＩＳＰによるプロセッサ３０００を含み、プロセッサ３０００が図示されないメモリに記憶されるプログラムに従い動作することで、ライブラリ部３０１０およびアプリケーション部３０２０が構成される。ライブラリ部３０１０は、情報処理部３０における個別の機能を提供するプログラムの集合であり、アプリケーション部３０２０は、ライブラリ部３０１０により提供される機能を利用して目的の処理を実行するプログラムの集合である。
　一例として、情報処理部３０が自律動作するロボット（移動体）の制御に適用される場合、ライブラリ部３０１０は、ロボットを駆動するための制御コマンドを出力する機能を含んでよい。また、アプリケーション部３０２０は、信号処理部２０から渡された地図情報、動作メタ情報、画像メタ情報などに基づきロボットの動作を決定する機能を含んでよい。別の例として、情報処理部３０が監視カメラの制御システムに適用される場合、ライブラリ部３０１０は、監視カメラの動作を制御するための制御コマンドを出力する機能を含んでよい。また、アプリケーション部３０２０は、監視カメラからの画像、信号処理部２０から渡された動作メタ情報、画像メタ情報などに基づく判断などを行う機能を含んでよい。
　この構成に限らず、ライブラリ部３０１０およびアプリケーション部３０２０に含まれる各部の一部または全部を、互いに協働して動作するハードウェア回路により構成することも可能である。また、情報処理部３０は、一般的なコンピュータを適用してもよい。
　図７の例では、センサ群１０００に含まれる各センサの出力（画像データ、ＦＭＣＷ－ＬｉＤＡＲデータ、ＩＭＵデータ）は、ＲＡＷデータとして、ファームウェア１０１０の制御に従い所定のフォーマットで出力され、図示されないインタフェースにより信号処理部２０に送信される。信号処理部２０は、センサ部１０から送信されたＲＡＷデータを受信部２００により受信して、ライブラリ部２０２１に渡す。
　ライブラリ部２０１０は、受信部２００から渡されたＲＡＷデータに対して上述したような処理を施してアプリケーション部２０２０に渡される。アプリケーション部２０２０は、ライブラリ部２０１０から渡されたデータに対して上述したような処理を施し、送信部２０１により、この処理により生成された各情報、点群にフレーム単位のタイムスタンプを付加して出力する。図７の例では、信号処理部２０の出力は、ＳＭＡＲＴデータとして情報処理部３０に送信される。
　情報処理部３０は、信号処理部２０から送信されたＳＭＡＲＴデータを受信して、ライブラリ部３０１０に渡す。
　ライブラリ部３０１０は、渡されたＳＭＡＲＴデータに対して個別の機能に係る処理を施して、アプリケーション部３０２０に渡す。アプリケーション部３０２０は、ライブラリ部３０１０から渡されたデータに対して所定の処理を施し、出力データを生成する。アプリケーション部３０２０により生成されたデータは、例えば、情報処理部３０から出力されてよい。
　情報処理部３０は、例えばアプリケーション部３０２０により、信号処理部２０に対する要求を生成し、生成した要求を信号処理部２０に送信してよい。例えば、アプリケーション部３０２０は、信号処理部２０に対して、信号処理部２０が送信部２０１により送信可能な各情報、点群から必要な情報、点群を指定する要求を生成、送信してよい。
　同様に、信号処理部２０は、例えばアプリケーション部２０２０により、センサ部１０に対する要求を生成し、生成した要求をセンサ部１０に対して送信してよい。例えば、アプリケーション部２０２０は、光検出測距センサ１１０に対して、走査範囲を注目領域に限定する要求を生成、送信してよい。
　図７の例では、センサ部１０から信号処理部２０へのデータ送信、信号処理部２０から情報処理部３０へのデータ送信のインタフェースとして、ＭＩＰＩ(Mobile　Industry　Processor　Interface)（登録商標）が適用されている。より詳細には、当該インタフェースとして、ＭＩＰＩ－ＣＳＩ(Camera　Serial　Interface)－２が適用されている。
　図８は、実施形態に適用可能な、ＭＩＰＩ－ＣＳＩ－２に規定されるデータフォーマットの例を示す模式図である。図８は、１フレームの画像データ(image　data)を送信する例を示している。ここでは、センサ部１０から信号処理部２０へのデータ送信の場合について説明する。
　図８において、図上の左上がデータ送信の開始位置であり、データは、図上で左から右に向けた順で送信され、さらに、図上で上から下に向けた順で送信される。左上の開始位置では、フレームの先頭を示すフィールドＦＳ(Frame　Start)が送信される。フレームの最後では、フィールドＦＥ(Frame　End)が送信される。
　左端のデータからデータ送信が開始され、順次、その行のデータ送信が右端に向けて進む。データ送信が右端に達すると、図上で１行下の行において、再び左端のデータから右端に向けて、順次にデータ送信が開始される。図において空白部分は、送信するデータが無いことを示している。
　フィールドＰＨは、パケットヘッダ(Packet　Header)が送信されるフィールドであり、フィールドＰＦは、パケットフッタ(Packet　Footer)が送信されるフィールドである。フィールドＩｍａｇｅ　Ｄａｔａは、画像データが送信されるフィールドである。フィールドＩｍａｇｅ　Ｄａｔａの画像データは、行ごとにフィールドＰＨとフィールドＰＦとに挟まれて、左側から右側に向けて順に送信される。イメージセンサ１２０から出力される画像データは、このフィールドＩｍａｇｅ　Ｄａｔａにより送信してよい。
　フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａは、画像データ以外のデータを送信するフィールドである。例えばオプションのデータを定義した場合は、当該データは、一般的には、フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａにより送信される。
　フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａのデータは、画像データと同様に、フィールドＰＨとフィールドＰＦとに挟まれて、左側から右側に向けて順に送信される。センサ部１０のＩＭＵ１００から出力されるＩＭＵデータは、このフィールドＥｍｂｅｄｄｅｄ　Ｄａｔａにより送信してよい。フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａにより送信されるデータの種別（ＩＭＵデータなど）を示す情報は、フィールドＰＨにより送信してよい。
　図８の例では、フィールドＥｍｂｅｄｄｅｄ　ＤａｔａがフィールドＩｍａｇｅ　Ｄａｔａの後に送信されているが、これはこの例に限定されない。図９は、実施形態に適用可能な、ＭＩＰＩ－ＣＳＩ－２に規定されるデータフォーマットの別の例を示す模式図である。
　図９において、セクション（ａ）は、図８と同様に、フィールドＥｍｂｅｄｄｅｄ　ＤａｔａがフィールドＩｍａｇｅ　Ｄａｔａの後に送信される例である。セクション（ｂ）は、フィールドＩｍａｇｅ　Ｄａｔａの送信を一旦中断して、フィールドＥｍｂｅｄｄｅｄ　ＤａｔａをフィールドＩｍａｇｅ　Ｄａｔａの途中に送信する例である。また、セクション（ｃ）は、フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａを、フィールドＩｍａｇｅ　Ｄａｔａの前後に送信する例である。さらに、フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａを、フィールドＩｍａｇｅ　Ｄａｔａの前に送信してもよい。
　また、ＭＩＰＩのフレームで点群を送信してよい。図１０は、実施形態に適用可能な、ＭＩＰＩ－ＣＳＩ－２に規定されるデータフォーマットで点群を送信する例を示す模式図である。
　図１０の例では、フィールドＰＨおよびＰＦに挟まれてフィールドＩｍａｇｅ　Ｄａｔａが送信された後に、同様にフィールドＰＨおよびＰＦに挟まれてフィールドＥｍｂｅｄｄｅｄ　Ｄａｔａが送信される。フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａでは、例えばＩＭＵデータが送信される。さらに、フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａの後に、フィールドＰＨおよびＰＦで挟まれて、点群データがフィールドＰｏｉｎｔ　Ｃｌｏｕｄ　Ｄａｔａにより送信される。フィールドＰｏｉｎｔ　Ｃｌｏｕｄ　Ｄａｔａの後に、フレームの最後を示すフィールドＦＥが送信される。
　上述では、フィールドＩｍａｇｅ　Ｄａｔａ、フィールドＥｍｂｅｄｄｅｄ　Ｄａｔａ、フィールドＰｏｉｎｔ　Ｃｌｏｕｄ　Ｄａｔａの順で送信しているが、各データの送信順は、この例に限定されない。また、フィールドＩｍａｇｅ　ＤａｔａやフィールドＰｏｉｎｔ　Ｃｌｏｕｄ　Ｄａｔａを１フレーム内で分割して送信してもよい。
　上述では、センサ部１０と信号処理部２０との間、信号処理部２０と情報処理部３０との間のデータ送信に係るインタフェースがＭＩＰＩである例について説明したが、これはこの例に限定されない。実施形態では、これらのデータ送信に係るインタフェースとして、他のインタフェースを適用してもよい。例えば、図１１に例示される信号処理システム１ａのように、センサ部１０と信号処理部２０との間は、Ｉ2Ｃ(Inter-Integrated　Circuit)、ＳＰＩ(Serial　Peripheral　Interface)、ＵＡＲＴ(Universal　Asynchronous　Receiver/Transmitter)といった、主に機器の内部的なデータ通信に用いられるシリアルインタフェースや、ＵＳＢ(Universal　Serial　Bus)、Ｅｔｈｅｒｎｅｔ（登録商標）といった通信インタフェースを適用してよい。また、信号処理部２０ａと情報処理部３０との間は、ＵＳＢ、Ｅｔｈｅｒｎｅｔ、Ｗｉ－Ｆｉ(Wireless　Fidelity)（登録商標）といった、主に機器の外部的な通信に用いられるインタフェースを適用してよい。
　図１２は、実施形態に係る信号処理部２０のアーキテクチャの例を示す図である。信号処理部２０は、ハードウェアである例えばＩＳＰといったプロセッサ２０００上でＯＳ(Operating　System)２０３０が動作し、ＯＳ２０３０上で、ライブラリ部２０１０とアプリケーション部２０２０とが動作する構造となっている。
　アプリケーション部２０２０は、ＡＰＩ(Application　Programming　Interface)呼出部２０４０を含み、ライブラリ部２０１０は、ＡＰＩ処理部２０４１を含む。ＡＰＩ呼出部２０４０は、アプリケーション部２０２０からの要求に従い、ライブラリ部２０１０の当該要求に応じた機能を呼び出す。ＡＰＩ処理部２０４１は、ＡＰＩ呼出部２０４０による機能の呼び出しに応じて、ライブラリ部２０１０における当該機能による応答をＡＰＩ呼出部２０４０に返す。
　一例として、アプリケーション部２０２０において、動体状態推定部２２１が動体状態情報の出力を要求する場合、ＡＰＩ呼出部２０４０は、ライブラリ部２０１０に対して、動体速度点群を要求する。ライブラリ部２０１０において、ＡＰＩ処理部２０４１は、この要求に応じて、動体点群補正部２１４の機能を呼び出して、動体点群補正部２１４から出力された動体速度点群をＡＰＩ呼出部２０４０に返す。ＡＰＩ呼出部２０４０は、ＡＰＩ処理部２０４１から返された動体速度点群を動体状態推定部２２１に渡す。
（２－２．実施形態に係る処理について）
　次に、実施形態に係る処理について、より詳細に説明する。
（２－２－１．実施形態に適用可能な計測技術）
　図１３は、実施形態に適用可能な、光検出測距センサ１１０における送信光信号検出処理を示す一例のフローチャートである。
　ステップＳ１０で、光検出測距センサ１１０は、光送受信部１１２により、同期信号に同期して周波数連続変調したレーザ光による送信光信号を送信（射出）、物体から反射して戻ってきた受信光信号を受信する。
　図１４は、実施形態に適用可能な、光送受信部１１２により送信される送信光信号を説明するための模式図である。図１４の上側の図は、送信光信号の光周波数と時間との関係を示し、下側に、上側の図における信号送信開始時刻ｔstを示している。図１４の上側の図において、縦軸が送信光信号の光周波数、横軸が時間をそれぞれ示している。
　図１４において、光送受信部１１２は、例えば信号送信開始時刻ｔst1から次の信号送信開始時刻ｔst2までを周期として、レーザ光の光周波数をそれぞれ直線的に上昇および下降させ、チャープ光を生成する。光送受信部１１２は、この光周波数の上昇および下降の組によるチャープ光を送信光信号として、光走査部１１１から所定の射出ポイントに向けて射出する。信号送信開始時刻ｔst2においても同様に、上述の周期に従い送信光信号の光周波数をそれぞれ直線的に上昇および下降させてチャープ光を生成し、送信光信号として、光走査部１１１から所定の射出ポイントに向けて射出する。ラスタ走査パターンでは、このように、送信光信号は、連続的に送信される、光周波数の上昇および下降の組によるチャープ光により形成される。
　説明は図１３に戻り、次のステップＳ１１で、光検出測距センサ１１０は、受信信号処理部１１３により、受信された受信光信号における第１の受信スペクトル信号から、第１のピーク値と、ピーク値における信号スペクトル周波数（ピーク周波数）を推定する。また、光検出測距センサ１１０は、受信信号処理部１１３により、受信された受信光信号における第２の受信スペクトル信号から、第２のピーク値と、ピーク値における信号スペクトル周波数（ピーク周波数）を推定する。
　なお、第１の受信スペクトル信号は、受信光信号のうち、光周波数が上昇する期間の送信光信号に対応する信号である。また、第２の受信スペクトル信号は、受信光信号のうち、光周波数が下降する期間の送信光信号に対応する信号である。
　次のステップＳ１２で、光検出測距センサ１１０は、受信信号処理部１１３において、ステップＳ１１で推定した第１のピーク値および第２のピーク値が閾値ｔｈ以上であるか否かを判定する。
　図１５は、実施形態に係る、受信光信号に対する判定処理を説明するための模式図である。図１５において、セクション（ａ）は、第１の受信スペクトル信号に対する判定処理を示す図、セクション（ｂ）は、第２の受信スペクトル信号に対する判定処理を示す図である。これらセクション（ａ）および（ｂ）の図において、縦軸は受信光信号の強度、横軸は受信スペクトル周波数を示している。
　図１５のセクション（ａ）において、第１の受信スペクトル信号の強度が、信号スペクトル周波数ｆpk1で閾値ｔｈを超え、第１のピーク値Ｖpk1が得られている。第１の受信スペクトル信号の強度が第１のピーク値Ｖpk1を取る信号スペクトル周波数ｆpk1を、第１のピーク周波数とする。
　図１５のセクション（ｂ）においても同様に、第２の受信スペクトル信号の強度が、信号スペクトル周波数ｆpk2で閾値ｔｈを超え、第２のピーク値Ｖpk2が得られている。第２の受信スペクトル信号の強度が第２のピーク値Ｖpk2を取る信号スペクトル周波数ｆpk2を、第２のピーク周波数とする。
　説明は図１３に戻り、ステップＳ１２で、受信信号処理部１１３は、第１のピーク値Ｖpk1および第２のピーク値Ｖpk2の少なくとも一方が閾値ｔｈ未満であると判定した場合（ステップＳ１２、「Ｎｏ」）、処理をステップＳ１５に移行させる。一方、受信信号処理部１１３は、第１のピーク値Ｖpk1および第２のピーク値Ｖpk2がそれぞれ閾値ｔｈ以上であると判定した場合（ステップＳ１２、「Ｙｅｓ」）、処理をステップＳ１３に移行させる。
　ステップＳ１３で、受信信号処理部１１３は、第１のピーク値Ｖpk1および第２のピーク値Ｖpk2と、第１のピーク値Ｖpk1および第２のピーク値Ｖpk2を取る信号スペクトル周波数ｆpk1およびｆpk2と、に基づき、物体までの距離と、物体に対するドップラー速度と、受信光信号の強度と、を算出する。また、受信信号処理部１１３は、同期信号に基づき信号送信開始時刻ｔst1を算出する。
　この、第１のピーク値Ｖpk1および第２のピーク値Ｖpk2がそれぞれ閾値ｔｈ以上である受信光信号に対応する射出ポイント４１を、物体が検出された検出点とする。
　次のステップＳ１４で、受信信号処理部１１３は、ステップＳ１３で算出した距離、ドップラー速度および強度を出力する。また、受信信号処理部１１３は、ステップＳ１３で算出した信号送信開始時刻を、計測ごと（射出ポイントごと）のタイプスタンプとして出力する。
　送信部１１５は、受信信号処理部１１３から出力された距離、ドップラー速度および強度と、タイムスタンプとを受け取る。送信部１１５は、受け取った距離、ドップラー速度および強度に対してタイムスタンプを付加し、ＦＭＣＷ－ＬｉＤＡＲデータとして光検出測距センサ１１０から出力する。
　次のステップＳ１５で、光検出測距センサ１１０は、計測が終了したか否かを判定する。例えば、光検出測距センサ１１０は、センサ部１０に対して外部から入力される制御信号（図示しない）に応じて、計測が終了したか否かを判定してよい。光検出測距センサ１１０は、計測が終了したと判定した場合、図１４のフローチャートによる一連の処理を終了させる。
　一方、光検出測距センサ１１０は、ステップＳ１５で計測が終了していないと判定した場合、処理をステップＳ１０に戻し、次の射出ポイントに対する処理を実行する。
　図１３のフローチャートにおけるステップＳ１２の判定処理について、具体的な例を用いて説明する。
　図１６は、実施形態に係る、光検出測距センサ１１０がラスタ走査パターンにより走査を行った場合のデータ出力の例を示す模式図である。図１６において、セクション（ａ）は、上述した図４に対応するもので、縦軸が走査の垂直方向角度を示し、横軸が走査の水平方向角度を示している。セクション（ｂ）は、各射出ポイント４１に対応して光検出測距センサ１１０から出力されるＦＭＣＷ－ＬｉＤＡＲデータを、時系列で並べた例を示している。また、図において塗り潰して示される射出ポイント４１ａおよび４１ｂは、例えば物体５０ａ、５０ｂ（図４参照）が存在する位置に対応する射出ポイントであって、物体５０ａ、５０ｂで反射された反射光が受信光信号として返される。
　光検出測距センサ１１０は、光走査部１１１により、所定の角度範囲４５内において、角度範囲４５の水平方向の両端で折り返す走査線４０に従い、角度範囲４５の上端と下端との間を走査する。この角度範囲４５内における各射出ポイント４１、４１ａおよび４１ｂにより、点群フレームが構成される。
　図１６のセクション（ｂ）に示すように、各射出ポイント４１の出力データは、送信光信号の射出タイミングに応じて、例えば所定の時間間隔で出力される。この場合において、物体５０ａ、５０ｂが存在する位置に対応する射出ポイント４１ａ、４１ｂ以外の射出ポイント４１（図において点線の丸印にて示す）では、光検出測距センサ１１０において、送信光信号に対応する受信光信号が受信されないか、若しくは、受信された受信光信号がノイズであると見做すことができる。
　図１５を参照し、例えば射出ポイント４１ａ、４１ｂでは、受信光信号の強度が閾値ｔｈ以上となる第１のピーク値Ｖpk1および第２のピーク値Ｖpk2が得られる。したがって、図１３のフローチャートにおけるステップＳ１２の判定に従い（ステップＳ１２、「Ｙｅｓ」）、処理がステップＳ１３に移行される。
　一方、図１６において点線の丸印で示す射出ポイント４１では、強度の第１のピーク値Ｖpk1および第２のピーク値Ｖpk2の少なくとも一方が明確に得られないか、若しくは、閾値ｔｈ未満の値となる。したがって、図１３のフローチャートにおけるステップＳ１２の判定に従い（ステップＳ１２、「Ｎｏ」）、処理がステップＳ１５に移行される。
　このように、点群フレームに含まれる射出ポイントのうち、物体５０ａ、５０ｂが存在する位置に対応する射出ポイント４１ａ、４１ｂにおいて図１３のフローチャートにおけるステップＳ１３、ステップＳ１４の処理が実行される。一方、点群フレームに含まれる射出ポイントのうち、当該射出ポイント４１ａ、４１ｂ以外の、図において点線の丸印で示す射出ポイント４１では、ステップＳ１３、ステップＳ１４の処理がキャンセルされ、次の射出ポイントに対する処理が実行される。
（２－２－２．実施形態に係るデータ構造例）
　次に、実施形態に係るデータ構造例について説明する。以下では、特に、センサ部１０から信号処理部２０に送信される各データのデータ構造例について説明する。
　図１７は、実施形態に適用可能なフレーム定義について説明するための模式図である。図１７において、セクション（ａ）は、イメージセンサ１２０から出力される画像データによるフレームの例を示している。図１７のセクション（ｂ）および（ｃ）は、それぞれ光検出測距センサ１１０から出力されるＦＭＣＷ－ＬｉＤＡＲデータによるフレームの例を示している。ここで、セクション（ｂ）は、ラスタ走査によるラスタ走査パターンの例を示し、セクション（ｃ）は、ドット走査によるドット走査パターンの例を示している。
　ラスタ走査は、例えば、機械式ミラースキャナにより実現される。また、ドット走査は、例えば、ＯＰＡ(Optical　Phase　Array)や光ビーム切替素子のような、ビームステアリングデバイスにより実現される。
　図１７のセクション（ａ）に示す画像データでは、それぞれＲ（赤）、Ｇ（緑）およびＢ（青）の各色の情報を含む画素６１の集合によりフレーム６０ａが構成される。フレーム６０ａのサイズは、幅方向および高さ方向の画素６１の数で表される。各画素６１は、画素アレイの有効画素領域に含まれる各画素に対応して、行列状の配列で配置される。
　図１７のセクション（ｂ）に示すラスタ走査パターンによるＦＭＣＷ－ＬｉＤＡＲデータでは、所定の角度範囲４５内の射出ポイント４１によりフレーム（点群フレーム）６０ｂが構成される。ラスタ走査パターンでは、上述したように、角度範囲４５の水平方向の両端で折り返す走査線４０に従い走査を行う。走査線４０上の各射出ポイント４１の位置は、光走査部１１１による送信光信号の、水平および垂直方向の走査角（水平角度θｈi、垂直角度θｖi）により表される。
　なお、ラスタ走査パターンでは、上述したように、光周波数の上昇および下降の組によるチャープ光が連続的に送信されることで、送信光信号が形成される。図１７のセクション（ｂ）に示す各射出ポイント４１は、例えば、チャープ光　において光周波数が上昇を開始する信号送信開始時刻ｔst1、ｔst2、…、における走査角（水平角度θｈi、垂直角度θｖi）に対応させてよい。
　図１７のセクション（ｃ）示すドット走査パターンによるＦＭＣＷ－ＬｉＤＡＲデータでは、所定の走査範囲４６内の各射出ポイント４３によりフレーム（点群フレーム）６０ｃが構成される。ドット走査パターンでは、射出ポイント４３ごとにチャープ光が完結する。すなわち、射出ポイント４３ごとに送信光信号が送信される。そのため、各射出ポイント４３の位置は、走査範囲４６内におけるｘｙ座標により表してよい。
　図１８Ａおよび図１８Ｂは、実施形態に係る、センサ部１０による各データの出力タイミングについて説明するための模式図である。なお、ここでは、イメージセンサ１２０は、全画素を同時に露光するグローバルシャッタ方式で露光が行われるものとする。
　図１８Ａは、実施形態に係る、センサ部１０の各センサによる計測タイミングの例を示している。センサ部１０において、ＩＭＵ１００、光検出測距センサ１１０およびイメージセンサ１２０は、それぞれ、同期信号生成部１３０から供給される同期信号Ｓｙｎｃに同期して計測動作を行い、データを出力する。なお、同期信号Ｓｙｎｃは、画像データの１フレーム周期に対応する周期で、同期信号生成部１３０から出力されるものとする。
　イメージセンサ１２０は、同期信号Ｓｙｎｃに同期して露光開始時刻ｔexst1で露光が開始され、露光期間ｔexの間、露光される。画像データの１フレーム周期後の露光開始時刻ｔexst2で、次の１フレームの露光が開始される。画像データのフレーム周期は、１０ｍｓ（ミリ秒）乃至１００ｍｓ程度とされる。例えば露光開始時刻ｔexst1から開始された露光による１フレームの画像データは、露光期間ｔexが終了してから次の露光開始時刻ｔexst2までの間に出力される。
　光検出測距センサ１１０は、同期信号Ｓｙｎｃに応じて、例えば露光開始時刻ｔexst1、ｔexst2、…、と同期したフレーム開始時刻ｔfst1、ｔfst2、…、に、それぞれ１フレームの走査が開始される。図１８Ｂは、イメージセンサ１２０における露光開始時刻ｔexstと、光検出測距センサ１１０におけるフレーム開始時刻ｔfstとの関係を模式的に示している。図１８Ｂに示されるように、光検出測距センサ１１０による、フレーム６０ｂの先頭の射出ポイント４１stにおける送信光信号の射出タイミングと、イメージセンサ１２０による、フレーム６０ａにおける各画素６１の露光開始時刻ｔexstとが同期される。
　光検出測距センサ１１０において、各射出ポイント４１における送信光信号の射出間隔は、例えば１μｓ（マイクロ秒）乃至数１００μｓ程度とされる。ＦＭＣＷ－ＬｉＤＡＲデータは、この各射出ポイント４１の射出タイミングに応じて出力される。
　ここで、図において点線で示される射出ポイント４１は、例えば物体などに送信光信号が反射されず、受信光信号が受信されなかった、或いは、受信光信号の強度が閾値ｔｈ未満の射出ポイントを示している。また、図において実線で示され斜線を付された射出ポイント４１は、例えば物体などに送信光信号が反射され閾値ｔｈ以上の強度の受信光信号が受信された射出ポイントを示している。したがって、ＦＭＣＷ－ＬｉＤＡＲデータは、実際には、図において実線で示され斜線を付された射出ポイント４１に対応するタイミングでのみ、出力される。
　ＩＭＵ１００は、同期信号Ｓｙｎｃに応じて、画像データおよびＦＭＣＷ－ＬｉＤＡＲデータのフレームの先頭に同期してＩＭＵデータを出力する。ＩＭＵ１００は、同期信号Ｓｙｎｃから次の同期信号Ｓｙｎｃまでの期間は、例えば同期信号Ｓｙｎｃを逓倍した信号に従い、ＩＭＵデータを出力してよい。ＩＭＵ１００におけるＩＭＵデータの出力間隔は、例えば１ｍｓ乃至１０００ｍｓ程度とされる。
　図１９は、実施形態に係る、センサ部１０から出力されるＦＭＣＷ－ＬｉＤＡＲデータおよびＩＭＵデータのデータ構造の例を示す模式図である。図１９において、セクション（ａ）は、ＦＭＣＷ－ＬｉＤＡＲデータのデータ構造の例を示し、セクション（ｂ）は、ＩＭＵデータのデータ構造の例を示している。セクション（ａ）および（ｂ）共に、図上で縦方向に時間を表している。
　図１９のセクション（ａ）において、ＦＭＣＷ－ＬｉＤＡＲデータは、例えばフレーム先頭にヘッダが送信され、ヘッダに続けて各射出ポイントに対応するデータが順次に送信される。なお、図において、各射出ポイントは、点＃１、点＃２、…、点＃Ｎとして示されている。以下、適宜、各射出ポイントを点＃１、点＃２、…、点＃Ｎのように記述する。また、点＃１、点＃２、…、点＃Ｎを特に区別する必要の無い場合は、点＃ｉで代表させ、図における添字による各数字も、ｉで代表させて説明を行う（セクション（ｂ）も同様）。
　ヘッダは、少なくともスキャン種別情報を含む。スキャン種別情報は、例えばラスタ走査、マルチレイヤ型走査、ドット走査といった、走査の方式を示す情報であってよい。
　点＃ｉのデータは、走査に関する情報としてタイムスタンプＴｓiと、走査角（水平角度θｈi、垂直角度θｖi）と、を含み、受信光信号に基づき得られた計測データとして距離ｄiと、ドップラー速度ｗiと、強度Ｌｐiと、を含む。タイムスタンプＴｓiは、点＃ｉにおける送信光信号を送信した時刻を示す。
　また、各点＃ｉは、上述した受信光信号の強度が、第１および第２の受信スペクトル信号において、閾値ｔｈ以上の値となった点であってよい。実施形態では、各点＃ｉのデータにタイムスタンプＴｓiが付加されているため、１フレームのＦＭＣＷ－ＬｉＤＡＲデータに含まれる各点ｉのデータがどの射出ポイントに対応する受信光信号に基づくデータであるかを、容易に知ることが可能である。
　また、各点＃ｉのデータに対する信号処理部２０における処理（例えば静止体点群補正部２１３および動体点群補正部２１４による補正処理）が、走査方式により異なる場合がある。実施形態では、フレームの先頭に送信されるヘッダに、走査方式を示すスキャン種別情報が含まれる。そのため、信号処理部２０は、このＦＭＣＷ－ＬｉＤＡＲデータに対して、走査方式に応じた処理を施すことが可能である。
　図１９のセクション（ｂ）において、ＩＭＵデータは、ＩＭＵ１００における計測タイミングごとに、タイムスタンプＴｓ－ＩＭＵi、３軸加速度ａi、３軸角速度ωiおよび３軸地磁気ｇｅiが順次に送信される。ＩＭＵデータは、タイムスタンプＴｓ－ＩＭＵ1が付加されるフレームの最初のデータが、例えばセクション（ａ）に示すＦＭＣＷ－ＬｉＤＡＲデータにおける最初の点＃１のデータと同期される。
　ここで、ＩＭＵ１００は、一定の時間間隔で計測を行う。そのため、例えばフレームの最初のデータをＦＭＣＷ－ＬｉＤＡＲデータの点＃１のデータと同期させることで、各データの計測タイミングを推定することができ、各タイムスタンプＴｓ－ＩＭＵiを省略することができる。
（２－２－３．実施形態に係る点群補正処理）
　次に、実施形態に係る点群補正処理について説明する。実施形態に係る点群補正処理は、図２に示した静止体点群補正部２１３および動体点群補正部２１４において実行される。
　図２０は、実施形態に係る光検出測距センサ１１０による出力データの例を示す模式図である。光検出測距センサ１１０は、検出点としての点ｉの検出時刻ｔiと、点ｉの位置ベクトルｒiと、点ｉのドップラー速度ｗiと、を出力する。なお、以下において、点ｉは、受信光信号における第１および第２の受信スペクトル信号の強度がそれぞれ閾値ｔｈ以上である射出ポイントを指す。
　図２１は、実施形態に適用可能な、射出ポイントの座標の定義を示す模式図である。図２１において、走査に係る角度範囲４５の中央を原点として、図上で垂直方向に垂直角度θv、水平方向に水平角度θhが定義される。角度範囲４５の左上隅の射出ポイント４１をポイント＃１として、左から右方向に向けてポイント＃１、＃２、…、とし、角度範囲４５の右下隅の射出ポイント４１をポイント＃Ｎとする。また、各射出ポイント４１のうち、計測の対象となる物体５２に対応する位置の射出ポイント４１を、点ｉとする。
　図２２は、光検出測距センサ１１０から見た各射出ポイント４１および物体５２を示す模式図である。
　なお、図２２、後述する図２３および図２６、ならびに、後述する各数式にて用いられる記号等は、下記の各表現に置き換えて記載することがある。
・図および数式において太字で表現されるベクトルは、「ベクトル_ｘ」のように、直前に「_」（アンダースコア）を付して示す。
・図および数式においてベクトルを示すために直上に「→」（矢印）が付された文字は、「ベクトル→ｘ」のように、その文字の直前に「→」矢印を付して示す。
・図および数式において文字に付された上付き文字「ｓ」は、その文字の示す値がセンサ座標系の値であることを示す。
・図および数式において文字に付された上付き文字「ｍ」は、その文字の示す値が動体剛体座標系の値であることを示す。
・図および数式において時間微分を示すために直上に「・」（点）が付された文字は、「・ｘ」のように、その文字の直前に「・」を付して示す。
・図および数式において角速度チルダ行列を示すために直上に「～」（チルダ）が付された文字は、「～ｘ」のように、その文字の直前に「～」を付して示す。
　図２２において、光検出測距センサ１１０の位置を位置Ｏsとし、位置Ｏsを原点として、座標軸ｘs、ｙsおよびｚsにより、センサ座標系が定義される。また、各射出ポイント４１は、便宜上、光検出測距センサ１１０から等距離の点であるものとして示している。
　図２２において、物体５２に対応する点ｉにおいて、ベクトル→ｖiは、物体５２の運動を示し、ベクトル→ｗiは、センサの位置Ｏsに対する物体５２のドップラー速度を示している。また、位置Ｏsから点ｉに向けたベクトル→ｒiは、光検出測距センサ１１０から物体５２における点ｉまでの距離を示している。最終的に取得したい情報は、ベクトル→ｖiである。
　図２３は、実施形態に係る信号処理部２０における、点群補正に係る処理の全体的な流れを示す模式図である。
　図２３において、センサ位置姿勢推定処理４００、静止体動体弁別処理４０１およびセンサ速度推定処理４０２は、それぞれ、図２のセンサ位置姿勢推定部２１０、動体・静止体分離部２１１およびセンサ速度推定部２１２による処理となる。また、点群補正処理４０３および４０４は、それぞれ、図２の静止体点群補正部２１３および動体点群補正部２１４の処理となる。さらに、動体検出処理４０５は、図２の動体状態推定部２２１の処理となる。なお、図２における地図変換部２２０の処理は、図２３においては、ＳＬＡＭ処理４１０として示されている。
　ＩＭＵ１００は、ＩＭＵデータとして、加速度_ａs、角速度_ωsおよび地磁気_μsを出力する。センサ位置姿勢推定処理４００は、ＩＭＵ１００から出力された、加速度_ａs、角速度_ωsおよび地磁気_μsと、ＳＬＡＭ処理４１０により得られたセンサ位置_ｐs,j、センサ姿勢Ｒs,j、センサ角速度～ωs,jおよびセンサ速度_ｖs,jと、に基づき、センサ位置_ｐs、センサ姿勢Ｒsおよびセンサ角速度～ωsを推定する。センサ位置姿勢推定処理４００は、推定したセンサ位置ｐsをＳＬＡＭ処理４１０に渡す。また、センサ位置姿勢推定処理４００は、推定したセンサ姿勢Ｒsおよびセンサ角速度～ωsをセンサ速度推定処理４０２に渡す。
　光検出測距センサ１１０は、ＦＭＣＷ－ＬｉＤＡＲデータとして、点ｉごとに、検出時刻ｔiと、検出時刻ｔiにおける位置としての距離_ｒs i(ｔi)と、ドップラー速度ｗi(ｔi)と、を出力する。光検出測距センサ１１０から出力された各データは、静止体動体弁別処理４０１に渡される。
　静止体動体弁別処理４０１は、光検出測距センサ１１０から渡された各データと、センサ速度推定処理４０２で推定されたセンサ速度_ｖsと、に基づき、検出時刻ｔｉにおける静止体の、位置としての距離｛_ｒs i(ｔi)｝i=staticと、ドップラー速度｛ｗi(ｔi)｝i=staticと、静止体の検出時刻｛ｔi｝i=staticと、を算出する。
　なお、中括弧「｛」および「｝」は、挟まれた部分が集合であることを示す。
　静止体動体弁別処理４０１は、算出した距離｛_ｒs i(ｔi)｝i=staticおよびドップラー速度｛ｗi(ｔi)｝i=staticをセンサ速度推定処理４０２に渡す。また、静止体動体弁別処理４０１は、算出した距離｛_ｒs i(ｔi)｝i=staticおよび静止体の検出時刻｛ｔi｝i=staticを、静止体点群の補正を行う点群補正処理４０３に渡す。
　さらに、静止体動体弁別処理４０１は、光検出測距センサ１１０から渡された各データと、センサ速度推定処理４０２で推定されたセンサ速度_ｖsと、に基づき、検出時刻ｔiにおける動体の、位置としての距離｛_ｒs i(ｔi)｝i=dynamicと、ドップラー速度｛ｗi(ｔi)｝i=dynamicと、静止体の検出時刻｛ｔi｝i=dynamicと、を算出する。
　静止体動体弁別処理４０１は、算出した距離｛_ｒs i(ｔi)｝i=dynamicと、ドップラー速度｛ｗi(ｔi)｝i=dynamicと、静止体の検出時刻｛ｔi｝i=dynamicと、を動体点群の補正を行う点群補正処理４０４に渡す。
　センサ速度推定処理４０２は、上述した、センサ位置姿勢推定処理４００から渡されたセンサ姿勢Ｒsおよびセンサ角速度～ωsと、静止体動体弁別処理４０１から渡された距離｛_ｔs i(ｔi)｝i=staticおよびドップラー速度｛ｗi(ｔi)｝i=staticとに基づき、センサ部１０の速度としてセンサ速度_ｖsを算出する。センサ速度推定処理４０２は、算出したセンサ速度_ｖsを、静止体動体弁別処理４０１と、点群補正処理４０３および４０４と、ＳＬＡＭ処理４１０と、に渡す。
　点群補正処理４０３は、センサ位置姿勢推定処理４００から渡されたセンサ姿勢Ｒsおよびセンサ角速度～ωsと、静止体動体弁別処理４０１から渡された距離｛_ｒs i(ｔi)｝i=staticおよび静止体の検出時刻｛ｔi｝i=staticと、に基づき、補正された静止体点群の位置としての距離｛_ｒs i(ｔN)｝i=staticを算出する。点群補正処理４０３は、算出した距離｛_ｒs i(ｔN)｝i=staticを、ＳＬＡＭ処理４１０に渡す。
　点群補正処理４０４は、センサ位置姿勢推定処理４００から渡されたセンサ姿勢Ｒsおよびセンサ角速度～ωsと、静止体動体弁別処理４０１から渡された距離｛_ｒs i(ｔi)｝i=dynamic、ドップラー速度｛ｗi(ｔi)｝i=dynamicおよび静止体の検出時刻｛ｔi｝i=dynamicと、センサ速度推定処理４０２から渡されたセンサ速度_ｖsと、ＳＬＡＭ処理４１０から渡されたセンサ位置_ｐs,jと、動体検出処理４０５から渡された動体位置｛_ｐm｝j、動体姿勢｛Ｒm｝jおよび｛_ｖm｝ｊ、ならびに、動体角速度｛～ωm｝jと、に基づき、補正された動体点群｛ｒs i(ｔN)｝i=dynamicと、補正されたドップラー速度｛ｗ'i(ｔN)｝i=dynamicと、を算出する。点群補正処理４０４は、算出した、補正された動体点群｛ｒs i(ｔN)｝i=dynamicおよび補正されたドップラー速度｛ｗ'i(ｔN)｝i=dynamicを、動体検出処理４０５に渡す。
　動体検出処理４０５は、点群補正処理４０４から渡された補正された動体点群｛_ｒs i(ｔN)｝i=dynamicおよび補正されたドップラー速度｛ｗ'i(ｔN)｝i=dynamicに基づき、動体位置｛_ｐm｝jと、動体姿勢｛Ｒm｝jおよび｛_ｖm｝jと、動体角速度｛～ωm｝jと、動体範囲｛Ｅm｝jと、を算出し、出力する。また、動体検出処理４０５は、フレームごとの検出時刻ｔjを出力する。動体検出処理４０５から出力された各値は、例えば送信部２０１（図２参照）に渡される。さらに、動体検出処理４０５は、算出した動体位置｛_ｐm｝j、動体姿勢｛Ｒm｝jおよび｛_ｖm｝j、ならびに、動体角速度｛～ωm｝jを、点群補正処理４０４に渡す。
　ＳＬＡＭ処理４１０は、センサ位置姿勢推定処理４００から渡されたセンサ位置_ｐs、センサ姿勢Ｒsおよびセンサ角速度～ωsと、センサ速度推定処理４０２から渡されたセンサ速度_ｖsと、点群補正処理４０３から渡された、補正された静止体点群｛_ｒs i(ｔN)｝i=staticと、に基づき、地図＜_ｍ＞（「＜」および「＞」はそれぞれ大括弧を示す）を作成し、出力する。また、ＳＬＡＭ処理４１０は、センサ位置_ｐs,jと、センサ姿勢Ｒs,jと、センサ角速度～ωs,jと、センサ速度_ｖs,jと、を出力する。さらに、ＳＬＡＭ処理４１０は、フレームごとの検出時刻ｔjを出力する。ＳＬＡＭ処理４１０から出力された各値は、例えば送信部２０１（図２参照）に渡される。
　図２４は、実施形態に係る信号処理部２０における、点群補正に係る処理を示す一例のフローチャートである。図２４における処理は、光検出測距センサ１１０による１フレームの計測処理ごとに実行される。
　ステップＳ２００で、信号処理部２０は、光検出測距センサ１１０から出力されるＦＭＣＷ－ＬｉＤＡＲデータによるフレームの取得を開始する。
　次のステップＳ２０１で、信号処理部２０は、静止体動体弁別処理４０１により、受信光信号が検出された検出点の距離およびドップラー速度と、検出時刻と、走査角とを取得する。次のステップＳ２０２で、信号処理部２０は、静止体動体弁別処理４０１により、センサ速度推定処理４０２が前回の処理で推定したセンサ速度を取得する。次のステップＳ２０３で、信号処理部２０は、静止体動体弁別処理４０１により、ステップＳ２０２で取得されたセンサ速度を用いて、検出点のドップラー速度を座標変換し、補正ドップラー速度を算出する。
　次のステップＳ２０４で、信号処理部２０は、静止体動体弁別処理４０１により、ステップＳ２０３で算出された検出点の補正ドップラー速度の絶対値が閾値以下であるか否かを判定する。
　信号処理部２０は、静止体動体弁別処理４０１により、検出点の補正ドップラー速度の絶対値が閾値以下であると判定した場合（ステップＳ２０４、「Ｙｅｓ」）、処理を静止体点群の処理に係るステップＳ２１０に移行させる。
　ステップＳ２１０で、信号処理部２０は、静止体点群の補正を行う点群補正処理４０３により、検出点を、静止体の速度点群フレームに追加する。次のステップＳ２１１で、信号処理部２０は、点群補正処理４０３により、センサ位置姿勢推定処理４００で推定されたセンサ姿勢および角速度を取得する。次のステップＳ２１２で、信号処理部２０は、センサ速度推定処理４０２により、静止帯の速度点群フレームと、センサ姿勢および角速度とを用いて、センサ速度を推定する。
　次のステップＳ２１３で、信号処理部２０は、点群補正処理４０３により、ステップＳ２１２で推定したセンサ速度と、ステップＳ２１１で取得したセンサ姿勢および角速度と、前回の処理からの時間差分とを用いて、静止体の速度点群フレームを補正する。
　次のステップＳ２１４で、信号処理部２０は、点群補正処理４０３により、フレーム内の全点に対する処理が完了したか否かを判定する。信号処理部２０は、点群補正処理４０３により処理が完了したと判定されると（ステップＳ２１４、「Ｙｅｓ」）、処理をステップＳ２３０に移行させる。一方、信号処理部２０は、点群補正処理４０３により処理が完了していないと判定されると（ステップＳ２１４、「Ｎｏ」）、処理をステップＳ２０１に戻し、ＦＭＣＷ－ＬｉＤＡＲデータによるフレーム内の次の検出点に対する処理を実行する。
　信号処理部２０は、上述したステップＳ２０４において、静止体動体弁別処理４０１により、検出点の補正ドップラー速度の絶対値が閾値を超えると判定した場合（ステップＳ２０４、「Ｎｏ」）、処理を動体点群の処理に係るステップＳ２２０に移行させる。
　ステップＳ２２０で、信号処理部２０は、動体点群の補正を行う点群補正処理４０４により、検出点を、動体の速度点群フレームに追加する。次のステップＳ２２１で、信号処理部２０は、点群補正処理４０４により、センサ位置姿勢推定処理４００で推定されたセンサ姿勢および角速度を取得する。
　次のステップＳ２１２で、信号処理部２０は、点群補正処理４０４により、動体の速度点群に対してクラスタリングを行い、動体の速度点群を局所速度点群に分割する。すなわち、信号処理部２０は、点群補正処理４０４により、動体の速度点群フレームに含まれる速度点群を、動体ごとの速度点群に分割する。
　次のステップＳ２２３で、信号処理部２０は、点群補正処理４０４により、センサ速度、センサ姿勢および角速度と、動体の位置、姿勢、速度および角速度と、前回の処理からの時間差分とを用いて、速度点群フレームにおける各動体の点群クラスタフレーム（局所速度点群）を補正する。
　次のステップＳ２２４で、信号処理部２０は、点群補正処理４０４により、フレーム内の全点に対する処理が完了したか否かを判定する。信号処理部２０は、点群補正処理４０４により処理が完了したと判定されると（ステップＳ２２４、「Ｙｅｓ」）、処理をステップＳ２３０に移行させる。一方、信号処理部２０は、点群補正処理４０４により処理が完了していないと判定されると（ステップＳ２２４、「Ｎｏ」）、処理をステップＳ２０１に戻し、フレーム内の次の検出点に対する処理を実行する。
　ステップＳ２３０で、信号処理部２０は、点群補正処理４０３で補正された静止体点群による静止体速度点群フレームと、点群補正処理４０４で補正された動体点群による動体速度点群フレームと、を出力する。出力された静止体速度点群フレームは、例えばＳＬＡＭ処理４１０に渡される。また、出力された動体速度点群フレームは、例えば動体検出処理４０５に渡される。
　図２５は、実施形態に係る信号処理部２０における、静止体の速度点群フレームの補正処理を示す一例のフローチャートである。図２５のフローチャートは、上述した図２４のフローチャートにおけるステップＳ２１３の処理を、より詳細に示すものである。
　ステップＳ２５０で、信号処理部２０は、点群補正処理４０３により、センサ速度推定処理４０２により推定されたセンサ速度を取得する。次のステップＳ２５１で、信号処理部２０は、点群補正処理４０３により、静止体の速度点群フレームに含まれる各点の座標を取得する。次のステップＳ２５２で、信号処理部２０は、点群補正処理４０３により、センサ位置姿勢推定処理４００で推定されたセンサ姿勢および加速度を取得する。次のステップＳ２５３で、信号処理部２０は、点群補正処理４０３により、ステップＳ２５０～ステップＳ２５２で取得された各値に基づき、静止体の速度点群フレームに含まれる各点の、瞬時速度ベクトルを推定する。
　次のステップＳ２５４で、信号処理部２０は、点群補正処理４０３により、前回の処理からの差分時間と、各点の瞬時速度ベクトルの積から、各点の微小位置変化を算出する。信号処理部２０は、点群補正処理４０３により、算出した各点の微小位置変化に基づき、各点の位置をシフトさせ、静止体の速度点群フレームの補正を行う。
（実施形態に係る処理の理論式による説明）
　ここで、図２３を用いて説明した、実施形態に係る信号処理部２０における処理を、理論式を用いて説明する。
（静止体に係る処理につて）
　先ず、静止体に係る処理について説明する。図２６は、以下の説明にて用いる座標系および各変数を定義するための模式図である。図２６においては、光検出測距センサ１１０（以下、センサ）の位置を位置Ｏsとし、位置Ｏsを原点として、互いに直交する座標軸ｘs、ｙsおよびｚsにより、センサ座標系が定義される。センサの角速度および速度を、それぞれ角速度→ωsおよび速度→ｖsとする。
　静止体（剛体）において、剛体上の位置Ｏmを原点として、互いに直交する軸ｘm，ｙmおよびｚmにより、剛体座標系が定義される。剛体の角速度および速度を、それぞれ→ωmおよび→ｖmとする。
　剛体上の検出位置である点ｉは、位置Ｏmからベクトル→ｕiで示される位置とされ、点ｉは、速度→ｖiと、光検出測距センサ１１０に対するドップラー速度→ｗiとを持つものとする。点ｉは、センサから見て距離→ｒiの位置にあるものとする。
　また、空間内の任意の位置Ｏを原点として、互いに直交する軸ｘ，ｙ，ｚにより、世界座標系が定義される。世界座標系の原点である位置Ｏから見て、剛体座標系の原点である位置Ｏmは、位置→ｐmにあり、点ｉは、位置→ｐiにあるものとする。また、位置Ｏから見て、センサ座標系の原点である位置Ｏsは、位置→ｐsにあるものとする。
（検出対象が静止体であり、且つ、センサが運動している場合）
　先ず、検出対象が静止体であり、且つ、センサが運動している場合の、各座標系における各値の関係について説明する。点ｉの位置ベクトル_ｐiは、次式（１）にて表される。
Figure JPOXMLDOC01-appb-M000001
　また、点ｉの速度ベクトル・_ｐiは、次式（２）にて表される。
Figure JPOXMLDOC01-appb-M000002
　なお、式（２）において、剛体の角速度を示す角速度チルダ行列～ωsは、次式（３）の角速度ベクトル_ωに基づき、次式（４）のように表される。
Figure JPOXMLDOC01-appb-M000003
Figure JPOXMLDOC01-appb-M000004
　式（２）において、剛体座標系が静止している場合、次式（５）が成り立つ。
Figure JPOXMLDOC01-appb-M000005
　また、ベクトル恒常式により、距離_ｒiおよび角速度チルダ行列～ωsに、次式（６）の関係が成り立つ。
Figure JPOXMLDOC01-appb-M000006
　また、点ｉのドップラー速度ｗiは、次式（７）にて表される。
Figure JPOXMLDOC01-appb-M000007
　上述の式（２）、式（５）～式（７）により、次式（８）が得られる。
Figure JPOXMLDOC01-appb-M000008
　式（８）を次式（９）のように変形して各値の関係を整理し、ドップラー速度ｗｉと点ｉの速度ベクトル・_ｐiとの関係を得る。
Figure JPOXMLDOC01-appb-M000009
（ドップラー速度を用いたセンサ速度の推定）
　次に、センサ速度推定処理４０２による、センサが運動し、点ｉが静止体の場合の、ドップラー速度を用いたセンサ速度の推定方法について説明する。ドップラー速度ｗiと、センサの速度ｖsとの関係は、上述した式（９）に基づき、次式（１０）にて表される。
Figure JPOXMLDOC01-appb-M000010
　式（１０）は、センサ姿勢Ｒsを用いて、次式（１１）のように表すことができる。
Figure JPOXMLDOC01-appb-M000011
　式（１１）において、－(ｒs i／ｒi)は、ビーム方向ベクトルを示し、これは、行列を用いて次式（１２）のように表すことができる。
Figure JPOXMLDOC01-appb-M000012
　式（１２）を式（１１）に代入することで、次式（１３）が得られる。
Figure JPOXMLDOC01-appb-M000013
　ここで、式（１３）に基づき、値_ｅiを次式（１４）のように定義する。
Figure JPOXMLDOC01-appb-M000014
　式（１４）を用いて、センサ速度_ｖsを表す次式（１５）を得ることができる。センサ速度推定処理４０２では、この式（１５）により、センサ速度_ｖsを推定する。
Figure JPOXMLDOC01-appb-M000015
　なお、式（１５）において、値ｅiによる行列は、静止体、世界座標系におけるビーム方向ベクトルの行列を示している。
（静止体点群の補正処理）
　次に、センサが運動している場合の、点群補正処理４０３による静止体点群の補正処理について説明する。
　点ｉの速度ベクトル_ｖiは、次式（１６）にて表される。
Figure JPOXMLDOC01-appb-M000016
　式（１６）を変形して、次式（１７）を得る。
Figure JPOXMLDOC01-appb-M000017
　式（１７）をさらに変形して、次式（１８）を得る。
Figure JPOXMLDOC01-appb-M000018
　式（１８）を離散方程式化して、次式（１９）を得る。なお、式（１９）において、Δｔiは、計測差分時間であって、Δｔi＝ｔi－ｔi-1で求められる。
Figure JPOXMLDOC01-appb-M000019
　式（１９）に基づき、補正係数Χs iを、次式（２０）のように定義する。
Figure JPOXMLDOC01-appb-M000020
　計測差分時間Δｔiと、式（２０）に示した補正係数Χs iとを用いて、次式（２１）により、静止体点群の各点の位置を補正する。
Figure JPOXMLDOC01-appb-M000021
　なお、式（２１）において、左辺は、補正後の各検出点（点ｉ）の、センサ座標系における位置ベクトルを示している。また、右辺の最後の項は、各検出点の補正前の、センサ座標系における位置ベクトルを示している。点群補正処理４０３では、式（２１）により、各検出点の補正前の、センサ座標系における検出点（点ｉ）の位置ベクトルに対して、当該点ｉにおける計測差分時間Δｔiと補正係数Χs iとに基づく補正項により位置補正値を加算することで、静止体点群の各点の位置を補正している。
（動体に係る処理につて）
　次に、動体に係る処理について説明する。座標系については、上述した図２６における静止体（剛体）を動体（剛体）に置き換えたものとなる以外は、図２６と共通であるため、ここでの説明を省略する。
（検出対象が動体であり、且つ、センサが運動している場合）
　先ず、検出対象が動体であり、且つ、センサが運動している場合の、各座標系における各値の関係について説明する。点ｉ（検出位置）の位置ベクトル_ｐiは、次式（２２）にて表わされる。
Figure JPOXMLDOC01-appb-M000022
　また、点ｉの剛体座標系の速度ベクトル・_ｐiは、次式（２３）にて表される。なお、点ｉのセンサ座標系の速度ベクトル・_ｐiは、上述した式（２）と同様であるので、ここでの説明を省略する。
Figure JPOXMLDOC01-appb-M000023
　ベクトル恒常式による上述した式（６）と、式（２）の、点ｉのセンサ座標系の速度ベクトル・_ｐiとを用いて式（２３）を変形して、次式（２４）が得られる。
Figure JPOXMLDOC01-appb-M000024
　式（２４）を、さらに次式（２５）のように変形し、各値の関係を整理する。
Figure JPOXMLDOC01-appb-M000025
（静止体と動体との速度弁別）
　次に、静止体動体弁別処理４０１による、静止体と動体との速度弁別について説明する。ドップラー速度ｗiを、点ｉの動きに応じて補正した補正ドップラー速度ｗｉ’は、次式（２６）にて与えられる。
Figure JPOXMLDOC01-appb-M000026
　静止体動体弁別処理４０１では、この補正ドップラー速度ｗi’に対して閾値判定を行うことで、点ｉが動体であるか、静止体であるかを判定する。例えば、動体を判定するための動体判定閾値ｖth1と、静止体を判定するための静止体判定閾値ｖth2を設定する。補正ドップラー速度ｗi’の絶対値が静止体判定閾値ｖth2未満の場合に（｜ｗi’｜＜ｖth2）、点ｉが静止体であると判定する。また、補正ドップラー速度ｗi’の絶対値が動体判定閾値ｖth1を超える場合に（｜ｗi’｜＞ｖth1）、点ｉが動体であると判定する。
（ドップラー速度を用いたセンサ速度の推定）
　次に、センサ速度推定処理４０２による、センサが運動し、点ｉが動体の場合の、ドップラー速度を用いたセンサ速度の推定方法について説明する。センサの動体に対する相対速度は、ドップラー速度ｗiを用いて次式（２７）のように表される。
Figure JPOXMLDOC01-appb-M000027
　式（２７）を、センサ姿勢Ｒsを用いて変形し、次式（２８）を得る。
Figure JPOXMLDOC01-appb-M000028
　ここで、値_ηiを、次式（２９）のように定義する。
Figure JPOXMLDOC01-appb-M000029
　式（２９）におけるベクトル_ｕiは、次式（３０）により得られる。
Figure JPOXMLDOC01-appb-M000030
　なお、式（２９）は、行列式を用いて次式（３１）のように表される。
Figure JPOXMLDOC01-appb-M000031
　上述した式（２８）を式（２９）～式（３１）を用いて変形して、次式（３２）を得る。
Figure JPOXMLDOC01-appb-M000032
　ここで、値ξiを、次式（３３）のように定義する。
Figure JPOXMLDOC01-appb-M000033
　上述した式（３２）を、式（３３）の値ξiと、式（１４）を用いて定義した値ｅiとを用いて変形し、動体速度_ｖmを示す次式（３４）を得る。
Figure JPOXMLDOC01-appb-M000034
（動体点群の補正処理）
　次に、センサが運動している場合の、点群補正処理４０４による動体点群補正処理について説明する。
　センサ座標系における点ｉの速度ベクトル_ｖiは、次式（３５）により表される。
Figure JPOXMLDOC01-appb-M000035
　また、剛体座標系における点ｉの速度ベクトル_ｖiは、次式（３６）により表される。
Figure JPOXMLDOC01-appb-M000036
　式（３５）と式（３６）とを等しいとし、式（３５）をセンサ姿勢Ｒsを用いて変形して、次式（３７）を得る。
Figure JPOXMLDOC01-appb-M000037
　式（３７）のベクトル_ｕiに、上述した式（３０）を代入し、次式（３８）を得る。
Figure JPOXMLDOC01-appb-M000038
　式（３８）を離散方程式化し、計測差分時間Δｔiを適用して次式（３９）を得る。
Figure JPOXMLDOC01-appb-M000039
　式３９に基づき、補正係数σs iを、次式（４０）のように定義する。
Figure JPOXMLDOC01-appb-M000040
　計測差分時間Δｔiと、式（４０）に示した補正係数σs iとを用いて、次式（４１）により、動体点群の各点の位置を補正する。
Figure JPOXMLDOC01-appb-M000041
　なお、式（４１）において、左辺は、補正後の各検出点（点ｉ）の、センサ座標系における位置ベクトルを示している。また、右辺の最後の項は、各検出点の補正前の、センサ座標系における位置ベクトルを示している。点群補正処理４０４では、式（４１）により、各検出点の補正前の、センサ座標系における検出点（点ｉ）の位置ベクトルに対して、当該点ｉにおける計測差分時間Δｔiと補正係数σs iとに基づく補正項により位置補正値を加算することで、動体点群の各点の位置を補正している。
（３．本開示に係る実施形態の第１の変形例）
　次に、本開示に係る実施形態の第１の変形例について説明する。実施形態の第１の変形例は、図２に示した実施形態に係る信号処理システム１に対して、光検出測距センサ１１０の走査を制御する構成を追加した例である。
　図２７は、実施の第１の変形例に係る信号処理システムの一例の構成を示すブロック図である。図２７において、信号処理システム１ａは、信号処理部２０ａに、図２に示した信号処理システム１における信号処理部２０に対して、制御通信部２３０と、走査制御部２３１と、パラメータ設定部２３２と、が追加されている。
　制御通信部２３０は、情報処理部３０から送信された座標基準情報およびＲＯＩ指定情報に基づき、光検出測距センサ１１０における走査を制御するための走査制御情報を生成する。
　例えば、制御通信部２３０は、受信した座標基準情報を、パラメータ設定部２３２に渡す。パラメータ設定部２３２は、渡された座標基準情報に基づき、例えば光検出測距センサ１１０の走査に係る初期値を設定する。制御通信部２３０は、パラメータ設定部２３２により設定された初期値を、走査制御情報として光検出測距センサ１１０に渡す。光検出測距センサ１１０は、制御通信部２３０から渡された走査制御情報に従い、走査機構などの初期化を行う。
　また例えば、制御通信部２３０は、受信したＲＯＩ指定情報を、走査制御部２３１に渡す。ＲＯＩ指定情報は、例えば画像データに対して注目領域として設定された座標情報を含んでよい。走査制御部２３１は、例えば、渡されたＲＯＩ指定情報に基づき、座標情報を、光検出測距センサ１１０における走査範囲を制御するための水平および垂直の角度範囲の情報に変換して、制御通信部２３０に渡す。制御通信部２３０は、走査制御部２３１から渡された水平および垂直の角度範囲の情報を、走査制御情報として光検出測距センサ１１０に渡す。光検出測距センサ１１０は、渡された走査制御情報に従い、走査範囲を制御する。
　これにより、光検出測距センサ１１０に対して、指定された注目領域を限定的に走査させるように制御でき、走査に要する時間を短縮することが可能となる。また、光検出測距センサ１１０に対して、指定された注目領域を限定的且つ高密度に走査させるように制御することもでき、注目領域においてより高精度な動作認識や画像認識を実現可能である。
　これに限らず、ＲＯＩ指定情報は、ＲＯＩ抽出部２２３から渡されてもよい。すなわち、ＲＯＩ抽出部２２３は、３Ｄ／２Ｄ変換部２２２から渡された２Ｄ座標による点群に基づき、イメージセンサ１２０から受信部２００を介して渡された画像データから抽出した注目領域を示す情報を、ＲＯＩ指定情報として制御通信部２３０に渡す。制御通信部２３０は、ＲＯＩ抽出部２２３から渡されたＲＯＩ指定情報を走査制御部２３１に渡す。
（４．本開示に係る実施形態の第２の変形例）
　次に、本開示に係る実施形態の第２の変形例について説明する。実施形態の第２の変形例は、図２に示した実施形態に係る信号処理システム１から、イメージセンサ１２０と画像データに係る処理を行う構成とを省略した例である。
　図２８は、実施形態の第２の変形例に係る信号処理システムの一例の構成を示すブロック図である。図２８において、信号処理システム１ｃは、センサ部１０ａにおいて、図２に示したセンサ部１０からイメージセンサ１２０が省略されている。また、信号処理システム１ｂは、信号処理部２０ｂにおいて、図２に示した信号処理部２０から画像データの処理に係る構成、具体的には、３Ｄ／２Ｄ変換部２２２と、ＲＯＩ抽出部２２３と、合成部２２４と、動作認識部２２５と、画像認識部２２６と、が省略されている。
　この実施形態の第２の変形例に係る信号処理システム１ｃの構成であっても、地図情報の出力と、静止体速度点群および動体速度点群の出力が可能である。動体の動作認識による動作メタ情報や、画像認識による画像メタ情報が不要な場合には、この実施形態の第２の変形例に係る信号処理システム１ｃの構成を適用することで、コストの削減が可能である。
（５．本開示に係る他の実施形態）
　上述した実施形態およびその各変形例では、静止体点群補正部２１３および動体点群補正部２１４は、常にＦＭＣＷ－ＬｉＤＡＲにおけるフレームの全体を補正処理の対象としているが、これはこの例に限定されない。例えば、静止体点群補正部２１３および動体点群補正部２１４による補正を行うか否かを、当該フレームの全体に設定してもよいし、当該フレーム内に設定した１以上の領域に対して設定してもよい。
　また例えば、当該フレームに含まれる点（検出点）ごとに、静止体点群補正部２１３および動体点群補正部２１４による補正を行うか否かを設定してもよい。この場合、補正を行うか否かの点ごとの判定は、当該フレーム内の各点の少なくとも１つの属性情報、或いは、当該フレーム以前のフレーム内の点の属性値に基づき行ってよい。
　このような、静止体点群補正部２１３および動体点群補正部２１４による補正を行うか否かの判定や、補正をどの単位（領域、点ごと、フレーム全体）で行うは、信号処理部２０の外部から指定することができる。例えば、情報処理部３０が信号処理部２０に対して、この指定を行ってよい。また、上述した実施形態の第１の変形例においては、情報処理部３０が信号処理部２０ａにおける制御通信部２３０に対して、この指定を示す指定情報を送信してよい。制御通信部２３０は、この指定情報に基づき、静止体点群補正部２１３および動体点群補正部２１４の動作を制御してよい。
　さらに、上述の実施形態およびその各変形例において、送信部２０１から出力される、静止体速度点群および動体速度点群といった各データに、上述した補正の単位に応じて、補正の有無を示す情報を付加してよい。
　なお、本明細書に記載された効果はあくまで例示であって限定されるものでは無く、また他の効果があってもよい。
　なお、本技術は以下のような構成も取ることができる。
（１）
　第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群データを受信する受信部と、
　前記速度点群データに含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部と、
　前記補正部により補正された前記属性値に対して前記所定の時刻を示す補正時刻情報を付加して送信する送信部と、
を備える、
信号処理装置。
（２）
　前記時刻情報は、前記第１のセンサにおいて前記複数の点それぞれが取得された時刻を示す、
前記（１）に記載の信号処理装置。
（３）
　前記所定の時刻は、前記第１のセンサによる検出動作のフレームごとに少なくとも１つが与えられる、
前記（１）または（２）に記載の信号処理装置。
（４）
　前記受信部は、
　さらに、第２のセンサから慣性計測データを受信し、
　前記補正部は、
　前記推定値を、前記速度情報と、前記時刻情報と、前記慣性計測データと、に基づき算出する、
前記（１）乃至（３）の何れかに記載の信号処理装置。
（５）
　前記補正部は、
　少なくとも、動体による速度点群である動体速度点群を前記補正の対象とする、
前記（１）乃至（４）の何れかに記載の信号処理装置。
（６）
　前記補正部は、
　前記動体速度点群を前記補正部による第１の補正の対象とし、静止体による速度点群である静止体速度点群を前記補正部による第２の補正の対象とする、
前記（５）に記載の信号処理装置。
（７）
　前記補正部により補正された前記静止体速度点群と、第２のセンサから受信した慣性計測データと、に基づき地図情報を生成する地図生成部、
をさらに備える、
前記（６）に記載の信号処理装置。
（８）
　注目領域を抽出する注目領域抽出部、
をさらに備え、
　前記受信部は、
　さらに、第３のセンサから画像データを受信し、
　前記注目領域抽出部は、
　前記補正部により補正された前記動体速度点群に基づき推定される、前記動体が含まれる領域に基づき、前記画像データから前記注目領域を抽出する、
前記（５）乃至（７）の何れかに記載の信号処理装置。
（９）
　前記動体速度点群と、前記画像データのうち前記注目領域の画像データと、を合成した合成データに基づき前記動体の動作を認識する動作認識部、
をさらに備える、
前記（８）に記載の信号処理装置。
（１０）
　前記画像データのうち前記注目領域の画像データに基づき画像認識処理を行う画像認識部、
をさらに備える、
前記（８）または（９）に記載の情報処理装置。
（１１）
　前記受信部は、
　前記第１のセンサから、前記第１のセンサによる検出動作の種別を示す種別情報をさらに受信する、
前記（１）乃至（１０）の何れかに記載の信号処理装置。
（１２）
　プロセッサにより実行される、
　第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群を受信することと、
　前記速度点群に含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正することと、
　前記補正することにより補正された前記属性値に対して前記所定の時刻を示す補正時刻情報を付加して送信することと、
を含む、
信号処理方法。
（１３）
　アプリケーション部からの要求に応じて所定の機能を実行する実行部を備え、
　前記実行部は、
　第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群を受信する受信部と、
　前記速度点群に含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部と、
　前記アプリケーション部からの前記要求を受け取るインタフェース部と、
を含み、
　前記インタフェース部は、
　前記要求に応じて、前記補正部により補正された前記速度点群を前記アプリケーション部に渡す、
情報処理装置。
（１４）
　前記時刻情報は、前記第１のセンサにおいて前記複数の点それぞれが取得された時刻を示す、
前記（１３）に記載の情報処理装置。
（１５）
　前記所定の時刻は、前記第１のセンサによる検出動作のフレームごとに少なくとも１つが与えられる、
前記（１３）または（１４）に記載の情報処理装置。
（１６）
　前記補正部は、
　前記推定値を、前記速度情報と、前記時刻情報と、前記受信部により第２のセンサから受信された慣性計測データと、に基づき算出する、
前記（１３）乃至（１５）の何れかに記載の情報処理装置。
（１７）
　前記補正部は、
　少なくとも、動体による速度点群である動体速度点群を前記補正の対象とする、
前記（１３）乃至（１６）の何れかに記載の情報処理装置。
（１８）
　前記補正部は、
　前記動体速度点群を前記補正部による第１の補正の対象とし、静止体による速度点群である静止体速度点群を前記補正部による第２の補正の対象とする、
前記（１７）に記載の情報処理装置。
（１９）
　所定の処理を実行するアプリケーション部と、
　所定の機能を実行する実行部に対して前記所定の処理に係る要求を渡すインタフェース部と、
を備え、
　前記アプリケーション部は、
　前記要求に応じて前記実行部より渡された、前記実行部が第１のセンサから受信したそれぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群に含まれる、少なくとも１つの点に関する少なくとも１つの属性値が所定の時刻における推定値に基づき補正された前記速度点群を、前記インタフェース部を介して受け取り、受け取った該速度点群に基づき、前記所定の処理を実行する、
情報処理装置。
（２０）
　前記アプリケーション部は、
　それぞれ前記インタフェース部を介して受け取った、前記推定値に基づき補正された静止体による速度点群である静止体速度点群と、前記実行部が第２のセンサから受信した慣性計測データと、に基づき地図情報を生成する地図生成部、
を含む、
前記（１９）に記載の情報処理装置。
（２１）
　前記アプリケーション部は、
　前記インタフェース部を介して受け取った、前記実行部が第３のセンサから受信した画像データに対する注目領域を、前記インタフェース部を介して受け取った、前記推定値に基づき補正された動体による速度点群である動体速度点群に基づき推定される、前記動体が含まれる領域に基づき抽出する注目領域抽出部、
を含む、
前記（１９）または（２０）に記載の情報処理装置。
（２２）
　前記アプリケーション部は、
　前記動体速度点群と、前記画像データのうち前記注目領域の画像データと、を合成した合成データに基づき前記動体の動作を認識する動作認識部、
を含む、
前記（２１）に記載の情報処理装置。
（２３）
　前記アプリケーション部は、
　前記画像データのうち前記注目領域の画像データに基づき画像認識処理を行う画像認識部、
を含む、
前記（２１）または（２２）に記載の情報処理装置。
１，１ａ，１ｂ，１ｃ　信号処理システム
１０，１０ａ　センサ部
２０，２０ａ，２０ｂ　信号処理部
３０　情報処理部
４０　走査線
４１，４１ａ，４１ｂ，４１st　射出ポイント
４２1，４２2，４２N　走査線
４５　角度範囲
４６　走査範囲
５０ａ，５０ｂ，５２　物体
６０ａ，６０ｂ，６０ｃ　フレーム
６１　画素
１００　ＩＭＵ
１１０　光検出測距センサ
１１１　光走査部
１１２　光送受信部
１１３　受信信号処理部
１１４　光走査制御部
１１５，２０１　送信部
１２０　イメージセンサ
１３０　同期信号生成部
２００　受信部
２１０　センサ位置姿勢推定部
２１１　動体・静止体分離部
２１２　センサ速度推定部
２１３　静止体点群補正部
２１４　動体点群補正部
２２０　地図変換部
２２１　動体状態推定部
２２２　３Ｄ／２Ｄ変換部
２２３　ＲＯＩ抽出部
２２４　合成部
２２５　動作認識部
２２６　画像認識部
２３０　制御通信部
２３１　走査制御部
２３２　パラメータ設定部
４００　センサ位置姿勢推定処理
４０１　静止体動体弁別処理
４０２　センサ速度推定処理
４０３，４０４　点群補正処理
４０５　動体検出処理
１０００　センサ群
１０１０　ファームウェア
２０００，３０００　プロセッサ
２０１０，３０１０　ライブラリ部
２０２０，３０２０　アプリケーション部
２０３０　ＯＳ
２０４０　ＡＰＩ呼出部
２０４１　ＡＰＩ処理部

Claims (20)

  1. 　第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群データを受信する受信部と、
    　前記速度点群データに含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部と、
    　前記補正部により補正された前記属性値に対して前記所定の時刻を示す補正時刻情報を付加して送信する送信部と、
    を備える、
    信号処理装置。
  2. 　前記時刻情報は、前記第１のセンサにおいて前記複数の点それぞれが取得された時刻を示す、
    請求項１に記載の信号処理装置。
  3. 　前記所定の時刻は、前記第１のセンサによる検出動作のフレームごとに少なくとも１つが与えられる、
    請求項１に記載の信号処理装置。
  4. 　前記受信部は、
    　さらに、第２のセンサから慣性計測データを受信し、
    　前記補正部は、
    　前記推定値を、前記速度情報と、前記時刻情報と、前記慣性計測データと、に基づき算出する、
    請求項１に記載の信号処理装置。
  5. 　前記補正部は、
    　少なくとも、動体による速度点群である動体速度点群を前記補正の対象とする、
    請求項１に記載の信号処理装置。
  6. 　前記補正部は、
    　前記動体速度点群を前記補正部による第１の補正の対象とし、静止体による速度点群である静止体速度点群を前記補正部による第２の補正の対象とする、
    請求項５に記載の信号処理装置。
  7. 　前記補正部により補正された前記静止体速度点群と、第２のセンサから受信した慣性計測データと、に基づき地図情報を生成する地図生成部、
    をさらに備える、
    請求項６に記載の信号処理装置。
  8. 　注目領域を抽出する注目領域抽出部、
    をさらに備え、
    　前記受信部は、
    　さらに、第３のセンサから画像データを受信し、
    　前記注目領域抽出部は、
    　前記補正部により補正された前記動体速度点群に基づき推定される、前記動体が含まれる領域に基づき、前記画像データから前記注目領域を抽出する、
    請求項５に記載の信号処理装置。
  9. 　前記動体速度点群と、前記画像データのうち前記注目領域の画像データと、を合成した合成データに基づき前記動体の動作を認識する動作認識部、
    をさらに備える、
    請求項８に記載の信号処理装置。
  10. 　前記受信部は、
    　前記第１のセンサから、前記第１のセンサによる検出動作の種別を示す種別情報をさらに受信する、
    請求項１に記載の信号処理装置。
  11. 　プロセッサにより実行される、
    　第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群を受信することと、
    　前記速度点群に含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正することと、
    　前記補正することにより補正された前記属性値に対して前記所定の時刻を示す補正時刻情報を付加して送信することと、
    を含む、
    信号処理方法。
  12. 　アプリケーション部からの要求に応じて所定の機能を実行する実行部を備え、
    　前記実行部は、
    　第１のセンサから、それぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群を受信する受信部と、
    　前記速度点群に含まれる少なくとも１つの点に関する少なくとも１つの属性値を、所定の時刻における推定値に基づき補正する補正部と、
    　前記アプリケーション部からの前記要求を受け取るインタフェース部と、
    を含み、
    　前記インタフェース部は、
    　前記要求に応じて、前記補正部により補正された前記速度点群を前記アプリケーション部に渡す、
    情報処理装置。
  13. 　前記時刻情報は、前記第１のセンサにおいて前記複数の点それぞれが取得された時刻を示す、
    請求項１２に記載の情報処理装置。
  14. 　前記所定の時刻は、前記第１のセンサによる検出動作のフレームごとに少なくとも１つが与えられる、
    請求項１２に記載の情報処理装置。
  15. 　前記補正部は、
    　前記推定値を、前記速度情報と、前記時刻情報と、前記受信部により第２のセンサから受信された慣性計測データと、に基づき算出する、
    請求項１２に記載の情報処理装置。
  16. 　前記補正部は、
    　少なくとも、動体による速度点群である動体速度点群を前記補正の対象とする、
    請求項１２に記載の情報処理装置。
  17. 　所定の処理を実行するアプリケーション部と、
    　所定の機能を実行する実行部に対して前記所定の処理に係る要求を渡すインタフェース部と、
    を備え、
    　前記アプリケーション部は、
    　前記要求に応じて前記実行部より渡された、前記実行部が第１のセンサから受信したそれぞれ速度情報と時刻情報とを持つ複数の点を含む速度点群に含まれる、少なくとも１つの点に関する少なくとも１つの属性値が所定の時刻における推定値に基づき補正された前記速度点群を、前記インタフェース部を介して受け取り、受け取った該速度点群に基づき、前記所定の処理を実行する、
    情報処理装置。
  18. 　前記アプリケーション部は、
    　それぞれ前記インタフェース部を介して受け取った、前記推定値に基づき補正された静止体による速度点群である静止体速度点群と、前記実行部が第２のセンサから受信した慣性計測データと、に基づき地図情報を生成する地図生成部、
    を含む、
    請求項１７に記載の情報処理装置。
  19. 　前記アプリケーション部は、
    　前記インタフェース部を介して受け取った、前記実行部が第３のセンサから受信した画像データに対する注目領域を、前記インタフェース部を介して受け取った、前記推定値に基づき補正された動体による速度点群である動体速度点群に基づき推定される、前記動体が含まれる領域に基づき抽出する注目領域抽出部、
    を含む、
    請求項１７に記載の情報処理装置。
  20. 　前記アプリケーション部は、
    　前記動体速度点群と、前記画像データのうち前記注目領域の画像データと、を合成した合成データに基づき前記動体の動作を認識する動作認識部、
    を含む、
    請求項１９に記載の情報処理装置。
PCT/JP2023/026725 2022-07-29 2023-07-21 信号処理装置および信号処理方法、ならびに、情報処理装置 Ceased WO2024024656A1 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
US18/878,655 US20260003075A1 (en) 2022-07-29 2023-07-21 Signal processing apparatus, signal processing method, and information processing apparatus
DE112023003273.4T DE112023003273T5 (de) 2022-07-29 2023-07-21 Signalverarbeitungsvorrichtung, signalverarbeitungsverfahren und informationsverarbeitungsvorrichtung
JP2024537663A JPWO2024024656A1 (ja) 2022-07-29 2023-07-21
CN202380055837.3A CN119630988A (zh) 2022-07-29 2023-07-21 信号处理装置、信号处理方法和信息处理装置

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022122136 2022-07-29
JP2022-122136 2022-07-29

Publications (1)

Publication Number Publication Date
WO2024024656A1 true WO2024024656A1 (ja) 2024-02-01

Family

ID=89706479

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2023/026725 Ceased WO2024024656A1 (ja) 2022-07-29 2023-07-21 信号処理装置および信号処理方法、ならびに、情報処理装置

Country Status (5)

Country Link
US (1) US20260003075A1 (ja)
JP (1) JPWO2024024656A1 (ja)
CN (1) CN119630988A (ja)
DE (1) DE112023003273T5 (ja)
WO (1) WO2024024656A1 (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2026084808A1 (en) * 2024-10-18 2026-04-23 Qualcomm Incorporated Velocity estimation distortion correction

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2016138878A (ja) * 2009-02-20 2016-08-04 デジタル・シグナル・コーポレーション ライダーとビデオ測定を使用する３次元画像の生成システム及び方法
US20200292706A1 (en) * 2019-03-14 2020-09-17 Aeva, Inc. Velocity estimation using doppler per point lidar systems
JP2022510198A (ja) * 2018-11-29 2022-01-26 エスゼット ディージェイアイ テクノロジー カンパニー リミテッド 移動プラットフォームの周囲の環境を検出するシステム及びその方法
WO2022123887A1 (ja) * 2020-12-11 2022-06-16 パナソニックＩｐマネジメント株式会社 測距装置および移動体

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2016138878A (ja) * 2009-02-20 2016-08-04 デジタル・シグナル・コーポレーション ライダーとビデオ測定を使用する３次元画像の生成システム及び方法
JP2022510198A (ja) * 2018-11-29 2022-01-26 エスゼット ディージェイアイ テクノロジー カンパニー リミテッド 移動プラットフォームの周囲の環境を検出するシステム及びその方法
US20200292706A1 (en) * 2019-03-14 2020-09-17 Aeva, Inc. Velocity estimation using doppler per point lidar systems
WO2022123887A1 (ja) * 2020-12-11 2022-06-16 パナソニックＩｐマネジメント株式会社 測距装置および移動体

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2026084808A1 (en) * 2024-10-18 2026-04-23 Qualcomm Incorporated Velocity estimation distortion correction

Also Published As

Publication number Publication date
JPWO2024024656A1 (ja) 2024-02-01
DE112023003273T5 (de) 2025-09-04
US20260003075A1 (en) 2026-01-01
CN119630988A (zh) 2025-03-14

Similar Documents

Publication Publication Date Title
US11841431B2 (en) System and method for increasing resolution of images obtained from a three-dimensional measurement system
US9435891B2 (en) Time of flight camera with stripe illumination
CN108020825B (zh) 激光雷达、激光摄像头、视频摄像头的融合标定系统及方法
JP2016138878A (ja) ライダーとビデオ測定を使用する３次元画像の生成システム及び方法
US20190266425A1 (en) Identification apparatus, identification method, and non-transitory tangible recording medium storing identification program
JP4193886B2 (ja) 画像表示装置
EP2890952A2 (en) System and method for refining coordinate-based three-dimensional images obtained from a three-dimensional measurement system
KR102240817B1 (ko) 티오에프 카메라에서 깊이 지도 생성 방법
JP2012518793A5 (ja)
WO2022195954A1 (ja) センシングシステム
US20240134053A1 (en) Time-of-flight data generation circuitry and time-of-flight data generation method
JP2018063222A (ja) 距離測定装置、距離測定方法及びプログラム
US20260003075A1 (en) Signal processing apparatus, signal processing method, and information processing apparatus
JP2023046746A (ja) 情報処理装置、制御方法、およびプログラム
US20230396746A1 (en) Image projection system and image projection method
JP7173342B2 (ja) レーダ装置、イメージング方法およびイメージングプログラム
WO2024024663A1 (ja) 認識システム、認識方法および認識装置
WO2021070814A1 (ja) 同期装置、同期方法、同期プログラム
US20250224516A1 (en) Information processing apparatus, system, information processing method, information processing program, and computer system
EP4094096B1 (en) A method and system for generating a colored tridimensional map
JP5803534B2 (ja) 光通信装置及びプログラム
JP2010157910A (ja) 画像表示装置
JP2019007921A (ja) 情報出力システム、情報出力装置、およびプログラム
KR20230156697A (ko) 정보 처리 장치 및 정보 처리 방법, 그리고 센싱 시스템

Legal Events

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

Ref document number: 23846387

Country of ref document: EP

Kind code of ref document: A1

WWE Wipo information: entry into national phase

Ref document number: 2024537663

Country of ref document: JP

WWE Wipo information: entry into national phase

Ref document number: 18878655

Country of ref document: US

WWE Wipo information: entry into national phase

Ref document number: 202380055837.3

Country of ref document: CN

WWE Wipo information: entry into national phase

Ref document number: 112023003273

Country of ref document: DE

WWP Wipo information: published in national office

Ref document number: 202380055837.3

Country of ref document: CN

122 Ep: pct application non-entry in european phase

Ref document number: 23846387

Country of ref document: EP

Kind code of ref document: A1

WWP Wipo information: published in national office

Ref document number: 112023003273

Country of ref document: DE

WWP Wipo information: published in national office

Ref document number: 18878655

Country of ref document: US