Identification system, identification method and identification device

Abstract

The purpose of the present invention is to provide a recognition system capable of more accurately acquiring the position and orientation of a moving body and the surrounding environment. An identification system (1a) according to the present disclosure includes: a sensor unit (10a) including: a first sensor (a light detection distance measurement sensor (110) to which FMCW-LiDAR is applied) for outputting velocity point cloud data including a plurality of points each having velocity information and time information; and a second sensor (IMU 100) for outputting inertial measurement data; a receiving unit (200) for receiving the velocity point cloud data and the inertial measurement data output from the sensor unit (10a); and an output unit (201) for outputting sensor state information including information indicating the position and orientation of the sensor unit (10a), and moving body state information including information indicating the position, orientation, and speed of the moving body, the sensor state information and the moving body state information are generated based on velocity point cloud data and inertial measurement data.

Description

Technical Field

[0001] The present disclosure relates to an identification system, an identification method and an identification device. Background Art

[0002] In order to realize the autonomous operation of a mobile body (such as a robot), it is necessary to perform both the estimation of its own position and the recognition of the surrounding environment. Therefore, the autonomous operation of a mobile body requires external environmental sensors that recognize the surrounding environment and internal environmental sensors that recognize the self-motion.

[0003] Commonly used internal environment sensors for measuring self-motion include inertial sensors and wheel encoders for odometers. In addition, external environment sensors commonly used to identify the surrounding environment include laser imaging detection and ranging (LiDAR) systems and cameras. By performing appropriate data processing, LiDAR capable of detecting Doppler velocity can also be used as a sensor for mileage measurement. For example, Patent Documents 1 and 2 disclose a technology for detecting the surrounding environment for self-position estimation using a laser imaging detection and ranging (LiDAR) system capable of detecting Doppler velocity.

[0004] Reference List

[0005] Patent Literature

[0006] Patent document 1: US2020 / 0292706 A

[0007] Patent document 2: US2020 / 0400821 A

[0008] Patent Document 3: WO 2019 / 239566 A Summary of the invention

[0009] Technical issues

[0010] Meanwhile, there are conditions where it is difficult to obtain information related to the movement of the moving body from the surrounding environment acquired by the external environment sensor mounted on the moving body. For example, the conditions include a case where the surrounding environment structure is a simple structure in the moving direction, a case where the features in the environment structure included in the field of view of the sensor are insufficient (degeneracy condition), or a case where a large number of moving objects exist around the moving body.

[0011] Under such surrounding environmental conditions, it is difficult to use external environment sensors (such as LiDAR or a camera installed on a moving object) to detect and identify itself and multiple surrounding moving objects with high accuracy in order to grasp the position and orientation of itself and the surrounding moving objects.

[0012] The object of the present disclosure is to provide a recognition system, a recognition method, and a recognition device that can obtain the position and orientation of a moving object and the surrounding environment with higher accuracy.

[0013] Solution to the problem

[0014] In order to solve the above problems, an identification system according to one aspect of the present disclosure has: a sensor unit, the sensor unit includes a first sensor and a second sensor, the first sensor is configured to output speed point cloud data including a plurality of points, each point has speed information and time point information, and the second sensor is configured to output inertial measurement data; a receiving unit, configured to receive the speed point cloud data and inertial measurement data output from the sensor unit; and an output unit, configured to output sensor status information and moving object status information, the sensor status information includes information indicating the position and orientation of the sensor unit, the moving object status information includes information indicating the position, orientation and speed of the moving object, the sensor status information and the moving object status information are information generated based on the speed point cloud data and the inertial measurement data. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] [ Figure 1 ] is a block diagram schematically showing a configuration example of a signal processing system according to an embodiment.

[0016] [ Figure 2 ] is a block diagram showing the configuration of an example of an identification system according to the related art.

[0017] [ Figure 3 ] is a block diagram showing the configuration of an example of a light detection and ranging sensor according to the related art.

[0018] [ Figure 4 ] is a block diagram illustrating the configuration of an example of a SLAM processing unit according to the prior art.

[0019] [ Figure 5 ] is a schematic diagram used to describe the situation where degradation or dynamic environment changes occur in point cloud scan matching according to the prior art.

[0020] [ Figure 6 ] is a schematic diagram showing an example of a static environment in which the environment does not include moving objects.

[0021] [ Fig. 7A ] is a schematic diagram showing an example of a dynamic environment in which the environment includes a moving object.

[0022] [ Figure 7B ] is a schematic diagram showing an example of a dynamic environment in which the environment includes multiple moving objects.

[0023] [ Fig. 8A] is a schematic diagram for illustrating scan matching according to the prior art when the environment does not include moving objects.

[0024] [ Figure 8B ] is a schematic diagram for illustrating scan matching according to the prior art when the environment includes moving objects.

[0025] [ Fig. 9 ] is a schematic diagram schematically illustrating a speed measurement technology that can be applied to a moving object.

[0026] [ Fig.10 ] is a schematic diagram showing features of examples of a laser Doppler speedometer, a Doppler radar, an IMU, a wheel sensor, an optical flow sensor, and a FMCW-LiDAR sensor.

[0027] [ Fig.11 ] is a block diagram showing a basic configuration example of a recognition system according to each embodiment of the present disclosure.

[0028] [ Fig.12 ] is a block diagram showing an example configuration of a light detection and ranging sensor according to various embodiments.

[0029] [ Fig.13 ] is a schematic diagram schematically illustrating an example of a scanning pattern of a transmission optical signal according to an embodiment.

[0030] [ Fig.14A ] is a schematic diagram schematically illustrating another example of a scanning pattern of a transmitted light signal.

[0031] [ Fig. 14B ] is a schematic diagram schematically illustrating another example of the scanning pattern of the transmitted light signal.

[0032] [ Fig.15 ] is a schematic diagram for describing situations in which degradation or dynamic environmental changes occur in point cloud scan matching according to various embodiments.

[0033] [ Fig.16A ] is a schematic diagram showing an example of self-movement and an environment including multiple moving objects according to an embodiment.

[0034] [ Fig. 16B ] is a schematic diagram for illustrating motion estimation of a moving object according to an embodiment.

[0035] [ Fig.17 ] is a block diagram showing the configuration of an example of the recognition system according to the first embodiment.

[0036] [ Fig.18 ] is a schematic diagram schematically illustrating a velocity point cloud correction process according to an embodiment.

[0037] [ Fig.19A ] is a block diagram showing a hardware configuration of an example of the recognition system applicable to the first embodiment.

[0038] [ Fig.19B ] is a schematic diagram showing an example in which Ethernet, which can be applied to the first embodiment, is used as an interface related to data transmission between units.

[0039] [ Fig. 20 ] is a diagram showing an example of the architecture of a signal processing unit according to the first embodiment.

[0040] [ Fig.21 ] is a flowchart showing an example of transmission light signal detection processing in the light detection and ranging sensor applicable to the first embodiment.

[0041] [ Fig. 22 ] is a schematic diagram showing a transmission optical signal transmitted by an optical transmission / reception unit which can be applied to the first embodiment.

[0042] [ Fig.23 ] is a schematic diagram for illustrating a determination process of a received light signal according to the first embodiment.

[0043] [ Fig.24 ] is a schematic diagram showing an example of data output in a case where the light detection and ranging sensor according to the first embodiment performs scanning in a raster scanning mode.

[0044] [ Fig.25 ] is a schematic diagram for illustrating a frame definition that can be applied to an implementation manner.

[0045] [ Fig.26A ] is a schematic diagram for illustrating the output timing of each data performed by the sensor unit according to the first embodiment.

[0046] [ Fig.26B ] is a schematic diagram for illustrating the output timing of each data performed by the sensor unit according to the first embodiment.

[0047] [ Fig. 27 ] is a schematic diagram showing an example of the output rate of each data according to the first embodiment.

[0048] [ Fig.28 ] 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 first embodiment.

[0049] [ Fig.29 ] is a flowchart showing an example of processing performed by a signal processing unit according to the first embodiment.

[0050] [ Fig.30 ] is a flowchart showing an example of processing related to point cloud correction in a signal processing unit according to the first embodiment.

[0051] [ Fig.31 ] is a flowchart showing an example of correction processing of a velocity point cloud frame of a stationary object in the signal processing unit of the first embodiment.

[0052] [ Fig.32 ] is a schematic diagram showing an example of output data of the light detection and ranging sensor according to the first embodiment.

[0053] [ Fig.33 ] is a schematic diagram showing the definition of the coordinates of the emission point applicable to the first embodiment.

[0054] [ Fig.34 ] is a schematic diagram showing each emission point and object as viewed from the light detection and ranging sensor.

[0055] [ Fig.35 ] is a schematic diagram showing the overall flow of processing related to point cloud correction in the signal processing unit according to the present embodiment.

[0056] [ Fig.36 ] is a schematic diagram used to define the coordinate system and each variable.

[0057] [ Fig.37 ] is a block diagram showing the configuration of an example of a recognition system according to a first modification example of the first embodiment.

[0058] [ Fig.38 ] is a block diagram showing a configuration of an example of a recognition system according to a second modification example of the first embodiment.

[0059] [ Fig.39 ] is a block diagram showing the configuration of an example of a recognition system according to a third modification example of the first embodiment.

[0060] [ Fig.40 ] is a block diagram showing the configuration of an example of a recognition system according to the second embodiment.

[0061] [ Fig.41 ] is a flowchart showing an example of processing performed by a signal processing unit according to the second embodiment.

[0062] [ Fig.42 ] is a block diagram showing the configuration of an example of a recognition system according to the third embodiment.

[0063] [ Fig.43] is a flowchart showing an example of processing by a signal processing unit according to the third embodiment. DETAILED DESCRIPTION

[0064] Embodiments of the present disclosure will be described in detail below with reference to the accompanying drawings. In each of the following embodiments, the same components are denoted by the same reference symbols, and repeated descriptions thereof will be omitted.

[0065] Hereinafter, embodiments of the present disclosure will be described in the following order.

[0066] 1. Overview of the technology according to the present disclosure

[0067] 1-1. Comparison with existing technologies

[0068] 1-1-1. Configuration example of prior art

[0069] 1-1-2. Overview of processing using existing technology

[0070] 1-2. Speed ​​measurement technology for moving objects

[0071] 1-3. Specific Examples of Technology According to Embodiments of the Present Disclosure

[0072] 1-3-1. Basic system configuration according to each embodiment of the present disclosure

[0073] 1-3-2. Overview of Processing According to Each Embodiment of the Present Disclosure

[0074] 2. According to the first embodiment of the present disclosure

[0075] 2-1. Specific example of configuration according to first embodiment

[0076] 2-1-1.Signal processing unit

[0077] 2-1-1-1. Library

[0078] 2-1-1-2. Application part

[0079] 2-1-2. System Configuration

[0080] 2-2. Processing according to the first embodiment

[0081] 2-2-1. Measurement technology applicable to the first embodiment

[0082] 2-2-2. Data Structure Example According to the First Embodiment

[0083] 2-2-3. Details of processing according to the first embodiment

[0084] 2-2-4. Point Cloud Correction Processing According to the First Embodiment

[0085] 2-2-5. Effects of the First Embodiment

[0086] 2-3. First Modification of the First Embodiment

[0087] 2-4. Second Modification of First Embodiment

[0088] 2-5. Third Modification of First Embodiment

[0089] 3. According to the second embodiment of the present disclosure

[0090] 4. According to the third embodiment of the present disclosure

[0091] (1. Overview of Technology According to the Present Disclosure)

[0092] First, the technology according to the present disclosure will be schematically described.

[0093] Figure 1 Schematically shows a block diagram of a configuration example of an identification system according to an embodiment. Figure 1 In the embodiment, the recognition system 1 includes a sensor unit 10 , a signal processing unit 20 and an information processing unit 30 .

[0094] The sensor unit 10 includes an inertial measurement unit (IMU) 100, a light detection and ranging sensor 110, and an image sensor 120. The IMU 100 includes, for example, a three-axis acceleration sensor, a three-axis angular velocity sensor, and a three-axis geomagnetic sensor, and outputs inertial measurement data obtained by these sensors (hereinafter, appropriately referred to as IMU data). The light detection and ranging sensor 110 is a sensor that performs ranging using light, and an example adopts frequency modulated continuous wave-laser imaging detection and ranging (FMCW-LiDAR), which is a method of performing ranging using frequency modulated continuous laser. The image sensor 120 is typically a camera, images an object, and outputs image data including information such as red (R), green (G), and blue (B). The image sensor 120 may be omitted.

[0095] exist Figure 1In the example, the signal processing unit 20 receives input of data, including: IMU data output from the IMU 100 of the sensor unit 10; FMCW-LiDAR data output from the light detection and ranging sensor 110; and image data output from the image sensor 120. 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 performs signal processing based on each data and generates, for example, map information, a velocity point cloud of each stationary object and moving object, motion meta information indicating the motion of the moving object, and image meta information related to the image in the region of interest. The signal processing unit 20 outputs the generated information to the information processing unit 30.

[0096] The information processing unit 30 performs predetermined processing according to each piece of information output from the signal processing unit 20. The processing performed by the information processing unit 30 according to each piece of information output from the signal processing unit 20 is not particularly limited. For example, in the case where the recognition system 1 is applied to a robot (moving body) that operates autonomously, the information processing unit 30 can perform drive control of the robot based on map information, etc. In addition, for example, in the case where the recognition system 1 is applied to a control system of a surveillance camera, the information processing unit 30 can perform notification, etc. based on motion meta information or image meta information.

[0097] In addition, the information processing unit 30 may send an instruction related to signal processing to the signal processing unit 20. The signal processing unit 20 may control processing for the output of the sensor unit 10 according to the instruction. In addition, the signal processing unit 20 may control the operation of the sensor (IMU 100, light detection and ranging sensor 110, and image sensor 120) in the sensor unit 10 according to this instruction. It is to be noted that the information processing unit 30 may be realized by applying a configuration as a typical computer including a central processing unit (CPU), a memory, a storage device, and the like.

[0098] In this way, the recognition system 1 according to the present embodiment can output various data based on the measurement result of the FMCW-LiDAR, so that the measurement result of the FMCW-LiDAR can be further generally utilized.

[0099] (1-1. Comparison with prior art)

[0100] Here, for ease of understanding, a measurement process according to the related art will be schematically described.

[0101] (1-1-1. Configuration example of prior art)

[0102] Figure 2 is a block diagram showing a configuration of an example of a recognition system according to the related art.

[0103] exist Figure 2 In the embodiment, the recognition system 500 includes a sensor unit 510, a signal processing unit 520 and an information processing unit 530. The sensor unit 510, the signal processing unit 520 and the information processing unit 530 each correspond to a reference Figure 1 The sensor unit 10, the signal processing unit 20 and the information processing unit 30 are described.

[0104] The sensor unit 510 includes an IMU 5100, a light detection and ranging sensor 5110, and a synchronization signal generator 5130. The IMU 5100 is equivalent to the above-mentioned IMU 100, and includes a three-axis acceleration sensor, a three-axis angular velocity sensor, and a three-axis geomagnetic sensor, and outputs sensor data obtained by each of these sensors as IMU data.

[0105] The synchronization signal generator 5130 generates a synchronization signal. The synchronization signal may be a pulse of a predetermined period. The synchronization signal generator 5130 supplies the generated synchronization signal to the IMU 5100 and the light detection and ranging sensor 5110. The operation and data output timing of each of the IMU 5100 and the light detection and ranging sensor 5110 are controlled in synchronization with the provided synchronization signal.

[0106] The light detection and ranging sensor 5110 is a sensor that performs ranging using light, and in the prior art, laser imaging detection and ranging (LiDAR) that performs ranging using laser is applied. For example, with LiDAR, a pulsed laser is scanned and emitted within a predetermined angle range, and ranging is performed based on the difference between the reception timing of the reflected light and the transmission timing of the emitted light. For example, in LiDAR, a ranging result can be obtained as a point cloud including points each having three-dimensional coordinates. The light detection and ranging sensor 5110 outputs a ranging result including a point cloud as LiDAR data.

[0107] Figure 3 is a block diagram showing the configuration of an example of a light detection and ranging sensor 5110 according to the prior art. Figure 3 , the light detection and ranging sensor 5110 includes an optical scanning unit 5111, an optical transmission / reception unit 5112, a reception signal processing unit 5113, an optical scanning controller 5114, and a transmission unit 5115.

[0108] The optical transmission / reception unit 5112 includes: an optical transmission unit, which generates a transmission light signal transmitted from the optical scanning unit 5111 described below; a transmission light controller, which controls the transmission light generated by the optical transmission unit; and a light receiving unit, which receives (receives) the reception light signal from the optical scanning unit 5111.

[0109] In the optical transmission / reception unit 5112, the optical transmission unit includes: a light source, for example, a laser diode for performing oscillation of laser light as transmission light; an optical system for emitting light oscillated by the light source; and a laser output modulator, for example, for driving the light source. The optical transmission unit uses the light source to perform optical oscillation and emits a transmission light signal according to the optical transmission control signal supplied from the transmission light controller. The transmission light signal is sent to the optical scanning unit 5111.

[0110] In the optical transmission unit, for example, the transmission light controller generates a pulse signal using pulses at a predetermined time interval. The transmission light controller generates a light emission control signal as a modulation synchronization timing signal based on the pulse signal, and the modulation synchronization timing signal is input to the laser output modulator included in the optical transmission unit. The transmission light controller generates a transmission control signal as a signal synchronized with the synchronization signal supplied from the synchronization signal generation unit 130. The transmission light controller transmits the generated optical transmission control signal to the optical transmission unit and the reception signal processing unit 5113 to be described below.

[0111] In the optical transmission / reception unit 5112, the optical receiving unit includes: for example, an optical receiver that receives a received light signal (performs optical reception) from the optical scanning unit 5111; and a driving circuit that drives the above-mentioned optical receiving unit. For example, the optical receiver can be implemented by applying a configuration that combines a focusing lens and a light receiving element (such as a photodiode). The optical receiving unit also includes an optical combiner that combines the received light received from the scanning unit with the local oscillator light sent from the optical transmission unit. When the received light is a reflected light from the target of the transmitted light, the received light will be a signal delayed from the local oscillator light according to the distance to the target. The optical transmission / reception unit 5112 transmits a signal corresponding to the received light as a received waveform signal to the received signal processing unit 5113.

[0112] The reception signal processing unit 5113 performs predetermined signal processing on the reception waveform signal delivered from the optical transmission / reception unit 5112, the processing being performed in synchronization with the synchronization signal provided from the synchronization signal generator 130, and acquires the distance to the object and the intensity of the received light signal. The reception signal processing unit 5113 adds a timestamp generated in synchronization with the synchronization signal to the acquired distance and speed, and delivers the data with the timestamp to the transmission unit 5115. Here, the timestamp is a timestamp related to the measurement, which indicates the timing at which the transmission light signal has been transmitted by the optical transmission / reception unit 5112, and is generated and added for each measurement.

[0113] The optical scanning controller 5114 generates a scanning control signal for controlling scanning of the transmission light signal in the optical scanning unit 5111. At this time, the optical scanning controller 5114 generates the scanning control signal so that the scanning of the transmission light signal is synchronized with the synchronization signal provided from the synchronization signal generator 130. The optical scanning controller 5114 may generate the scanning control signal for performing scanning within a predetermined scanning range, or may generate the scanning control signal according to scanning control information transmitted from the control communication unit 230 described below.

[0114] Furthermore, the optical scanning controller 5114 receives an angle detection signal indicating a scanning angle of the transmission light signal from the optical scanning unit 5111. The optical scanning controller 5114 transfers information indicating the scanning angle to the transmission unit 5115 based on the received angle detection signal.

[0115] The optical scanning unit 5111 transmits the transmission light signal sent from the optical transmission / reception unit 5112, the transmission is performed at an angle according to the scanning pattern corresponding to the scanning control signal provided from the optical scanning controller 5114, and receives the light incident at the angle, and outputs the light as a received light signal. In the optical scanning unit 5111, for example, a scanning mechanism of the transmission light signal can be realized by applying a biaxial mirror scanner. In this case, the scanning control signal is, for example, a driving voltage signal applied to each axis of the biaxial mirror scanner.

[0116] Furthermore, the optical scanning unit 5111 detects the angles of the transmission light signal in the horizontal direction and the vertical direction, and outputs an angle detection signal indicating the detected angles.

[0117] The transmission unit 5115 transmits the point cloud based on the distance and intensity transmitted from the reception signal processing unit 5113 and the scanning angle transmitted from the optical scanning controller 5114 to the signal processing unit 520 as LiDAR data. At this time, the transmission unit 5115 adds the time stamp transmitted from the reception signal processing unit 5113 to each point of the point cloud, and sends the data with the time stamp to the signal processing unit 520.

[0118] exist Figure 2 In the embodiment, the signal processing unit 520 includes a receiving unit 5200 , a sensor position / orientation estimation unit 5210 , a SLAM processing unit 5220 and a transmission unit 5201 .

[0119] The receiving unit 5200 receives the IMU data and the LiDAR data transmitted from the sensor unit 510. The receiving unit 5200 transmits the received IMU data to the sensor position / orientation estimating unit 5210, and transmits the LiDAR data to the SLAM processing unit 5220.

[0120] The sensor position / orientation estimation unit 5210 estimates the position, orientation, and angular velocity of the IMU 5100 (sensor unit 510) based on the IMU data transmitted from the receiving unit 5200. The sensor position / orientation estimation unit 5210 transmits the estimated position, orientation, and angular velocity to the SLAM processing unit 5220.

[0121] The SLAM processing unit 5220 creates map information using simultaneous localization and mapping (SLAM) technology based on the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimation unit 5210 and the LiDAR data transmitted from the receiving unit 5200 (ie, point cloud).

[0122] SLAM is a technology that performs positioning (estimation of own position) and creation of environment map at the same time. In SLAM, the own position is estimated using the data of internal environment sensors (e.g., IMU data, or odometry data obtained by, for example, wheel encoders that measure the rotation of the wheels of the mobile body). For example, in SLAM, the surrounding environment is detected by external environment sensors such as LiDAR so as to simultaneously perform the update of the own position in the environment map and the update of the environment map.

[0123] Figure 4 5 is a block diagram illustrating an example configuration of a SLAM processing unit 5220 according to the prior art. Figure 4 In the embodiment, the SLAM processing unit 5220 includes a scan matching processing unit 5222, a map updating unit 5223, and a map storage unit 5224.

[0124] In the SLAM processing unit 5220, for example, the map updating unit 5223 creates a reference map using the point cloud at the initial sensor position and orientation in the initial state. The map updating unit 5223 may use, for example, an occupancy grid map that divides the space and maintains the occupancy of each grid. The map updating unit 5223 generates a map by calculating the spatial occupancy of each grid point based on the lines connecting each detection point of the point cloud transmitted from the receiving unit 5200 via the scan matching processing unit 5222 and the map.

[0125] The scan matching processing unit 5222 performs point cloud scan matching based on the map generated last time and stored in the map storage unit 5224 and the current point cloud transmitted from the receiving unit 5200, and takes the last sensor position orientation estimated by the sensor position orientation estimation unit 5210 as a reference, and searches for the sensor position orientation that best matches the map and the point cloud. For example, the scan matching processing unit 5222 may apply a matching processing algorithm using iterative closest point (ICP) to matching processing using point cloud scan matching.

[0126] The scan matching processing unit 5222 passes the sensor position and orientation updated by the search to the map updating unit 5223, and outputs the updated sensor position and orientation together with this from the SLAM processing unit 5220. In addition, the scan matching processing unit 5222 passes the updated sensor position / orientation to the sensor position / orientation estimating unit 5210.

[0127] The map updating unit 5223 integrates and connects the point cloud transmitted from the receiving unit 5200 via the scan matching processing unit 5222 with the map to create an environment map based on the updated sensor position and orientation transmitted from the scan matching processing unit 5222. The map updating unit 5223 outputs map information indicating the created environment map from the SLAM processing unit 5220, and stores the map information in the map storage unit 5224 together with the output.

[0128] exist Figure 2 , the transmission unit 5201 transmits the information output from the SLAM processing unit 5220 (specifically, information indicating the sensor position and orientation, information indicating the sensor speed and angular velocity, and map information) to the information processing unit 530. At this time, for example, the transmission unit 5201 may add a timestamp indicating a time point at which measurement of the light detection and ranging sensor 5110 is performed to each piece of information, and pass the information to the information processing unit 530.

[0129] (1-1-2. Overview of processing using existing technology)

[0130] Next, an outline of the processing according to the above-mentioned prior art will be described. First, a problem called degeneration in point cloud scan matching according to the prior art will be described.

[0131] In the prior art, in the case where the surrounding environment structure is simple or monotonous in the moving direction, in the case where the environmental structure included in the sensor field of view is insufficient, etc., there will be a state called degradation or degeneration (hereinafter, referred to as degradation). As degradation occurs, when the information through the external environment sensor (such as the light detection and ranging sensor 5110) is compared with the reference information using a process such as scan matching, the matching becomes uncertain, resulting in an increase in the error in the estimation of the own position / orientation.

[0132] Therefore, in the event of degradation, motion estimation using internal environmental sensors such as the IMU 5100 or wheel encoders (i.e., using odometry techniques) is used rather than using external environmental sensors to estimate own position / orientation.

[0133] Odometry technology is a technology for estimating the moving distance and direction change of a moving body based on the rotation speed of the wheels driving the moving body obtained by wheel encoders, etc., and the acceleration and angular velocity information obtained by the IMU 5100 installed on the moving body. Generally, these odometry technologies have a large cumulative error, resulting in the occurrence of drift depending on environmental conditions, etc., leading to inaccurate estimation results. In addition, a three-dimensional moving body such as a drone does not have wheels, etc., and therefore, odometers cannot be used by the previous method using wheels.

[0134] Figure 5 It is a schematic diagram for describing a situation where degradation or dynamic environment change occurs in point cloud scan matching according to the prior art. Figure 5 The diagram schematically shows the states of measurement regarding the relative motion between the target object and itself, when the object is moving (upper figure) and when the object is moving (lower figure), as shown for each of the following: (a) longitudinal translation motion; (b) lateral translation motion; (c) rotational motion around itself; (d) rotational motion around the object; and (e) motion when the object is a non-rigid body.

[0135] It should be noted that in Figure 5 In the figure, the sensor 70 itself is a sensor that performs measurement by LiDAR, wherein the direction of the center of the scanning range is defined as the front direction of the sensor 70.

[0136] (a) In the upper figure, the translational movement in the longitudinal direction represents an example in which an object 72 having a smooth and flat surface is moved so as to pass through the side surface of the sensor 70 in a direction parallel to the front surface of the sensor 70 while being irradiated with the transmission light signal 71 transmitted from the sensor 70. In addition, the lower figure is an example in which the sensor 70 is moved toward the front in parallel with the surface of the object 72 while transmitting the transmission light signal 71. In either case, the surface of the object 72 is irradiated with the transmission light signal 71, and the point irradiated with the transmission light signal 71 is defined as the detection point 77 as the distance measurement target.

[0137] Note that a smooth surface means that there are no feature points on the surface.

[0138] (b) Translational movement in the lateral direction The upper figure indicates an example in which an object 73 having a smooth and flat surface is moved so as to pass through the front surface of the sensor 70 in a direction perpendicular to (at right angles to) the front surface of the sensor 70 while being illuminated with a transmission light signal 71 transmitted from the sensor 70. The lower figure is an example in which the sensor 70 is moved parallel to the surface of the object 73 while transmitting the transmission light signal 71 with the front face of the sensor 70 facing the surface of the object 73. In either case, the surface of the object 73 is illuminated with the transmission light signal 71, and the point illuminated with the transmission light signal 71 is defined as a detection point 77 as a distance measurement target.

[0139] (c) Rotational motion about itself The upper figure shows an example in which an object 74 having a smooth surface in a spherical or cylindrical surface shape is rotated while being irradiated with a transmission light signal 71 by a sensor 70 located at the center of an arc of a cross section of the object 74, and rotating about the center. In addition, the lower figure is an example in which the sensor 70 performs a rotational motion about the center while transmitting the transmission light signal 71. In either case, the surface of the subject 74 is irradiated with the transmission light signal 71, and the point irradiated with the transmission light signal 71 is defined as a detection point 77 as a distance measurement target.

[0140] (d) Regarding rotational motion of an object The upper figure shows an example in which an object 75 having a smooth surface of a spherical or cylindrical surface rotates around the center of a circular cross section of the object 75 while being irradiated with a transmission light signal 71 by the sensor 70. In addition, the lower figure is an example in which the sensor 70 performs rotational motion around the object 75 about an axis while transmitting the transmission light signal 71. In either case, the surface of the object 75 is irradiated with the transmission light signal 71, and the point irradiated with the transmission light signal 71 is defined as a detection point 77 as a distance measurement target.

[0141] (e) Movement in the case where the object is a non-rigid body The upper figure shows an example in which an object 76, which is a non-rigid body having an irregular surface (such as a trembling cloth), moves toward the sensor 70 while being irradiated with the transmission light signal 71 by the sensor 70. In addition, the lower figure is an example in which the sensor 70 moves toward the object 76 while transmitting the transmission light signal 71. In either case, the surface of the object 76 is irradiated with the transmission light signal 71, and the point irradiated with the transmission light signal 71 is defined as the detection point 77 as the distance measurement target.

[0142] exist Figure 5 In any case shown in , in the direction of the relative motion of the objects 72 to 75 with respect to the sensor 70, the point cloud frames do not change, resulting in the occurrence of degradation. This makes it possible to perform motion estimation based on frame differences in scan matching. In addition, with respect to the relative motion of the object 76 with respect to the sensor 70, the state of the object 76 being a non-rigid body included in the environment reduces the structural correlation between the point cloud frames, making it very difficult to perform matching. Therefore, in the case where the object 76 as a non-rigid body is included in the environment, it is difficult to perform motion estimation based on frame differences in scan matching.

[0143] In addition, typical SLAM performs processing based on the assumption that the environment is static. Therefore, if a moving object is identified as the environment, errors will occur in the estimation of the own position or map estimation, resulting in processing failure. Figures 6 to 8B Describes the problem when the environment includes moving objects. Fig. 8A and Figure 8BIn , for simplicity, the point cloud and the environment map are represented in two dimensions. However, in practice, the point cloud and the environment map need to be processed in three dimensions.

[0144] Figure 6 is a schematic diagram showing an example of a static environment in which the environment does not include a moving object. Figure 6 In the cross section (a), for example, a sensor by LiDAR is mounted on a stationary object 81 whose position is fixed, and a transmitted light signal is scanned in a scanning range 86 to perform measurement. The scanning range 86 includes a stationary object 81 such as a wall whose position is fixed. Figure 6 , part (b) shows an example in which the sensor is mounted on a moving body 82 that moves in the direction of arrow 83 in the state of part (a).

[0145] Fig. 7A is a schematic diagram showing an example of a dynamic environment in which the environment includes a moving object. Fig. 7A In part (a), the Figure 6 An example of a moving target 84 moving in the direction of arrow 85 is included in the scanning range 86 in the state shown in part (a) of FIG. Fig. 7A In part (b), the Figure 6 An example in which the sensor is installed on a moving body 82 that moves in the direction of arrow 83 in the state shown in part (a).

[0146] Figure 7B is a schematic diagram showing an example of a dynamic environment in which the environment includes multiple moving objects. Figure 7B In the Figure 6 In the state shown in part (a) of FIG. 8 , the scanning range 86 includes examples of a plurality of moving objects 84a, 84b, and 84c moving in the directions of arrows 85a, 85b, and 85c, respectively. Figure 7B In part (b), the Figure 6 An example in which the sensor is installed on a moving body 82 that moves in the direction of arrow 83 in the state shown in part (a).

[0147] Fig. 8A Schematic diagram for illustrating scan matching according to the prior art when the environment does not include a moving object. In the example of (a), the sensor is mounted on a moving body 82 moving in the direction of arrow 83, and a stationary object 81 such as a wall at a fixed position exists within the scanning range 86 of the sensor.

[0148] The sensor scans the scanning range 86 from the scanning start position to the scanning end position to form a measurement frame.

[0149] Fig. 8APart (b) in FIG. 8 shows an example of scanning in frame #1 measured by the sensor in the state of part (a) and an example of scanning in frame #2 (e.g., the frame immediately following frame #1) after frame #1 in time. It can be seen that the positional relationship between the stationary object 81 and the moving body 82 changes as the moving body 82 moves.

[0150] Fig. 8A Part (c) shows an example of scan matching of the respective detection points 77 of frames #1 and #2 detected by the sensor in part (b). In this case, even if the position of the moving body 82 changes, the positional relationship of the respective detection points 77 does not change between frame #1 and frame #2, which facilitates scan matching.

[0151] Figure 8B : is a schematic diagram for illustrating scan matching according to the prior art in a case where the environment includes a moving object. A sensor is mounted on a moving body 82 moving in the direction of arrow 83 shown in section (a), and a stationary object 81 exists within a scanning range 86 of the sensor. In addition, the scanning range 86 of the sensor includes moving objects 84a, 84b, and 84c moving at arbitrary speeds in the directions of arrows 85a, 85b, and 85c, respectively.

[0152] Figure 8B Part (b) in FIG. 1 shows an example of a scan in frame #1 measured by the sensor in the state of part (a) and an example of a scan in frame #2 that is temporally subsequent to frame #1. It can be seen that the moving body 82 moves, and the moving bodies 84a to 84c move along with it, and the positional relationship between the stationary object 81 and the moving body 82 changes, and further, the positional relationship between the stationary object 81 and the moving body 82 and the moving bodies 84a to 84c changes.

[0153] Figure 8B Part (c) in FIG. 1 shows an example of scan matching for individual detection points 77 of frames #1 and #2 detected by the sensor in part (b). In this case, as the moving bodies 84a to 84c move, the positional relationship between the detection points 77 of the moving bodies 84a to 84c in frames #1 and 32 and the detection points 77 of the stationary object 81 changes, making it difficult to perform scan matching. In this way, the estimation process of SLAM is most unstable under the condition that the sensor itself and multiple moving objects around the sensor are moving.

[0154] See Figure 8B As described, methods of dealing with the problem in the case where a moving object is identified as an environment include a method of explicitly (directly) dealing with the moving object and a method of removing points corresponding to the moving object as outliers.

[0155] As the former method, a method of processing a moving object and a stationary object in the same frame so as to be able to reacquire data associated with a past observation point is proposed. In addition, as the latter method, the following methods are proposed: a method of tracking a moving object and removing it from a target, and a method of separating a stationary object from a moving object by pre-learning.

[0156] However, each method separates a moving object and a stationary object from each other by performing advanced processing using a multi-frame sensor output, thereby causing a problem in that it is difficult to process an object that moves at a high speed or has complex motion.

[0157] (1-2. Speed ​​measurement technology applicable to moving objects)

[0158] Here, we will refer to Fig. 9 and Fig.10 The velocity measurement techniques applicable to a moving object are schematically described. Velocity measurement on a moving object is roughly divided into a technique for measuring the velocity of the moving object itself (internal environment) and a technique for measuring the velocity of an external object observed from the moving object (external environment).

[0159] Fig. 9 FIG. 1 is a schematic diagram schematically showing a velocity measurement technique applicable to a moving object. Fig. 9 , part (a) shows an example of velocity measurement using a laser Doppler velocity meter 90. For example, a transmission light signal 91 of modulated light is transmitted from the laser Doppler velocity meter 90 and is reflected by the object 80. The laser Doppler velocity meter 90 determines the Doppler velocity w of the object 80 toward the laser Doppler velocity meter 90 based on the transmission light signal 91 and the reflected light signal from the object 80.

[0160] exist Fig. 9 , part (b) shows an example of velocity measurement using a millimeter wave Doppler radar 92. A millimeter wave measurement signal 93 is emitted from the Doppler radar 92 and reflected by the object 80. The Doppler radar 92 determines the Doppler velocity w of the target 80 toward the Doppler radar 92 based on the measurement signal 93 and the signal reflected from the target 80.

[0161] Fig. 9 The measurement techniques shown in parts (a) and (b) are applied to an external environment as a measurement target.

[0162] Fig. 9 Part (c) in FIG. 8 shows an example of velocity measurement using the IMU 94. Inertial measurement data measured by the IMU 94 mounted on the mobile body 82 is integrated to obtain the velocity v of the mobile body 82.

[0163] Fig. 9Part (d) in FIG. 8 shows an example of speed measurement using a wheel sensor 95. The wheel sensor 95 mounted on the moving body 82 detects, for example, a rotation angle or a rotation speed (per unit time) of a wheel 87 driving the moving body 82, and obtains a speed v of the moving body 82 based on the detection value.

[0164] Fig. 9 Part (e) in FIG. 8 shows an example of speed measurement using an optical flow sensor 96. The optical flow sensor 96 mounted on the moving body 82 captures images of the road surface on the rear side of the moving direction of the moving body 82 at a predetermined time interval, and compares a plurality of captured images to determine the speed v of the moving body 82.

[0165] exist Fig. 9 In the measurement techniques shown in parts (c) to (e), the internal environment is the measurement target.

[0166] exist Fig. 9 , part (f) shows an example of velocity measurement using the FMCW-LiDAR sensor 97. A transmission light signal 98 of chirped light is transmitted from the FMCW-LiDAR sensor 97 and is reflected by the object 80. The FMCW-LiDAR sensor 97 obtains the Doppler velocity w of the object 80 toward the FMCW-LiDAR sensor 97 based on the transmission light signal 98 and the reflected light from the object 80. At this time, the FMCW-LiDAR sensor 97 performs scanning within a predetermined angle range in the horizontal and vertical directions, and acquires a velocity point cloud based on a plurality of beams of reflected light from a plurality of detection points 77 on the object 80. This enables the FMCW-LiDAR sensor 97 to obtain a velocity distribution on the object 80.

[0167] Fig.10 is a schematic diagram showing exemplary features of the above-mentioned sensors (ie, the laser Doppler speedometer 90 , the Doppler radar 92 , the IMU 94 , the wheel sensor 95 , the optical flow sensor 96 , and the FMCW-LiDAR sensor 97 ). Fig.10 The example performs comparison of characteristics of five items of measurement targets, the number of measurement points per frame, angular resolution, environmental dependence, and error accumulation between sensors.

[0168] The laser Doppler speed meter 90, the Doppler radar 92, and the optical flow sensor 96 measure the external environment, and the IMU 94 and the wheel sensor 95 measure the internal environment. In contrast, the FMCW-LiDAR sensor 97 can measure both the internal environment and the external environment as measurement objects.

[0169] The angular resolution and the number of measurement points per frame are applied to the laser Doppler velocity meter 90, the Doppler radar 92, and the FMCW-LiDAR sensor 97 that perform scanning. With the laser Doppler velocity meter 90, the number of measurement points is 1 to a few points, and with the Doppler radar 92, the number of measurement points is several to several tens of points, and with the FMCW-LiDAR sensor 97, the number of measurement points is several tens of thousands of points. In addition, for example, in the laser Doppler velocity meter 90, the angular resolution is greater than 0.1 degrees (°), and, for example, in the Doppler radar 92, the angular resolution is greater than several degrees (°). In contrast, the angular resolution of the FMCW-LiDAR sensor 97 is 0.1° to 0.01°.

[0170] Regarding the environmental dependency, speed measurement based on the Doppler principle (such as the laser Doppler speedometer 90, the Doppler radar 92, and the FMCW-LiDAR sensor 97 using laser or millimeter wave) does not require features in the environment, while the optical flow sensor 96 requires features (feature points) in the environment. In addition, regarding the wheel sensor 95, the measurement result will vary depending on the environment, that is, the measurement result will be affected by the occurrence of slippage on the wheel 87, etc. In addition, the Doppler radar 92 sometimes has difficulty in detecting non-metallic measurement objects.

[0171] Regarding error accumulation in speed measurement, the laser Doppler speedometer 90, the Doppler radar 92, and the FMCW-LiDAR sensor 97 perform measurement instantaneously, and therefore there is no error accumulation. In contrast, the IMU 94 performs measurement based on the integration of acceleration, and the optical flow sensor 96 performs measurement based on the frame difference that accumulates or increases the error. In addition, regarding the wheel sensor 95, there is a possibility that the error increases due to slippage of the wheel 87, etc.

[0172] Among the above-mentioned speed measurement techniques, the present disclosure applies measurement using the FMCW-LiDAR sensor 97 , which has a high angular resolution and is capable of obtaining a very large number of measurement points per frame including both speed and distance, as a measurement technique for performing speed measurement.

[0173] (1-3. Specific Examples of Technology According to Embodiments of the Present Disclosure)

[0174] Next, the technology according to each embodiment of the present disclosure will be described using more specific examples.

[0175] (1-3-1. Basic System Configuration According to Each Embodiment of the Present Disclosure)

[0176] First, a basic system configuration according to each embodiment of the present disclosure will be schematically described. Fig.11 : is a block diagram showing a basic configuration example of the recognition system 1 according to each embodiment of the present disclosure.

[0177] exist Fig.11 In the embodiment, the recognition system 1 includes a sensor unit 10, a signal processing unit 20 and an information processing unit 30, as shown in FIG. Figure 1 The sensor unit 10 includes an IMU 100, a light detection and ranging sensor 110 that performs ranging and velocity measurement using FMCW-LiDAR, and a synchronization signal generator 130. As described above, the IMU 100 includes, for example, a three-axis acceleration sensor, a three-axis angular velocity sensor, and a three-axis geomagnetic sensor, and outputs inertial measurement data (IMU data) obtained by these sensors.

[0178] The light detection and ranging sensor 110 is a sensor that performs distance measurement using light, and applies the FMCW-LiDAR method as described above. Fig.12 is a block diagram illustrating a configuration of an example of the light detection and ranging sensor 110 according to various embodiments.

[0179] exist Fig.12 , the light detection and ranging sensor 110 includes an optical scanning unit 111 , an optical transmission / reception unit 112 , a reception signal processing unit 113 , an optical scanning controller 114 , and a transmission unit 115 .

[0180] The light transmission / receiving unit 112 includes: a light transmission unit that generates a transmission light signal transmitted from the optical scanning unit 111 described below; a transmission light controller that controls the transmission light generated by the light transmission unit; and a light receiving unit that receives (receives) the reception light signal from the optical scanning unit 111.

[0181] In the optical transmission / reception unit 112, the optical transmission unit includes: a light source, such as a laser diode for oscillating laser light as transmission light; an optical system for emitting light oscillated by the light source; and a laser output modulator, for example, for driving the light source. The optical transmission unit uses the light source to perform optical oscillation according to the light emission control signal supplied by the transmission light controller, and emits a transmission light signal through chirped light whose frequency changes linearly within a prescribed frequency range as time passes. The transmission light signal is transmitted to the optical scanning unit 111, and is transmitted to the light receiving unit together with the local oscillation light.

[0182] In the optical transmission unit, the transmission light controller generates a signal whose frequency changes linearly (increases or decreases) over time within a predetermined frequency range. Such a signal whose frequency changes linearly over time within a predetermined frequency range is called a chirp signal. The transmission light controller generates a light emission control signal as a modulation synchronization timing signal input to the laser output modulator included in the optical transmission unit according to the linear frequency modulation signal. The transmission light controller generates a transmission control signal as a signal synchronized with the synchronization signal supplied from the synchronization signal generation unit 130. The transmission light controller transmits the generated optical transmission control signal to the optical transmission unit and the reception signal processing unit signal processing unit 113 described later.

[0183] In the optical transmission / reception unit 112, the optical receiving unit includes: for example, an optical receiver that receives a received light signal (performs optical reception) from the optical scanning unit 111; and a driving circuit that drives the above-mentioned optical receiving unit. For example, the optical receiver can be realized by applying a configuration that combines a focusing lens and a light receiving element (such as a photodiode). The optical receiving unit also includes an optical combiner that combines the received light received from the scanning unit with the local oscillation light sent from the optical transmission unit. When the received light is reflected light from a target that emits light, the received light is a signal delayed according to the distance to the object compared to the local oscillation light, and therefore, the combined signal obtained by combining the received light and the local oscillation light is a signal (beat signal) having a frequency component corresponding to the distance. The optical transmission / reception unit 112 passes the signal as a received waveform signal to the received signal processing unit 113.

[0184] The reception signal processing unit 113 performs predetermined signal processing such as fast Fourier transform on the reception signal transmitted from the optical transmission / reception unit 112 in synchronization with the synchronization signal provided from the synchronization signal generator 130. With this signal processing, the reception signal processing unit 113 acquires the distance to the target, the Doppler speed of the target, and the intensity of the received optical signal. The reception signal processing unit 113 adds a timestamp generated in synchronization with the synchronization signal to the acquired distance, speed, and Doppler speed, and transmits the data with the timestamp to the transmission unit 115. Here, the timestamp is a timestamp related to the measurement indicating the timing of transmitting the transmission optical signal by the optical transmission / reception unit 112, and is generated and added for each measurement.

[0185] The optical scanning controller 114 generates a scanning control signal for controlling scanning of a transmission light signal in the optical scanning unit 111. At this time, the optical scanning controller 114 generates the scanning control signal so that the scanning of the transmission light signal is synchronized with the synchronization signal provided from the synchronization signal generator 130. The optical scanning controller 114 may generate a scanning control signal for performing scanning within a predetermined scanning range, or may generate a scanning control signal according to scanning control information transmitted from the control communication unit 230 described below.

[0186] Furthermore, the optical scanning controller 114 receives an angle detection signal indicating a scanning angle of the transmission light signal from the optical scanning unit 111. The optical scanning controller 114 transfers information indicating the scanning angle to the transmission unit 115 based on the received angle detection signal.

[0187] The optical scanning unit 111 transmits the transmission light signal sent from the optical transmission / reception unit 112, the transmission is performed at an angle according to the scanning mode corresponding to the scanning control signal provided from the optical scanning controller 114, and receives the light incident from the angle, and outputs the light as a received light signal. In the optical scanning unit 111, for example, a scanning mechanism of the transmission light signal can be realized by applying a biaxial mirror scanner. In this case, the scanning control signal is, for example, a driving voltage signal applied to each axis of the biaxial mirror scanner.

[0188] Furthermore, the optical scanning unit 111 detects the angle at which the transmission light signal is transmitted in the horizontal direction and the vertical direction, and outputs an angle detection signal indicating the detected angle.

[0189] The transmission unit 115 transmits the timestamp, distance, Doppler velocity, and intensity transmitted from the reception signal processing unit 113 and the scanning angle transmitted from the optical scanning controller 114 as FMCW-LiDAR data to the signal processing unit 20. That is, the FMCW-LiDAR data includes a timestamp for each measurement point of the light detection and ranging sensor 110.

[0190] Fig.13 is a diagram schematically illustrating an example of a scanning pattern of a transmission optical signal according to an embodiment. Fig.13 An example of a raster scanning mode in the scanning mode is shown. The optical scanning unit 111 performs scanning within a predetermined angular range 45 along a 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 track obtained by scanning 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 along the scanning line 40 according to the scanning control signal.

[0191] For example, according to the scanning control signal, the optical scanning unit 111 sequentially and discretely changes the emission point 41 of the chirped light as the transmission light signal along the scanning line 40 at a constant time interval (point rate). The emission points 41 within the angle range 45 constitute one frame in the FMCW-LiDAR. The scanning time from the upper end to the lower end of the angle range 45 forms a 1-frame time span. The transmission light signal at each emission point 41 is sequentially emitted at a predetermined time interval in one frame according to the scanning line 40. Therefore, the measurement data at each emission point 41 is acquired at different time points for each emission point 41 according to the scanning sequence.

[0192] The scanning speed of the biaxial mirror scanner is reduced near the turning points at the left and right ends of the angular range 45 of the scan line 40. Therefore, the individual emission points 41 are not arranged in a grid pattern in the angular range 45.

[0193] At the emission points 41a and 41b (solid lines) corresponding to the positions where the objects 50a and 50b exist, for example, the emitted transmitted light signal is reflected by the objects 50a and 50b, and the reflected light is returned as the received light signal. On the other hand, at the emission points 41 corresponding to the positions where the objects 50a and 50b do not exist, no reflected light is obtained, and therefore no received light signal is obtained.

[0194] It should be noted that the optical transmission / reception unit 112 may transmit the transmission light signal once or a plurality of times to one emission point 41. Furthermore, while the optical scanning unit 111 continuously changes the scanning angle, the optical transmission / reception unit 112 transmits the transmission light signal by chirped light transmission whose frequency continuously changes in a time series at each emission point 41. Thus, for example, the transmission light signal is applied to the object in an elliptical shape along the scanning line 40.

[0195] The scanning pattern of the transmission light signal is not limited to the above-mentioned raster scanning pattern. Fig.14A and Fig. 14B is a diagram schematically showing another example of the scanning pattern of the transmission light signal.

[0196] Fig.14A An example of a multi-layer scanning mode in the scanning mode is shown. In the multi-layer scanning, scanning of a plurality of lines is performed simultaneously by rotating a plurality of beams 360°. More specifically, in the multi-layer scanning, an emission point 41 of a transmission light signal is transmitted along scanning lines 421, 422, ..., and 42 N The scanning lines 421, 422, ..., and 423 do not change sequentially and discretely at constant time intervals. N folding, thereby arranging the emission points 41 in a grid pattern.

[0197] Fig. 14B An example of a spot scanning mode in the scanning mode is shown. In spot scanning, the beam scanning trajectory is not continuous but discrete because it is fixed at each emission point 41. At each emission point 41, an emission light signal is emitted, and after the reception of the reflected light signal is completed, the light beam is switched to the next emission point 41. Fig. 14B , part (a) shows an example in which beam switching is performed sequentially according to a prescribed order indicated by an arrow 47 in the figure. In addition, part (b) shows an example in which beam switching is performed in an arbitrary order by a scan control setting from the signal processing unit 20 (for example, as shown by an arrow 48 in the figure). In the example of part (b), only a predetermined area in the entire scan area can be scanned by the scan control setting, and a plurality of predetermined areas can be set in one frame. In addition, the prescribed area can also be determined based on the position of a detected moving object or a stationary object.

[0198] return Fig.11 , the signal processing unit 20 includes a receiving unit 200, a sensor position / orientation estimation unit 210, a moving object / stationary object separation unit 211, a SLAM processing unit 220, and a transmission unit 201. The parts other than the moving object / stationary object separation unit 211 correspond to the parts described in the embodiment of the present invention. Figure 2 The receiving unit 5200, the sensor position and posture estimation unit 5210, the SLAM processing unit 5220, and the transmission unit 5201 are described.

[0199] The receiving unit 200 receives the IMU data and the FMCW-LiDAR data transmitted from the sensor unit 10. The receiving unit 200 transmits the received IMU data to the sensor position / orientation estimating unit 210, and transmits the FMCW-LiDAR data to the moving object / stationary object separating unit 211.

[0200] The sensor position / orientation estimation unit 210 estimates the position, orientation, and angular velocity of the IMU 100 (sensor unit 10) based on the IMU data transmitted from the receiving unit 200 and the sensor position / orientation transmitted from the SLAM processing unit 220. The sensor position / orientation estimation unit 210 transmits the estimated position, orientation, and angular velocity to the SLAM processing unit 220.

[0201] Based on the FMCW-LiDAR data transmitted from the receiving unit 200, the moving object / stationary object separation unit 211 extracts a stationary object velocity point cloud as a velocity point cloud of a stationary object and a moving object velocity point cloud as a velocity point cloud of a moving object. Next, the processing of the moving object / stationary object separation unit 211 is described. The moving object / stationary object separation unit 211 transmits the extracted stationary object velocity point cloud to the SLAM processing unit 220 and the transmission unit 201. In addition, the moving object / stationary object separation unit 211 transmits the extracted moving object velocity point cloud to the transmission unit 201.

[0202] The SLAM processing unit 220 creates map information using the SLAM technology based on the sensor position, orientation, and angular velocity data transmitted from the sensor position / orientation estimation unit 210 and the stationary object velocity point cloud transmitted from the moving object / stationary object separation unit 211. The configuration of the SLAM processing unit 220 is equivalent to that according to Figure 4 The configuration of the prior art SLAM processing unit 5220 is described.

[0203] According to various embodiments, the SLAM processing unit 220 includes Figure 4 The SLAM processing unit 220 according to the present embodiment inputs the stationary object velocity point cloud instead of the point cloud to the scan matching processing unit corresponding to the scan matching processing unit 5222. Since the scan matching processing unit performs the scan matching processing based on the stationary object velocity point cloud excluding the velocity point cloud of the moving object, the scan matching processing can be performed with higher accuracy compared to the scan matching processing unit 5222 of the related art.

[0204] The SLAM processing unit 220 transmits the sensor position and orientation output from the scan matching processing unit and the map information created and updated by the map updating unit 5223 to the transmission unit 201. In addition, the SLAM processing unit 220 transmits the sensor velocity and angular velocity to the transmission unit 201.

[0205] The transmission unit 201 transmits the sensor position, orientation, speed and angular velocity, map information, stationary object speed point cloud, and moving object speed point cloud passed from the SLAM processing unit 220 to the information processing unit 30. At this time, the transmission unit 201 adds a timestamp indicating a time point when the light detection and ranging sensor 110 has measured a frame to each piece of information, and sends the information to the information processing unit 30.

[0206] (1-3-2. Overview of Processing According to Each Embodiment of the Present Disclosure)

[0207] An overview of a process according to each embodiment of the present disclosure will be described.

[0208] Fig.15 is a schematic diagram for describing the situation where degradation or dynamic environment change occurs in point cloud scan matching according to various embodiments. Figure 5 of Fig.15 The diagram schematically shows the states of measurement regarding the relative motion between the target object and itself, when the object is moving (upper figure) and when the object is moving (lower figure), as shown for each of (a) longitudinal translation motion; (b) lateral translation motion; (c) rotational motion around itself; (d) rotational motion around the object; and (e) motion when the object is a non-rigid body.

[0209] exist Fig.15 In each of the examples shown in FIG. 1 , the relationship between the light detection and ranging sensor 110 and the object 72 (itself) is the same as in FIG. Figure 5 The relationship between the sensor 70 and the object 72 in each instance is the same, and therefore its description is omitted here.

[0210] exist Fig.15 In the example of (a), (b) lateral translational motion, (d) rotational motion around the object, and (e) Doppler velocity 78 of motion when the object is a non-rigid body can be detected because the velocity in the distance direction occurs according to the relative motion of the light detection and ranging sensor 110 and the object 72. Therefore, in the examples of (a), (b), (d), and (e), the Doppler velocity 78 can be used to perform velocity estimation without performing scan matching.

[0211] On the contrary, Fig.15 In (c) the rotational motion around itself, no velocity occurs in the distance direction according to the relative motion between the light detection and ranging sensor 110 and the object 72. Therefore, the Doppler velocity 78 cannot be detected, and the angular velocity cannot be estimated using the Doppler velocity 78. For example, the angular velocity can be obtained by using the detection result of the IMU 100 together.

[0212] refer to Fig.16A and Fig. 16B , scan matching in a case where an object moves and an environment includes a plurality of moving objects according to each embodiment will be described.

[0213] Fig.16A is a schematic diagram showing an example of self-movement and an environment including a plurality of moving objects according to an embodiment. Fig.16AIn part (a), the Figure 8B Similar to part (a) of FIG. 1 , a moving body 82 moving in the direction of an arrow 83 (not shown) is provided with the recognition system 1 according to various embodiments, and a stationary object 81 exists within a scanning range 86 of a light detection and ranging sensor 110 included in the recognition system 1. In addition, the scanning range 86 of the light detection and ranging sensor 110 includes moving objects 84a, 84b, and 84c moving at arbitrary speeds in the directions of arrows 85a, 85b, and 85c, respectively.

[0214] exist Fig.16A , part (b) shows an example of a point cloud detected by scanning one frame in the state of part (a). The transmission light signal 71 transmitted from the light detection ranging sensor 110 mounted on the moving body 82 is irradiated to the stationary object 81 and the moving objects 84a to 84c, and the Doppler velocity 78 is detected at each detection point 77 using the transmission light signal 71.

[0215] Fig. 16B is a schematic diagram for illustrating motion estimation of a moving object according to an embodiment. Fig. 16B Shown in Fig.16A A case where motion estimation of each moving object 84a to 84c is performed in the state shown in part (b).

[0216] First, if Fig. 16B As shown in part (a) of FIG. 8 , for example, the signal processing unit 20 estimates the velocity and angular velocity of the moving object 82 based on the IMU data output from the IMU 100 .

[0217] Next, the signal processing unit 20 performs coordinate transformation on each detection point 77 based on the estimated speed and angular velocity of the moving body 82, and obtains the Doppler velocity 78 of each detection point 77 obtained by subtracting the speed and angular velocity of the moving body 82. Based on the Doppler velocity 78 at each detection point 77 obtained in this way, the signal processing unit 20 extracts the detection points 77 that are considered to be stationary. Among the extracted detection points 77, the detection points 77 that exist as a cluster in a specific area constitute a partial environment point cloud 79 (refer to Fig.16A , part (b)). Fig. 16B As shown in part (c) of , the signal processing unit 20 creates a map based on the partial environment point cloud 79 .

[0218] The signal processing unit 20 further extracts the detection points 77 in the moving bodies 84a to 84c based on the Doppler velocity 78 of each detection point 77 obtained by subtracting the velocity and angular velocity of the moving body 82. In this way, the signal processing unit 20 estimates the velocity and angular velocity of each moving body 84a to 84c based on the Doppler velocity 78 of each extracted detection point 77. In addition, the signal processing unit 20 estimates the position and orientation of the moving bodies 84a to 84c based on each extracted detection point 77.

[0219] (2. According to the first embodiment of the present disclosure)

[0220] Next, a first embodiment according to the present disclosure will be described.

[0221] (2-1. Specific Example of Configuration According to First Embodiment)

[0222] First, the configuration of the first embodiment will be described more specifically. Fig.17 : is a block diagram showing the configuration of an example of the recognition system according to the first embodiment. Fig.17 In the embodiment, the recognition system 1 a according to the first embodiment includes a sensor unit 10 a , a signal processing unit 20 a , and an information processing unit 30 .

[0223] The sensor unit 10a includes an IMU 100, a light detection and ranging sensor 110 to which FMCW-LiDAR is applied, an image sensor 120, and a synchronization signal generator 130. Fig.12 The described configuration implements the light detection and ranging sensor 110. The image sensor 120 is typically a camera, images a subject, and outputs image data including information of, for example, red (R), green (G), and blue (B).

[0224] The synchronization signal generator 130 generates a synchronization signal. The synchronization signal may be a pulse of a predetermined period. The synchronization signal generator 130 supplies the generated synchronization signal to the IMU 100, the light detection and ranging sensor 110, and the image sensor 120. The operation and output timing of the output data of each of the IMU 100, the light detection and ranging sensor 110, and the image sensor 120 are controlled in synchronization with the provided synchronization signal.

[0225] (2-1-1. Signal processing unit)

[0226] exist Fig.17 In the present invention, each configuration included in the signal processing unit 20 can be roughly divided into a part (referred to as a library part) that provides each function and a part (referred to as an application part) that executes target processing using the function provided by the part.

[0227] exist Fig.17 In the example, the library part includes, for example, a receiving unit 200, a sensor position / orientation estimation unit 210, a moving object / stationary object separation unit 211, a sensor speed estimation unit 212, a stationary object point cloud correction unit 213, and a moving object point cloud correction unit 214. In addition, the application part includes, for example, a SLAM processing unit 220, a moving object state estimation unit 221, a 3D / 2D converter 222, a region of interest (ROI) extraction unit 223, and an image recognition unit 226.

[0228] In the signal processing unit 20, the configuration included in the library part and the application part is not limited to the above-mentioned example. For example, the moving object state estimation unit 221, or the moving object state estimation unit 221 and the 3D / 2D converter 222 may be included in the library part. In addition, the transmission unit 201 may be included in the library part or the application part, or may be a part that is not included in any of them. The receiving unit 200 may also be included in the application part, may be included in the library part or the application part, or may be a part that is not included in any of them.

[0229] (2-1-1-1. Library section)

[0230] First, the library portion of the signal processing unit 20 will be described.

[0231] In the library part, the receiving unit 200 receives each data output from the sensor unit 10 and transmits the received data to each unit of the signal processing unit 20. More specifically, the receiving unit 200 transmits the IMU data output from the IMU 100 of the sensor unit 10 to the sensor position / orientation estimation unit 210. The receiving unit 200 also transmits the FMCW-LiDAR data output from the light detection and ranging sensor 110 of the sensor unit 10 to the moving object / stationary object separation unit 211. In addition, the receiving unit 200 transmits the image data output from the image sensor 120 of the sensor unit 10 to the ROI extraction unit 223.

[0232] In this manner, the receiving unit 200 functions as a receiving unit that receives velocity point cloud data including a plurality of points each having velocity information and time point information, and inertial measurement data (IMU data).

[0233] The sensor position / orientation estimation unit 210 estimates the position, orientation, and angular velocity of the IMU 100 based on the IMU data transmitted from the receiving unit 200. For example, the sensor position / orientation estimation unit 210 estimates the current position, orientation, and angular velocity of the IMU 100 by performing a sensor fusion process (such as a Kalman filter), for example, using each sensor data of a three-axis acceleration sensor, a three-axis angular velocity sensor, and a three-axis geomagnetic sensor included in the IMU data. The sensor position / orientation estimation unit 210 preliminarily acquires calibration data, such as the positional relationship of the reference coordinate system of the light detection and ranging sensor 110 relative to the reference coordinate system of the IMU 100 and the internal parameters of each sensor, and uses the prepared calibration data when estimating. The sensor position / orientation estimation unit 210 transmits the data of the estimated sensor position, orientation, and angular velocity to the sensor velocity estimation unit 212, the SLAM processing unit 220, and the transmission unit 201.

[0234] The moving object / stationary object separation unit 211 receives the FMCW-LiDAR data from the receiving unit 200, and also receives the sensor speed indicating the speed of the light detection and ranging sensor 110 estimated by the sensor speed estimation unit 212 described below. Using speed discrimination based on the Doppler speed and the sensor speed included in the delivered FMCW-LiDAR data, the moving object / stationary object separation unit 211 separates the speed point cloud indicated by the FMCW-LiDAR data into a speed point cloud indicated by a moving object and a speed point cloud indicated by a stationary object. The moving object / stationary object separation unit 211 can extract the moving object speed point cloud and the stationary object speed point cloud from the FMCW-LiDAR data corresponding to the scan of one frame.

[0235] For example, the moving object / stationary object separation unit 211 subtracts the optical axis direction component of the sensor velocity from the Doppler velocity of each measurement point included in the FMCW-LiDAR data to eliminate the influence of the movement of the sensor unit 10 from the velocity information of the velocity point cloud, and obtains the corrected Doppler velocity of each measurement point viewed from the coordinate system of the stationary object. That is, the corrected Doppler velocity is a velocity obtained by excluding the influence of the movement of the sensor unit 10 from the Doppler velocity included in the FMCW-LiDAR data. The moving object / stationary object separation unit 211 performs threshold determination on the magnitude of the corrected Doppler velocity, and extracts a velocity point cloud (referred to as a local velocity point cloud) based on measurement points localized within a specific spatial range (corresponding to the size of the target object) among measurement points whose size is greater than the threshold as a moving object velocity point cloud. The moving object / stationary object separation unit 211 can extract a plurality of moving object velocity point clouds from one frame of the FMCW-LiDAR data.

[0236] In addition, the moving object / stationary object separation unit 211 performs threshold determination on the magnitude of the corrected Doppler velocity of the measurement point based on the FMCW-LiDAR data. As a result of the threshold determination, the moving object / stationary object separation unit 211 extracts a set of measurement points whose magnitude of the corrected Doppler velocity is below the threshold as a stationary object velocity point cloud.

[0237] Here, in the threshold determination, the moving object / stationary object separation unit 211 may provide two types of thresholds: threshold v tha and threshold v thb (<v tha ). The moving object / stationary object separation unit 211 can extract the amplitude of the corrected Doppler velocity as the threshold value v tha or larger measurement points are used as the moving object velocity point cloud, and the amplitude of the corrected Doppler velocity can be extracted as the threshold v thb or smaller measurement points as stationary object velocity point clouds. This means that there are intermediate point clouds that do not belong to moving object velocity point clouds or stationary object velocity point clouds. For the purpose of extracting clearly stationary measurement points, it is appropriate to provide two types of thresholds in this way.

[0238] Note that the point cloud frame consists of velocity point clouds obtained from one frame of FMCW-LiDAR data.

[0239] The moving object / stationary object separation unit 211 transfers the extracted stationary object speed point cloud to the sensor speed estimation unit 212 and the stationary object point cloud correction unit 213. Furthermore, the moving object / stationary object separation unit 211 transfers the extracted moving object speed point cloud to the moving object point cloud correction unit 214.

[0240] The sensor speed estimation unit 212 estimates the sensor speed indicating the speed of the sensor unit 10 based on the stationary object point cloud passed from the moving object / stationary object separation unit 211 and the sensor position, orientation, and angular velocity passed from the sensor position / orientation estimation unit 210. A specific example of the estimation process of the sensor speed by the sensor speed estimation unit 212 will be described below.

[0241] The sensor speed estimation unit 212 transfers the estimated sensor speed to the stationary object point cloud correction unit 213 and the moving object / stationary object separation unit 211, and together with this operation, inputs the estimated sensor speed to the moving object / stationary object separation unit 211. The sensor speed estimation unit 212 transfers the estimated sensor speed to the transmission unit 201.

[0242] The stationary object point cloud correction unit 213 corrects the stationary object velocity point cloud using the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimation unit 210 and the sensor velocity transmitted from the sensor velocity estimation unit 212. The moving object point cloud correction unit 214 corrects the moving object velocity point cloud by using the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimation unit 210, the sensor velocity transmitted from the sensor velocity estimation unit 212, and information indicating the state of the moving object transmitted from the moving object state estimation unit 221 described below.

[0243] As described above, in FMCW-LiDAR, the acquisition time of the measurement data at each measurement point in the frame varies according to the position of the measurement point in the frame according to the type of scanning pattern. The stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 estimate the data of each point in the case where each point of the velocity point cloud in the frame is acquired at the same predetermined time, and correct the velocity point cloud based on the estimation result.

[0244] Fig.18 FIG. 2 is a schematic diagram schematically illustrating a velocity point cloud correction process according to an embodiment. Figure 6 , part (a) shows an example of the timing of the FMCW-LiDAR data output from the light detection and ranging sensor 110. As shown in the figure, from the frame start time t fst1 To the next frame start time t fst2 , outputting FMCW-LiDAR data at a constant time interval. Each point of the velocity point cloud based on FMCW-LiDAR data has time information corresponding to the output timing of each data.

[0245] In the figure, data corresponding to the emission point 41 is indicated by a solid line (hatched line), and the emission point 41 corresponds to the objects 50a and 50b (refer to Fig.13 ) etc. In addition, the data of the transmission point 41 corresponding to the position where the objects 50a and 50b do not exist is indicated by a dotted line. The data indicated by the dotted line indicates that no object is actually detected from the received light signal.

[0246] exist Fig.18 , part (b) schematically shows a state in which the FMCW-LiDAR data output from the light detection and ranging sensor 110 is corrected by the stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214. The stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 estimate the frame starting time t based on, for example, the sensor position, orientation, and angular velocity, and the sensor velocity. fst1 To the frame start time t fst2The stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 individually correct the stationary object velocity point cloud and the moving object velocity point cloud based on the estimation result.

[0247] More specifically, the stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 correct the time information of each point included in the stationary object velocity point cloud and the moving object velocity point cloud to the reference time. This eliminates the distortion of the point cloud caused by the self-motion or the moving object motion within the frame time span, so that in an embodiment, FMCW-LiDAR can achieve more universal use of the measurement results.

[0248] The stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 output the corrected stationary object velocity point cloud and the moving object velocity point cloud, respectively. At this time, the stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 indicate the time ( Fig.18 The frame start time t of the example fst2 ) is added to the corrected stationary object velocity point cloud and moving object velocity point cloud and output. Here, the time stamp is a value determined for each frame.

[0249] In this manner, the stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 function as correction units that correct at least one attribute value associated with at least one point included in the velocity point cloud data based on the estimated value at a predetermined time point.

[0250] The stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 transmit the stationary object velocity point cloud and the moving object velocity point cloud to which the correction and time stamp are added to the transmission unit 201 , respectively.

[0251] A specific example of correction processing of the sensor position, orientation, and angular velocity and the sensor velocity based on the stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 will be described below.

[0252] In this embodiment, through this correction processing, each point included in the stationary object speed point cloud and the moving object speed point cloud within one frame can be processed as a point obtained at the same time, which can reduce the processing load of the stationary object speed point cloud and the moving object speed point cloud in the subsequent stage.

[0253] In addition, in an embodiment, the velocity point cloud is corrected based on the value based on the sensor position, orientation, angular velocity, and sensor velocity estimation. Therefore, in each embodiment, a velocity point cloud having time information of the same time for each point can be obtained through FMCW-LiDAR data for one frame.

[0254] Note that the method of acquiring a velocity point cloud in which each point has time information at the same time point is not limited to the above method. For example, by using FMCW-LiDAR data of a plurality of frames and by performing linear interpolation based on corresponding data having the emission point 41 in the plurality of frames, a velocity point cloud in which each point has time information at the same time point can also be acquired.

[0255] (2-1-1-2. Application part)

[0256] Next, the application part of the signal processing unit 20 will be described.

[0257] exist Fig.17 In the process, the stationary object point cloud correction unit 213 transmits the corrected stationary object velocity point cloud to the transmission unit 201 and the SLAM processing unit 220 .

[0258] The SLAM processing unit 220 estimates and creates map information based on the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimation unit 210 and the stationary object velocity point cloud transmitted from the stationary object point cloud correction unit 213. The map information includes the own position of the sensor unit 10 and a map of the surrounding environment of the sensor unit 10. The SLAM processing unit 220 transmits the created map information to the transmission unit 201.

[0259] In addition, the SLAM processing unit 220 transmits the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimating unit 210 and the velocity transmitted from the sensor velocity estimating unit 212 to the transmission unit 201 .

[0260] The moving object point cloud correction unit 214 transmits the corrected moving object speed point cloud to the transmission unit 201 and the moving object state estimation unit 221 .

[0261] For example, the mobile object state estimation unit 221 separates each mobile object contained in one frame based on the corrected mobile object speed point cloud. The mobile object state estimation unit 221 estimates the state of the mobile object including the position, orientation, speed, and angle of the mobile object for each separated mobile object based on the corresponding mobile object speed point cloud. The mobile object state estimation unit 221 transmits mobile object state information, which is information indicating the estimated state of the mobile object, to the transmission unit 201, and also transmits the mobile object state information to the mobile object point cloud correction unit 214 and the 3D / 2D converter 222.

[0262] The 3D / 2D converter 222 converts the moving object speed point cloud, which is three-dimensional (3D) information, into two-dimensional (2D) data corresponding to the image data output from the image sensor 120. More specifically, the 3D / 2D converter 222 converts the 3D coordinates of the moving object speed point cloud into 2D coordinates according to the coordinate system of the image data obtained by the image sensor 120. The 3D / 2D converter 222 acquires calibration data (e.g., the positional relationship of the reference coordinate system of the light detection and ranging sensor 110 relative to the reference coordinate system of the image sensor 120) and internal parameters of each sensor in advance, and uses the prepared calibration data at the time of conversion. This makes it possible to obtain the position of each moving object indicated in the moving object speed point cloud in the image data. The 3D / 2D converter 222 transfers the point cloud, which is a point cloud obtained by converting the coordinates of the moving object speed point cloud into 2D coordinates, to the ROI extraction unit 223.

[0263] The ROI extraction unit 223 extracts a region of interest (ROI) from the image data transmitted from the image sensor 120 via the receiving unit 200, based on the point cloud in the 2D coordinates transmitted from the 3D / 2D converter 222. More specifically, for example, the ROI extraction unit 223 can extract, in the image data, a region corresponding to the point cloud in the 2D coordinates transmitted from the 3D / 2D converter 222 as the region of interest. Incidentally, knowing the positional relationship between the light detection and ranging sensor 110 and the image sensor 120, the ROI extraction unit 223 can instantaneously calculate the correspondence between the image data and the point cloud on the 2D coordinates.

[0264] The ROI extraction unit 223 transmits the extracted image data of the region of interest to the transmission unit 201 and the image recognition unit 226 .

[0265] 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 (indicating that the object is a person, the name of the object, or indicating that the object is a vehicle, etc.). For example, the image recognition unit 226 may perform image recognition using a learning model trained by machine learning based on image data using known images. The image meta information output from the image recognition unit 226 is transmitted to the transmission unit 201.

[0266] The transmission unit 201 transmits the sensor position, orientation, angular velocity, and sensor velocity transmitted from the SLAM processing unit 220 to the information processing unit 30. In addition, the transmission unit 201 sends the sensor position, orientation, angular velocity, and sensor velocity transmitted from the SLAM processing unit 220, map information, and the corrected stationary object velocity point cloud transmitted from the stationary object point cloud correction unit 213 to the information processing unit 30. In addition, the transmission unit 201 sends the moving object velocity point cloud transmitted from the moving object point cloud correction unit 214 and the moving object state information transmitted from the moving object state estimation unit 221 to the information processing unit 30. Further, the transmission unit 201 transmits the image meta information transmitted from the image recognition unit 226 to the information processing unit 30.

[0267] The transmission unit 201 adds a timestamp to each of the above information and point clouds and transmits the data with the timestamp to the information processing unit 30. Here, the timestamp added by the transmission unit 201 to each piece of information and point cloud may be time point information in units of one frame scan performed by the light detection and ranging sensor 110. As a specific example, referring to the above Fig.18 , indicating the frame start time t used as a reference for the correction of the velocity point cloud fst2 The information can be added as a timestamp to the scan performed by the light detection and ranging sensor 110 from the frame start time t fst1 To the next frame start time t fst2 In each piece of information obtained during the time period.

[0268] In this way, the transmission unit 201 is used as an output unit, which outputs sensor status information and moving object status information generated based on the velocity point cloud data and inertial measurement data, the sensor status information is information including information indicating the position and orientation of the sensor unit, and the moving object status information is information including information indicating the position, orientation and speed of the moving object.

[0269] The transmission unit 201 may selectively transmit the above information and the information in the point cloud or the point cloud to the information processing unit 30 in response to a request from the information processing unit 30. In addition, the transmission unit 201 may transmit each piece of information and the above point cloud to a transmission destination different from the information processing unit 30.

[0270] (2-1-2. System Configuration)

[0271] Next, we will refer to Fig.19A and Fig.19B A system configuration applicable to the first embodiment is described. Fig.19A and Fig.19BThe illustrated configuration is also applicable to the modification of the first embodiment described below, as well as the second embodiment and the third embodiment.

[0272] Fig.19A : is a block diagram showing a hardware configuration of an example of the recognition system 1 a applicable to the first embodiment.

[0273] exist Fig.19A In the embodiment of the present invention, the sensor unit 10a includes a sensor group 1000 and a firmware 1010. The sensor group 1000 includes an IMU 100, a light detection and ranging sensor 110, and an image sensor 120.

[0274] The firmware 1010 may be a program for controlling the operation of the sensor group 1000. For example, the firmware 1010 may control an interface (not shown) which is hardware for controlling the input / output of data of each sensor included in the sensor group 1000 and the operation of the synchronization signal generator 130. For example, the interface may include a memory storing the firmware 1010 in advance and a processor operating according to the firmware 1010 stored in the memory.

[0275] The signal processing unit 20a includes a processor 2000. The processor 2000 operates according to a program stored in a memory (not shown) to implement the above-mentioned library part 2010 and application part 2020. The processor 2000 may be, for example, an image signal processor (ISP). The processor 2000 is not limited thereto and may be a digital signal processor (DSP) or a central processing unit (CPU).

[0276] Furthermore, the configuration is not limited to this configuration, and part or all of the units included in the above-mentioned library part 2010 and application part 2020 may be realized by hardware circuits operating in cooperation with each other.

[0277] Similar to the signal processing unit 20a, the information processing unit 30 includes, for example, a processor 3000 including an ISP. The processor 3000 operates according to a program stored in a memory (not shown) to implement a library part 3010 and an application part 3020. The library part 3010 is a group of programs that provide various functions in the information processing unit 30, and the application part 3020 is a group of programs that use the functions provided by the library part 3010 to perform target processing.

[0278] As an example, in the case where the information processing unit 30 is applied to the control of an autonomously operated robot (mobile body), the library part 3010 may include a function of outputting a control command for driving the robot. In addition, the application part 3020 may include a function of determining the movement of the robot based on map information, motion meta information, image meta information, etc. transmitted from the signal processing unit 20a. As another example, in the case where the information processing unit 30 is applied to a control system of a monitoring camera, the library part 3010 may include a function of outputting a control command for controlling the operation of the monitoring camera. In addition, the application part 3020 may include a function of performing judgment based on an image from a monitoring camera, motion meta information, image meta information, etc. transmitted from the signal processing unit 20a.

[0279] Without being limited to this configuration, some or all of the units included in the library section 3010 and the application section 3020 may be realized by hardware circuits that operate in cooperation with each other. In addition, a typical computer may be used as the information processing unit 30 .

[0280] exist Fig.19A In the example of FIG. 1 , the output (IMU data, FMCW-LiDAR data, image data) of each sensor included in the sensor group 1000 is output as RAW data in a predetermined format under the control of the firmware 1010, and is transmitted to the signal processing unit 20a through an interface (not shown). The signal processing unit 20a receives the RAW data transmitted from the sensor unit 10a through the receiving unit 200 and transmits the RAW data to the library part 2021.

[0281] The library section 2010 performs the above-mentioned processing on the RAW data transferred from the receiving unit 200, and transfers the processed RAW data to the application section 2020. The application section 2020 performs the above-mentioned processing on the data transferred from the library section 2010, and the transmission unit 201 adds a time stamp in units of frames to each piece of information and a point cloud generated by the processing, and outputs the data with the time stamp. Fig.19A In the example of , the output of the signal processing unit 20 a is transmitted to the information processing unit 30 as SMART data.

[0282] The information processing unit 30 receives the SMART data transmitted from the signal processing unit 20 a and delivers the SMART data to the library part 3010 .

[0283] The library part 3010 performs processing related to individual functions on the received SMART data and transfers the data to the application part 3020. The application part 3020 performs predetermined processing on the data transferred from the library part 3010 to generate output data. For example, the data generated by the application part 3020 can be output from the information processing unit 30.

[0284] For example, the information processing unit 300 may generate a request to the signal processing unit 20a by the application section 3020, and send the generated request to the signal processing unit 20a. For example, the application section 3020 may generate a request specifying that the signal processing unit 20a can use each piece of information sent by the transmission unit 201, necessary information from the point cloud, and the point cloud, and send it to the signal processing unit 20a.

[0285] Similarly, for example, the signal processing unit 20a can generate a request to the sensor unit 10a using the application part 2020 and send the generated request to the sensor unit 10a. For example, the application part 2020 can generate and send a request to the light detection and ranging sensor 110 to limit the scanning range to the area of ​​interest.

[0286] exist Fig.19A In the example of the present invention, the Mobile Industry Processor Interface (MIPI) (registered trademark) is applied as an interface for data transmission from the sensor unit 10a to the signal processing unit 20a and data transmission from the signal processing unit 20a to the information processing unit 30. More specifically, the MIPI-Camera Serial Interface 2 (MIPI-CSI-2) may be applied as the interface.

[0287] Although the example in which the interface related to data transmission between the sensor unit 10a and the signal processing unit 20a and between the signal processing unit 20a and the information processing unit 30 is MIPI has been described above, the interface is not limited to this example. In this embodiment, another interface may be applied as the interface related to data transmission. For example, Fig.19B In the identification system 1a' shown, the interface between the sensor unit 10a and the signal processing unit 20a can be realized by using a serial interface (such as an inter-integrated circuit (IIC)) mainly used for data communication within the device. 2 C), serial peripheral interface (SPI) or universal asynchronous receiver / transmitter (UART)) or a communication interface such as a universal serial bus (USB) or Ethernet (registered trademark). In addition, the interface between the signal processing unit 20a and the information processing unit 30 can be implemented by applying an interface mainly used for external communication of the device, such as USB, Ethernet or Wireless Fidelity (Wi-Fi) (registered trademark).

[0288] Fig. 20 20a has a structure in which an operating system (OS) 2030 operates on a processor 2000 such as an ISP as a hardware component, and a library part 2010 and an application part 2020 operate on the OS 2030.

[0289] The application part 2020 includes an application programming interface (API) calling unit 2040, and the library part 2010 includes an API processing unit 2041. In response to a request from the application part 2020, the API calling unit 2040 calls a function of the library part 2010. In response to the function call of the API calling unit 2040, the API processing unit 2041 returns a response of the function in the library part 2010 to the API calling unit 2040.

[0290] As an example, in the application section 2020, in the case where the moving object state estimation unit 221 requests output of the moving object state information, the API calling unit 2040 requests the moving object speed point cloud from the library section 2010. In response to the request, the API processing unit 2041 calls the function of the moving object point cloud correction unit 214 in the library section 2010, and returns the moving object speed 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 object speed point cloud returned from the API processing unit 2041 to the moving object state estimation unit 221.

[0291] (2-2. Processing according to the first embodiment)

[0292] Next, the processing according to the first embodiment will be described in more detail. The processing described here is also applicable to each modification example of the first embodiment, the second embodiment, and the third embodiment.

[0293] (2-2-1. Measurement Technology Applicable to First Embodiment)

[0294] Fig.21 : is a flowchart showing an example of a transmitted light signal detection process in the light detection and ranging sensor 110 applicable to the first embodiment.

[0295] In step S10 , the light detection and ranging sensor 110 transmits (emits) a transmission light signal of laser light subjected to continuous frequency modulation in synchronization with a synchronization signal using the light transmission / reception unit 112 , and receives a reception light signal reflected on and returned from a target.

[0296] Fig. 22 is a schematic diagram showing a transmission optical signal transmitted by the optical transmission / reception unit 112 applicable to the first embodiment. Fig. 22 The upper diagram shows the relationship between the optical frequency and time of the transmitted optical signal, and the lower diagram shows the signal transmission start time t in the upper diagram. st .exist Fig. 22 In the upper diagram of , the vertical axis represents the optical frequency of the transmitted light signal, and the horizontal axis represents time.

[0297] exist Fig. 22 For example, the optical transmission / reception unit 112 uses the time t from the start of signal transmission st1 Until the next signal transmission starts time t st2 The optical frequency of the laser is linearly increased and decreased in a period of t, and chirped light is generated. The optical transmitting / receiving unit 112 transmits chirped light (chirp light) formed with a set of increasing and decreasing patterns of the optical frequency from the optical scanning unit 111 to a predetermined transmitting point as a transmitting optical signal. Similarly, at the signal transmission start time t st2 , the optical frequency of the transmission optical signal is linearly increased and decreased according to the above cycle to generate chirped light, and the generated chirped light is emitted from the optical scanning unit 111 toward a predetermined emission point as the transmission optical signal. In the raster scanning pattern, the transmission optical signal is formed by continuously transmitting chirped light having a set of optical frequency increase and decrease patterns.

[0298] return Fig.21 In the next step S11, the light detection and ranging sensor 110 estimates the first peak and the signal spectrum frequency (peak frequency) at the peak from the first received spectrum signal in the received received light signal using the received signal processing unit 113. In addition, the light detection and ranging sensor 110 estimates the second peak and the signal spectrum frequency (peak frequency) at the peak from the second received spectrum signal in the received received light signal using the received signal processing unit 113.

[0299] The first received spectrum signal is a signal of a partial signal of the received optical signal corresponding to the transmitted optical signal in a period of increasing optical frequency. The second received spectrum signal is a signal of a partial signal of the received optical signal corresponding to the transmitted optical signal in a period of decreasing optical frequency.

[0300] In step S12 , the reception signal processing unit 113 of the light detection and ranging sensor 110 determines whether the first peak value and the second peak value estimated in step S11 are a threshold value th or more.

[0301] Fig.23 FIG. 1 is a schematic diagram for illustrating a process of determining a received light signal according to the first embodiment. Fig.23 In the figure, part (a) is a simplified diagram showing the determination process of the first received spectrum signal, and part (b) is a simplified diagram showing the determination process of the second received spectrum signal. In the figures of parts (a) and (b), the vertical axis represents the intensity of the received optical signal, and the horizontal axis represents the received spectrum frequency.

[0302] exist Fig.23 In part (a), the intensity of the first received spectrum signal is at the signal spectrum frequency f pk1 exceeds the threshold value th, and obtains the first peak value Vpk1 The intensity of the first received spectrum signal is represented by the first peak value V pk1 The signal spectrum frequency f pk1 Set to the first peak frequency.

[0303] Similarly, in Fig.23 In part (b), the intensity of the second received spectrum signal is at the signal spectrum frequency f pk2 exceeds the threshold value th, and obtains the second peak value V pk2 The intensity of the second received spectrum signal is represented by the second peak value V pk2 The signal spectrum frequency f pk2 Set to the second peak frequency.

[0304] return Fig.21 In step S12, the received signal processing unit 113 has determined the first peak value V pk1 and the second peak V pk2 When at least one of the first peak value V is less than the threshold value th (step S12, "No"), the process proceeds to step S15. pk1 and the second peak V pk2 When each of is greater than or equal to the threshold value th (step S12, "Yes"), the reception signal processing unit 113 proceeds to the process of step S13.

[0305] In step S13, the received signal processing unit 113 processes the received signal based on the first peak value V pk1 and the second peak V pk2 and has a first peak value V pk1 and the second peak V pk2 The signal spectrum frequency f pk1 and f pk2 The distance to the object, the Doppler velocity relative to the object, and the intensity of the received light signal are calculated. In addition, the received signal processing unit 113 calculates the signal transmission start time t based on the synchronization signal. st1 .

[0306] will be with the first peak V pk1 and the second peak V pk2 The emission points 41 corresponding to the received light signals that are equal to or greater than the threshold value th are set as the detection points where the subject is detected.

[0307] In the next step S14, the reception signal processing unit 113 outputs the distance, Doppler velocity, and intensity calculated in step S13. Furthermore, the reception signal processing unit 113 outputs the signal transmission start time calculated in step S13 as a type stamp for each measurement (for each transmission point).

[0308] The transmission unit 115 receives the distance, Doppler velocity, intensity, and time stamp as data output from the reception signal processing unit 113. The transmission unit 115 adds a time stamp to the received distance, Doppler velocity, and intensity, and outputs the data with the time stamp as FMCW-LiDAR data of the light detection and ranging sensor 110.

[0309] In the next step S15, the light detection and ranging sensor 110 determines whether the measurement has been completed. For example, the light detection and ranging sensor 110 may determine whether the measurement has been completed based on a control signal (not shown) input from the outside to the sensor unit 10. When it is determined that the measurement is completed, the light detection and ranging sensor 110 determines whether the measurement has been completed based on the control signal (not shown) input from the outside to the sensor unit 10. Fig. 22 The flowchart completes a series of processes.

[0310] In contrast, when it is determined in step S15 that the measurement has not been completed, the light detection and ranging sensor 110 returns the process to step S10 , and performs the process for the next emission point.

[0311] A specific example will be used to describe Fig.21 The determination processing in step S12 in the flowchart of FIG.

[0312] Fig.24 1 is a schematic diagram showing an example of data output in the case where the light detection and ranging sensor 110 according to the first embodiment performs scanning using the raster scanning mode. Fig.24 In the above, part (a) corresponds to Fig.13 , and the vertical axis represents the angle in the vertical direction of the scan, and the horizontal axis represents the angle in the horizontal direction of the scan. Part (b) shows an example in which the FMCW-LiDAR data output from the light detection and ranging sensor 110 corresponding to each emission point 41 is arranged in a time series. In addition, the emission points 41a, 41b shown in the solid line in the figure are, for example, the emission points 41a, 41b corresponding to the objects 50a, 50b (see Figure 4 )'s existence position corresponds to the emission point, and the reflected light, that is, the light reflected by objects 50a, 50b, is returned as a received light signal.

[0313] The light detection and ranging sensor 110 uses the optical scanning unit 111 to scan between the upper and lower ends of the angle range 45 according to the scanning line 40 folded within the predetermined angle range 45 at both ends in the horizontal direction of the angle range 45. Each of the emission points 41, 41a and 41b within the angle range 45 constitutes a point cloud framework.

[0314] like Fig.24As shown in part (b) of the embodiment, for example, output data of each emission point 41 is output at predetermined time intervals according to the emission timing of the transmitted light signal. In this case, the emission points 41 (indicated by dotted lines in the figure) other than the emission points 41a and 41b corresponding to the positions where the objects 50a and 50b exist can be regarded as points where no light signal corresponding to the transmitted light signal is received in the light detection and ranging sensor 110, or the received light signal is noise.

[0315] Reference Fig.23 For example, at the transmitting points 41a and 41b, a first peak value V is obtained in which the intensity of the received optical signal is a threshold value th or more. pk1 and the second peak V pk2 Therefore, according to Fig.21 The determination in step S12 in the flowchart of (step S12, "Yes"), the processing proceeds to step S13.

[0316] On the other hand, in Fig.24 At the emission point 41 indicated by the dotted circle, the first peak value V pk1 and the second peak V pk2 At least one of is not clearly obtained or is less than the threshold value th. Fig.21 The determination of step S12 in the flowchart of (step S12, "No"), the processing proceeds to step S15.

[0317] In this way, among the emission points included in the point cloud frame, the emission points 41a and 41b corresponding to the positions where the objects 50a and 50b exist are performed. Fig.21 On the other hand, among the emission points 41 shown by the dotted circle in the figure except for the emission points 41a and 41b, the processing of steps S13 and S14 is released among the emission points included in the point cloud frame, and the processing of the next emission point is performed.

[0318] (2-2-2. Data Structure Example According to First Embodiment)

[0319] Next, a data structure example according to the first embodiment will be described. A data structure example regarding each data transmitted from the sensor unit 10a to the signal processing unit 20a will be specifically described below.

[0320] Fig.25 FIG. 1 is a schematic diagram for illustrating a frame definition applicable to an embodiment. Fig.25 , part (a) shows an example of a frame based on image data output from the image sensor 120 . Fig.25Parts (b) and (c) of FIG. 1 each show an example of a frame based on FMCW-LiDAR data output from the light detection and ranging sensor 110. Here, part (b) shows an example of a raster scanning pattern used in raster scanning, and part (c) shows an example of a point scanning pattern used in point scanning.

[0321] Raster scanning is achieved, for example, by a mechanical mirror scanner.Point scanning is achieved, for example, by a beam steering device such as an optical phase array (OPA) or a beam switching element.

[0322] exist Fig.25 In the image data shown in part (a) of FIG. 1 , a group of pixels 61 including information of respective colors of red (R), green (G), and blue (B) constitute a frame 60a. The size of the frame 60a is represented by the number of pixels 61 in the width direction and the height direction. Each pixel 61 is arranged in a matrix array corresponding to each pixel included in the effective pixel area of ​​the pixel array.

[0323] In accordance with Fig.25 In the FMCW-LiDAR data of the raster scanning pattern shown in part (b) of FIG. 1 , the emission points 41 within the predetermined angle range 45 constitute the frame (point cloud frame) 60b. In the raster scanning pattern, as described above, scanning is performed according to the scanning line 40 that is turned back in the horizontal direction of the angle range 45. The position of each emission point 41 on the scanning line 40 is determined by the scanning angle (horizontal angle θh) in the horizontal and vertical directions of the transmission light signal from the optical scanning unit 111. i , vertical angle θv i )express.

[0324] In the raster scanning mode, as described above, a set of rising and falling chirped lights of the optical frequency are successively transmitted to form a transmission optical signal. Fig.25 Each transmission point 41 shown in part (b) of the present invention may be defined as corresponding to, for example, a signal transmission start time point t st1 , t st2 , ... Scanning angle (horizontal angle θh i and vertical angle θv i ). In chirped light, the frequency of light begins to rise.

[0325] In accordance with Fig.25 In the FMCW-LiDAR data of the point scanning mode shown in part (c) of FIG. 4 , each emission point 43 within a predetermined scanning range 46 constitutes a frame (point cloud frame) 60c. In the point scanning mode, chirped light is performed for each emission point 43. That is, a transmission light signal for each emission point 43 is transmitted. Therefore, the position of each emission point 43 can be represented by an xy coordinate in the scanning range 46.

[0326] Fig.26A and Fig.26B 1 is a schematic diagram for illustrating the output timing of each data of the sensor unit 10 a according to the first embodiment. Here, it is assumed that the image sensor 120 performs exposure by a global shutter system that exposes all pixels at the same time.

[0327] Fig.26A An example of measurement timing of each sensor of the sensor unit 10a according to the first embodiment is shown. In the sensor unit 10a, the IMU 100, the light detection and ranging sensor 110, and the image sensor 120 each perform a measurement operation in synchronization with the synchronization signal Sync provided from the synchronization signal generator 130, and output data. Note that it is assumed that the synchronization signal Sync is output from the synchronization signal generator 130 in a period corresponding to one frame period of image data.

[0328] The image sensor 120 is synchronized with the synchronization signal Sync at the exposure start time t exst1 The exposure starts, and during the exposure period t ex The exposure starts at time t after one frame period of image data. exst2 , start the exposure of the next frame. The frame period of the image data is about 10ms (milliseconds) to 100ms. For example, from the exposure period t ex The end of exposure to the next start time t exst2 During the period of time, the output is from the exposure start time t exst1 Start exposure of one frame of image data.

[0329] According to the synchronization signal Sync, the light detection and ranging sensor 110 starts at the frame start time tf st1 , tf st2 , ..., starts scanning a frame at each point. For example, at the exposure start time point t exst1 ,t exst2 ,...synchronous. Fig.26B Schematically shows the exposure start time t in the image sensor 120 exst The frame start time t fst The relationship between Fig.26B As shown in FIG. 1 , the emission timing of the emission light signal at the emission point 41st at the head of the frame 60b by the light detection and ranging sensor 110 is related to the exposure start time t of each pixel 61 in the frame 60a by the image sensor 120. exst synchronous.

[0330] In the light detection and ranging sensor 110 , for example, the emission interval of the transmission light signal at each emission point 41 is set to about 1 μs (microseconds) to several 100 μs. FMCW-LiDAR data is output according to the emission timing of each emission point 41 .

[0331] Here, the transmission point 41 shown with a dotted line in the figure represents a transmission point where the transmission light signal is not reflected by an object or the like and thus the reception light signal is not received, or a transmission point where the intensity of the reception light signal is less than, for example, a threshold value th. In addition, the transmission point 41 shown with a solid line (hatched line) in the drawing represents, for example, a transmission light signal has been reflected by an object or the like and thus a reception light signal having an intensity of the threshold value th or more has been received. Therefore, the FMCW-LiDAR data is actually output only at the timing corresponding to the transmission point 41 shown with a hatched solid line in the figure.

[0332] The IMU 100 outputs the IMU data in synchronization with the head of the frame of the image data and the FMCW-LiDAR data according to the synchronization signal Sync. The IMU 100 may output the IMU data according to a signal obtained by, for example, multiplying the synchronization signal Sync in a period from the synchronization signal Sync to the next synchronization signal Sync. For example, the output interval of the IMU data in the IMU 100 is set to about 1 ms to 1000 ms.

[0333] Fig. 27 : is a schematic diagram showing an example of the output rate of each data according to the first embodiment. Among the data output from the SLAM processing unit 220, the sensor position / orientation, and sensor speed / angular velocity data, and the mobile object state data (mobile object position / orientation, and mobile object speed / angular velocity) output from the mobile object state estimation unit 221 may have an output rate of about 10 Hz according to the output timing of the image sensor 120. In this case, each data is output every 0.1 seconds.

[0334] On the other hand, the generation process of the map information in the SLAM processing unit 220 includes a process of combining with reference to the map information having a large information capacity stored in the map storage unit. Therefore, the output rate of the map information is generally lower than the output rate of each data measured by the above-mentioned sensor. Fig. 27 In the example of FIG. 1 , the output rate of the map information is set to about 1 Hz, which is lower than the output rate of each data measured by the sensor or the moving object state data.

[0335] Note that the data including the map information output from the SLAM processing unit 220 and the moving object state data output from the moving object state estimation unit 221 are preferably synchronized with each other. Fig. 27Each output rate shown in is an example and is not limited to these values.

[0336] Fig.28 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. Fig.28 , part (a) shows an example of the data structure of FMCW-LiDAR data, and part (b) shows an example of the data structure of IMU data. In sections (a) and (b), time is represented in the vertical direction of the figure.

[0337] exist Fig.28 In part (a), in FMCW-LiDAR data, for example, a header is transmitted at the head of a frame, and then data corresponding to each transmission point is transmitted sequentially after the header. It should be noted that in the figure, each transmission point is represented as point #1, point #2, ..., and point #N. In the following, each transmission point is appropriately described as point #1, point #2, ..., and point #N. In addition, in the case where there is no need to particularly distinguish between point #1, point #2, ... to point #N, the description will be represented by point #i, and each number using a subscript in the figure is also represented by i (the same applies to part (b)).

[0338] The header includes at least scanning type information. The scanning type information may be information indicating a scanning method such as raster scanning, multi-layer scanning, or point scanning.

[0339] The data of point #i includes the timestamp Ts i and scanning angle (horizontal angle θh i and vertical angle θv i ) as information related to the scan, and includes the distance d i , Doppler velocity w i and intensity Lp i As the measurement data obtained based on the received light signal. Timestamp Ts i Indicates the time at which the transmitted optical signal is transmitted at point #i.

[0340] In addition, each point #i may be a point where the intensity of the received light signal in the first received light spectrum signal and the second received light spectrum signal increases to a value above the threshold value th. i is added to the data of each point #i, so it is easy to grasp the basis of the data of each point i contained in one frame of FMCW-LiDAR data, that is, which transmission point the data that is the basis of the received light signal corresponds to.

[0341] In addition, there is a case where the processing of the data of each point #i in the signal processing unit 20 (for example, the correction processing of the stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214) is different according to the scanning method. In an embodiment, the header transmitted at the head of the frame includes scanning type information indicating the scanning method. This allows the signal processing unit 20 to perform processing on the FMCW-LiDAR data according to the scanning method.

[0342] exist Fig.28 In part (b), in the IMU data, the timestamp Ts-IMU i 、Triaxial acceleration a i , three-axis angular velocity ωi and three-axis geomagnetic field ge i The coordinates are transmitted sequentially at each measurement moment in the IMU 100. In the IMU data, for example, a timestamp Ts-IMU is added. i The first data of the frame is synchronized with the data of the first point #1 in the FMCW-LiDAR data shown in part (a).

[0343] Here, the IMU 100 performs measurement at constant time intervals. Therefore, for example, by synchronizing the first data of the frame with the data of point #1 of the FMCW-LiDAR data, the measurement timing of each data can be estimated and each time stamp Ts-IMU can be omitted. i .

[0344] (2-2-3. Details of Processing According to First Embodiment)

[0345] Next, the processing according to the first embodiment will be described in more detail.

[0346] Fig.29 is a flowchart showing an example of processing performed by the signal processing unit 20 a according to the first embodiment.

[0347] exist Fig.29 In the flowchart of FIG. 1 , steps S100 to S101 represent processing related to IMU data, steps S110 to S134 represent processing related to FMCW-LiDAR data, and steps S140 to S146 represent processing related to image data. It should be noted that, for example, by the light detection and ranging sensor 110, for each measurement of a frame, the Fig.29 Flowchart of the process.

[0348] exist Fig.29In step S100, the signal processing unit 20a acquires IMU data from the IMU 100 through the receiving unit 200. In the next step S101, the signal processing unit 20a estimates the current sensor position, orientation, and angular velocity based on the IMU data acquired in step S100 and the sensor position, orientation, and angular velocity acquired in the previous process using the sensor position / orientation estimation unit 210. The sensor position / orientation estimation unit 210 passes the estimated sensor position, orientation, and angular velocity to the process of step S121, and enters the process of step S121.

[0349] In step S110, the signal processing unit 20a acquires FMCW-LiDAR data from the light detection and ranging sensor 110 using the receiving unit 200. In the next step S111, the signal processing unit 20a uses the moving object / stationary object separation unit 211 to distinguish and cluster the velocity point cloud included in the FMCW-LiDAR data, and then extracts the point cloud clusters of each stationary object and moving object. The signal processing unit 20a corrects the extracted point cloud clusters of the stationary object through the stationary object point cloud correction unit 213. In addition, the signal processing unit 20a corrects the extracted point cloud clusters of the moving object using the moving object point cloud correction unit 214.

[0350] Hereinafter, steps S120 to S122 are a series of processes for the point cloud cluster of a stationary object. In addition, steps S130 to S134 are a series of processes for the point cloud cluster of a moving object.

[0351] In step S120, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to obtain a stationary object velocity point cloud based on the stationary object point cloud group identified, clustered, and corrected in step S111. In the next step S121, the signal processing unit 20a uses the SLAM processing unit 220 to perform SLAM processing using the stationary object velocity point cloud acquired in step S12 and the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimation unit 210 in step S101.

[0352] In the next step S122 , the signal processing unit 20 a uses the transmission unit 201 to output its own position, orientation, speed, and angular speed acquired by the SLAM processing in step S121 .

[0353] In step S130, the signal processing unit 20a uses the moving object point cloud correction unit 214 to obtain a velocity point cloud among the point cloud clusters (velocity point cloud clusters) of each moving object identified, clustered, and corrected in step S111. The acquired velocity point cloud is a local velocity point cloud of the velocity point cloud of the moving object that exists locally. For example, a local velocity point cloud is formed for each moving object corresponding to each moving object.

[0354] In the next step S131, the signal processing unit 20a uses the mobile object state estimation unit 221 to determine whether the number of points included in the local point cloud of the mobile object obtained in step S130 is greater than a certain number. When it is determined that the number of points included in the target local point cloud is less than a certain number (step S131, "No"), the mobile object state estimation unit 221 returns the processing to step S130 and performs processing on the next local point cloud. On the contrary, when it has been determined in step S131 that the number of points included in the target local point cloud is a specific number or more (step S131, "Yes"), the mobile object state estimation unit 221 proceeds to the processing of step S132.

[0355] In step S132, the mobile object state estimation unit 221 estimates the mobile object state information of the mobile object based on the position of the mobile object corresponding to the local velocity point cloud acquired in step S130 and the velocity point cloud of the mobile object. Here, the mobile object state information refers to the area information indicating the area of ​​the mobile object in the local velocity point cloud, and may include, for example, the position, orientation, velocity, angular velocity, and size of the mobile object.

[0356] The signal processing unit 20a converts the moving object state information estimated by the moving object state estimation unit 221 in step S132 into 2D data using the 3D / 2D converter 222, passes the obtained 2D data to the process of step S141, and then proceeds to the process of step S133.

[0357] In step S133, the signal processing unit 20a uses the mobile object state estimation unit 221 to determine whether the processing of all partial point clouds of the point cloud cluster of the mobile object extracted and corrected in step S111 is completed. When it is determined that the processing is not completed (step S133, "No"), the mobile object state estimation unit 221 returns the processing to step S130 and performs processing on the next partial point cloud. On the contrary, when the mobile object state estimation unit 221 has determined in step S133 that the processing of all partial point clouds is completed (step S133, "Yes"), the processing proceeds to step S134.

[0358] In step S134 , the signal processing unit 20 a outputs the moving object state information of each moving object acquired in the processing of steps S130 to S133 using the transmission unit 201 .

[0359] In step S140, the signal processing unit 20a acquires image data from the image sensor 120 using the receiving unit 200. In the next step S141, the signal processing unit 20a uses the ROI extraction unit 223 to extract a partial image of each moving object from the entire image based on the image data acquired in step S140 by using the region information of each moving object based on the 2D data, which is the data obtained by converting the moving object state information and passed from the moving object state estimation unit 221 in step S132.

[0360] In the next step S142, the signal processing unit 20a performs recognition processing on one of the partial images extracted in step S141 using the image recognition unit 226, and estimates attribute information related to the partial image. In the next step S143, the signal processing unit 20a uses the image recognition unit 226 to determine whether the confidence level of the recognition result of the recognition processing in step S142 is a specific value or more.

[0361] In step S143, when the image recognition unit 226 determines that the confidence level of the recognition result is less than a specific value (step S143, "No"), the signal processing unit 20a returns the process to step S142 and performs processing on the next portion of the image. On the contrary, when the image recognition unit 226 determines in step S143 that the confidence level of the recognition result is above a specific value (step S143, "Yes"), the signal processing unit 20a proceeds to the process of step S144.

[0362] In step S144, the signal processing unit 20a estimates the mobile object state information of the mobile object based on the position of the mobile object and the velocity point cloud of the mobile object using the image recognition unit 226, corresponding to the partial image for which the recognition process has been performed in step S142. Here, the mobile object state information refers to the area information indicating the area of ​​the mobile object in the local velocity point cloud, and may include, for example, the position, orientation, velocity, angular velocity, and size of the mobile object.

[0363] In the next step S145, the signal processing unit 20a uses the image recognition unit 226 to determine whether the processing of all partial images extracted in step S141 is completed. When it is determined that the processing for all partial images is not completed (step S145, "No"), the image recognition unit 226 returns the processing to step S142 and performs processing for the next partial image. On the contrary, when the image recognition unit 226 has determined that the processing of all partial images is completed (step S145, "Yes"), the signal processing unit 20a proceeds to the processing of step S146.

[0364] In step S146 , the signal processing unit 20 a outputs the attribute information of each partial image estimated by the image recognition unit 226 as image meta information of each moving object corresponding to each partial image using the transmission unit 201 .

[0365] For example, the processing of the above steps S100, S110 and S140 may be performed in synchronization with the start time of the measurement processing of one frame by the light detection and ranging sensor 110. In addition, for example, the data output in steps S122, S134 and S146 may refer to Fig. 27 The output rates described are output synchronously with each other. In this case, for example, the image meta information output by the image recognition unit 226 can be output at an output timing corresponding to the map information. Alternatively, the image meta information can be output at an output timing corresponding to another piece of information.

[0366] Fig.30 The following is a flowchart showing an example of processing related to point cloud correction in the signal processing unit 20a according to the first embodiment. Fig.30 and Fig.31 in the processing. Fig.30 The processing is similarly applied to each modification of the first example, and the second and third embodiments.

[0367] In step S200 , the signal processing unit 20 a starts acquiring frames based on the FMCW-LiDAR data output from the light detection and ranging sensor 110 .

[0368] In the next step S201, the signal processing unit 20a uses the moving object / stationary object separation unit 211 to obtain the distance and Doppler velocity of the detection point where the received light signal has been detected, as well as the detection time and the scanning angle. In the next step S202, the signal processing unit 20a uses the moving object / stationary object separation unit 211 to obtain the sensor velocity estimated by the sensor velocity estimation unit 212 in the previous processing. In the next step S203, the signal processing unit 20a uses the moving object / stationary object separation unit 211 to perform coordinate transformation on the Doppler velocity at the detection point by using the sensor velocity acquired in step S202, and calculates the corrected Doppler velocity.

[0369] In the next step S204 , the signal processing unit 20 a determines whether the absolute value of the corrected Doppler velocity of the detection point calculated in step S203 is equal to or less than a threshold value using the moving object / stationary object separation unit 211 .

[0370] When the moving object / stationary object separation unit 211 has determined that the absolute value of the corrected Doppler velocity at the detection point is equal to or less than the threshold value (step S204, "Yes"), the signal processing unit 20a proceeds to the processing of step S210, which is a step related to the processing of the stationary object point cloud.

[0371] In step S210, the signal processing unit 20a adds the detection point to the velocity point cloud frame of the stationary object using the stationary object point cloud correction unit 213. In the next step S211, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to obtain the sensor orientation and angular velocity estimated by the sensor position / orientation estimation unit 210. In the next step S212, the signal processing unit 20a uses the sensor velocity estimation unit 212 to estimate the sensor velocity by using the velocity point cloud frame of the stationary belt, the sensor orientation, and the angular velocity.

[0372] In the next step S213, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to correct the speed point cloud frame of the stationary object by using the sensor speed estimated in step S212, the sensor orientation and angular velocity acquired in step S211, and the time difference from the previous processing.

[0373] In the next step S214, the signal processing unit 20a determines whether the stationary object point cloud correction unit 213 has completed the processing for all points in the frame. In the case where the stationary object point cloud correction unit 213 determines that the processing is completed (step S214: "Yes"), the signal processing unit 20a enters the processing of step S230. On the contrary, when it is determined that the stationary object point cloud correction unit 213 has not completed the processing (step S214, "No"), the signal processing unit 20a returns the processing to step S201 and performs the processing for the next detection point in the frame based on the FMCW-LiDAR data.

[0374] When the moving object / stationary object separation unit 211 has determined in the above-mentioned step S204 that the absolute value of the corrected Doppler velocity at the detection point exceeds the threshold value (step S204, "No"), the signal processing unit 20a proceeds to the processing of step S220, which is a step related to the processing of the moving object point cloud.

[0375] In step S220, the signal processing unit 20a uses the moving object point cloud correction unit 214 that corrects the moving object point cloud to add the detection point to the velocity point cloud frame of the moving object. In the next step S221, the signal processing unit 20a uses the moving object point cloud correction unit 214 to obtain the sensor orientation and angular velocity estimated by the sensor position / orientation estimation unit 210.

[0376] In the next step S212, the signal processing unit 20a performs clustering on the velocity point cloud of the moving object using the moving object point cloud correction unit 214, and divides the velocity point cloud of the moving object into local velocity point clouds. That is, the signal processing unit 20a uses the moving object point cloud correction unit 214 to divide the velocity point cloud included in the velocity point cloud frame of the moving object into the velocity point cloud of each moving object.

[0377] In the next step S223, the signal processing unit 20a uses the moving object point cloud correction unit 214 to correct the point cloud cluster frame (local velocity point cloud) of each moving object in the velocity point cloud frame by using the sensor speed, sensor orientation, angular velocity, the position, orientation, speed, angular velocity of the moving object, and the time difference from the previous processing.

[0378] In the next step S224, the signal processing unit 20a determines whether the moving object point cloud correction unit 214 has completed the processing for all points in the frame. When it is determined that the moving object point cloud correction unit 214 has completed the processing (step S224, "Yes"), the signal processing unit 20a proceeds to the processing of step S230. On the contrary, when it is determined that the moving object point cloud correction unit 214 has not completed the processing (step S224, "No"), the signal processing unit 20a returns the processing to step S201 and performs the processing of the next detection point in the frame.

[0379] In step S230, the signal processing unit 20a outputs a stationary object speed point cloud frame formed by the stationary object point cloud corrected by the stationary object point cloud correction unit 213 and a moving object speed point cloud frame formed by the moving object point cloud corrected by the moving object point cloud correction unit 214. For example, the output stationary object speed point cloud frame is transferred to the SLAM processing unit 220. In addition, for example, the output moving object speed point cloud frame is transferred to the moving object state estimation unit 221.

[0380] Fig.31 1 is a flowchart showing an example of a correction process of a velocity point cloud frame of a stationary object in the signal processing unit 20 a according to the first embodiment. Fig.31 The flowchart shows in more detail Fig.30 The processing of step S213 in the above flowchart.

[0381] In step S250, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to obtain the sensor speed estimated by the sensor speed estimation unit 212. In the next step S251, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to obtain the coordinates of each point included in the speed point cloud frame of the stationary object. In the next step S252, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to obtain the sensor orientation and acceleration estimated by the sensor position / orientation estimation unit 210. In the next step S253, the signal processing unit 20a estimates the instantaneous velocity vector of each point included in the speed point cloud frame of the stationary object using the stationary object point cloud correction unit 213 based on each value obtained in steps S250 to S252.

[0382] In the next step S254, the signal processing unit 20a uses the stationary object point cloud correction unit 213 to calculate the micro displacement of each point based on the product of the differential time during the previous processing and the instantaneous velocity vector of each point. The signal processing unit 20a uses the stationary object point cloud correction unit 213 to offset the position of each point based on the calculated micro displacement of each point, and correct the velocity point cloud frame of the stationary object.

[0383] (2-2-4. Point Cloud Correction Processing According to the First Embodiment

[0384] Next, the point cloud correction process according to the first embodiment will be described. Fig.17 The illustrated stationary object point cloud correction unit 213 and moving object point cloud correction unit 214 perform point cloud correction processing according to the first embodiment.

[0385] Fig.32 1 is a schematic diagram showing an example of output data of the light detection and ranging sensor 110 according to the first embodiment. The light detection and ranging sensor 110 outputs the detection time t of the detection point i. i , the position vector r of the detection point i i and the Doppler velocity w of the detection point i i In the following description, the detection point i represents an emission point at which the intensity of each of the first received spectrum signal and the second received spectrum signal in the received light signal is equal to or greater than the threshold value th.

[0386] Fig.33 : is a schematic diagram showing the definition of the coordinates of the emission point applicable to the first embodiment. Fig.33 In the figure, when the center of the angular range 45 associated with scanning is set as the origin, a vertical angle θ is defined in the vertical direction. v And define the horizontal angle θ in the horizontal direction h When the emission point 41 at the upper left corner of the angle range 45 is set as point #1, points such as point #1, #2, ... are set from left to right so as to reach the lower right corner of the angle range 45 of the emission point 41, which is set as point #N. The emission point 41 at a position corresponding to the object 52 of the measurement object among the emission points 41 is set as the detection point i.

[0387] Fig.34 is a schematic diagram showing each emission point 41 and the object 52 as viewed from the light detection and ranging sensor 110 .

[0388] The following will describe Fig.34 as well as Fig.35 and Fig.36 The symbols used in and the like, and each mathematical expression to be described below can be replaced with the following mathematical expression.

[0389] Vectors and mathematical expressions in bold in the figures are indicated by an "_" (underscore) immediately preceding them, such as "Vector_x".

[0390] In the drawings and mathematical expressions, a character "→" (arrow) added immediately above to indicate a vector is represented by adding "→" (arrow) immediately before the character, such as "vector→x".

[0391] In the drawings and mathematical expressions, a superscript “s” attached to a character indicates that the value represented by the character is a value in the sensor coordinate system.

[0392] In the drawings and mathematical expressions, a superscript "m" attached to a character indicates that the value represented by the character is a value in a moving object / rigid body coordinate system.

[0393] In the drawings and mathematical formulas, a character with a “·” (dot) immediately above it to indicate a time derivative is represented by adding the “·” immediately before the character, for example, “·x”.

[0394] In the drawings and mathematical formulas, a character with a "~" (tilde) added immediately above it to indicate an angular velocity tilde matrix is ​​represented by adding "~" before the character, such as "~x".

[0395] exist Fig.34 When passing through position O s The position of the light detection and ranging sensor 110 is set and the position O s When it is set as the origin, through the coordinate axis x s ,y s and z s The sensor coordinate system is defined. In addition, for convenience, each emission point 41 is shown as a point equidistant from the light detection and ranging sensor 110 .

[0396] exist Fig.34 In the example, at the detection point i corresponding to object 52, the vector →v i represents the motion of object 52, and the vector →w i represents the position O of the object 52 relative to the sensor s The Doppler velocity of s The vector starting from the detection point i →r i represents the distance from the light detection and ranging sensor 110 to the detection point i in the object 52. The final target information to be obtained is the vector →v i .

[0397] Fig.35 2 is a schematic diagram showing the overall flow of processing related to point cloud correction in the signal processing unit 20 a according to the embodiment.

[0398] exist Fig.35In the process, the sensor position / orientation estimation process 400, the stationary object / moving object discrimination process 401, and the sensor speed estimation process 402 are respectively performed by Fig.17 The point cloud correction processing 403 and 404 are respectively performed by the sensor position / orientation estimation unit 210, the moving object / stationary object separation unit 211, and the sensor speed estimation unit 212. Fig.17 The stationary object point cloud correction unit 213 and the moving object point cloud correction unit 214 in the processing are performed. In addition, the moving object detection processing 405 is performed by Fig.17 The processing performed by the moving object state estimation unit 221 in Fig.17 The processing performed by the SLAM processing unit 220 in is the SLAM processing 410.

[0399] IMU 100 output acceleration_a s , angular velocity_ω s and geomagnetic_μ s As IMU data. Based on the acceleration_a output from the IMU 100 s , angular velocity_ω s and geomagnetic_μ s , and the sensor position _p obtained by SLAM processing 410 s,j , sensor facing R s,j 、Sensor angular velocity_ω s,j and sensor speed_v s,j , sensor position / orientation estimation process 400 estimates sensor position _p s , sensor facing R s and sensor angular velocity_ω s The sensor position / orientation estimation process 400 converts the estimated sensor position p s is passed to the SLAM process 410. In addition, the sensor position / orientation estimation process 400 converts the estimated sensor orientation R s and sensor angular velocity ω s Passed to sensor speed estimation processing 402.

[0400] The light detection and ranging sensor 110 outputs the detection time t i , detection time t i The distance of the position _r s i (t i ), and the Doppler velocity w of each detection point i i (t i ) as FMCW-LiDAR data. Each piece of data output from the light detection and ranging sensor 110 is passed to the stationary object / moving object discrimination processing 401.

[0401] The stationary object / moving object discrimination process 401 is based on the various data transmitted from the light detection and ranging sensor 110 and the sensor speed v estimated by the sensor speed estimation process 402. s , for stationary objects at detection time t i The position of s i (t i )} i＝static , Doppler velocity {w i (t i )} i＝static , and the stationary object detection time {t i} i＝static .

[0402] Brackets "{" and "}" indicate that the enclosed parts are a set.

[0403] The stationary object / moving object discrimination processing 401 calculates the distance {_r s i (t i )} i＝static and Doppler velocity {w i (t i )} i＝static The sensor speed estimation process 402 is passed on. In addition, the stationary object / moving object discrimination process 401 calculates the distance {_r s i (t i )} i＝static and the stationary object detection time {t i} i＝static The point cloud is passed to the point cloud correction process 403 which corrects the stationary object point cloud.

[0404] In addition, based on the various data transmitted from the light detection and ranging sensor 110 and the sensor speed estimated by the sensor speed estimation process 402, s The stationary object / moving object identification process 401 is for the detection time t i The distance of the moving object is calculated by s i (t i )} i＝dynamic , Doppler velocity {w i (t i )} i＝dynamic , and the stationary object detection time {t i} i＝dynamic .

[0405] The stationary object / moving object discrimination processing 401 calculates the distance {_rs i (t i )} i＝dynamic , Doppler velocity {w i (t i )} i＝dynamic and the stationary object detection time {t i} i＝dynamic The point cloud is passed to the point cloud correction process 404 which corrects the moving object point cloud.

[0406] The sensor speed estimation process 402 is based on the sensor orientation R obtained from the sensor position / orientation estimation process 400. s and sensor angular velocity ω s , and the distance {_t s i (t i )} i＝static and Doppler velocity {w i (t i )} i＝static , calculate the sensor speed _v s The sensor speed estimation process 402 calculates the sensor speed _v as the speed of the sensor unit 10. s Passed to the stationary object / moving object identification process 401, the point cloud correction processes 403 and 404, and the SLAM process 410.

[0407] Based on the sensor orientation R obtained from the sensor position / orientation estimation process 400 s and sensor angular velocity ω s , and the distance {_r s i (t i )} i＝static and the stationary object detection time {t i} i＝static , point cloud correction processing 403 calculates the distance {_r s i (t N )} i＝static As the correction position of the stationary object point cloud. The point cloud correction processing 403 calculates the distance {_r s i (t N )} i＝static Passed to SLAM processing 410.

[0408] Based on the sensor orientation R passed from the sensor position / orientation estimation process 400 s and sensor angular velocity ω s , the distance {_rs i (t i )} i＝dynamic , Doppler velocity {w i (t i )} i＝dynamic , the stationary object detection time {t i} i＝dynamic , the sensor speed v after the sensor speed estimation process 402 s , the sensor position _p passed from the SLAM processing 410 s,j 、Moving object position {_p m} j , moving the object towards {R m} and {_v m} j , and based on the angular velocity of the moving object {~ω m} j , the point cloud correction process 404 transferred from the moving object detection process 405 calculates the corrected moving object point cloud {r s i (t N )} i＝dynamic and the corrected Doppler velocity {w' i (t N )} i＝dynamic The point cloud correction processing 404 converts the calculated corrected moving object point cloud {r s i (t N )} i＝dynamic and the corrected Doppler velocity {w' i (t N )} i＝dynamic Passed to the moving object detection processing 405.

[0409] Based on the corrected moving object point cloud {_r s i (t N )} i＝dynamic and the corrected Doppler velocity {{w' i (t N )} i＝dynamic , the moving object detection process 405 calculates the moving object position {_p m} j 、Moving object towards {R m} j and {_v m} j 、Angular velocity of moving object {~ω m} j And the range of moving objects {Em} j , and then outputs the calculated data. In addition, the moving object detection processing 405 outputs the detection time t of each frame j For example, each value output from the moving object detection process 405 is passed to the transmission unit 201 (refer to Fig.17 ). In addition, the moving object detection processing 405 calculates the moving object position {_p m} j , moving the object towards {R m} j and {_v m} j , and the angular velocity of the moving object {~ω m} j Transmitted to point cloud correction processing 404.

[0410] The SLAM process 410 is based on the sensor position / orientation estimation process 400. s , sensor facing R s and sensor angular velocity ω s , sensor speed _v after the sensor speed estimation process 402 s , and the corrected stationary object point cloud {_r s i (t N )} i＝static To create and output a map <_m> ("<" and ">" represent outer brackets). In addition, the SLAM process 410 outputs the sensor position _p s,j , sensor facing R s,j 、Sensor angular velocity ~ω s,j and sensor speed_v s,j In addition, the SLAM process 410 outputs the detection time t for each frame. j Each value output from the SLAM process 410 is passed to, for example, the transmission unit 201 (see Fig.17 ).

[0411] (Description of Theoretical Formula Using Process According to First Embodiment)

[0412] Here, the theoretical formula will be used to describe the reference Fig.35 The processing in the signal processing unit 20a according to the first embodiment is described.

[0413] (Processing related to stationary objects)

[0414] First, the processing related to stationary objects will be described. Fig.36 is a schematic diagram used to define the coordinate system and each variable used in the following description. Fig.36 In the embodiment, when the position of the light detection and ranging sensor 110 (hereinafter, the sensor) is set to position O s _ and position O s _ is set as the origin, the sensor coordinate system consists of mutually orthogonal coordinate axes x s ,y s and _z s Definition. The angular velocity and speed of the sensor are defined as angular velocity →ωs and speed →v respectively. s .

[0415] In a fixed object (rigid body), if the position O on the rigid body m As the origin, the orthogonal axis x m ,y m 、z m The angular velocity and velocity of the rigid body are defined as →ωm and →v respectively. m .

[0416] The point i as the detection position on the rigid body is assumed to be the position O m The vector of →u i denoted by the position, and point i is assumed to have a velocity →v relative to the light detection and ranging sensor 110 i and Doppler velocity →w i Assume that point i is at distance →r when viewed from the sensor i at the location.

[0417] In addition, when an arbitrary position O in space is set as the origin, the world coordinate system is defined by axes x, y, and z that are orthogonal to each other. When viewed from the position O that is the origin of the world coordinate system, the position O that is the origin of the rigid body coordinate system is assumed to be m Located at →p m , and assume that point i is located at →p i In addition, assuming that position O s (the origin of the sensor coordinate system) is located at →p when viewed from position O s at the location.

[0418] (When the detection target is a stationary object and the sensor is in motion)

[0419] First, the relationship between the values ​​in each coordinate system in the case where the detection target is a stationary object and the sensor is in motion will be described. The position vector p of point i is i It is expressed by the following formula (1).

[0420] p i =p s +r i …(1)

[0421] Velocity vector of point i·p i It is expressed by the following formula (2).

[0422]

[0423] In formula (2), the angular velocity tilde matrix ω indicating the angular velocity of the rigid body is s The angular velocity vector _ω based on the following formula (3) is expressed as the following formula (4).

[0424]

[0425] In formula (2), when the rigid body coordinate system is stationary, the following formula (5) holds.

[0426]

[0427] In addition, according to the vector constant expression, for the distance _r i and the angular velocity tilde matrix ~ω s , the following relationship (6) holds.

[0428]

[0429] In addition, the Doppler velocity w at point i i It is expressed by the following formula (7).

[0430]

[0431] The following formula (8) is obtained by the above formulas (2) and (5) to (7).

[0432]

[0433] Formula (8) is transformed into the following formula (9) to rearrange the relationship between the values, thereby obtaining the Doppler velocity w at point i: i With the velocity vector ·_p i The relationship between.

[0434]

[0435] (Use Doppler velocity to estimate sensor velocity)

[0436] Next, a method of estimating the sensor velocity using the Doppler velocity by the sensor velocity estimation process 402 in the case where the sensor is in motion and the point i is a stationary object will be described. i and sensor velocity v s The relationship between is expressed by the following formula (10) based on the above formula (9).

[0437]

[0438] Use sensor direction R s , formula (10) can be expressed as the following formula (11).

[0439] [Mathematical expression 11]

[0440]

[0441] In formula (11), -(r s i / r i ) represents the beam direction vector, which can be expressed using a matrix as shown in the following formula (12).

[0442]

[0443] By substituting equation (12) into equation (11), the following equation (13) is obtained.

[0444]

[0445] Here, based on formula (13), the value _e i It is defined as the following formula (14).

[0446]

[0447] Using formula (14), we can get the value representing the sensor velocity _v s In the sensor speed estimation process 402, the sensor speed _v is estimated by the formula (15). s .

[0448]

[0449] Note that in formula (15), based on the value e i The matrix represents the matrix of the beam direction vector in the stationary object and world coordinate systems.

[0450] (Point cloud correction processing for stationary objects)

[0451] Next, the stationary object point cloud correction process performed by the point cloud correction process 403 while the sensor is in motion will be described.

[0452] The velocity vector _v of point i i It is expressed by the following formula (16).

[0453]

[0454] Transform equation (16) to obtain equation (17).

[0455]

[0456] Formula (17) is further transformed to obtain the following formula (18).

[0457]

[0458] Transform formula (18) into a discrete equation to obtain the following formula (19). In formula (19), Δt i is the measurement of differential time and through Δt i =t i -t i-1 get.

[0459]

[0460] Based on formula (19), the correction factor X s i It is defined as the following formula (20).

[0461]

[0462] Using the measured differential time Δt in equation (20) i and correction factor X s i , the position of each point of the stationary object point cloud is corrected by the following formula (21).

[0463]

[0464] In formula (21), the left side represents the position vector of each corrected detection point (point i) in the sensor coordinate system. The last term on the right side represents the position vector of each detection point in the sensor coordinate system before correction. In point cloud correction processing 403, the measured differential time Δt based on point i is used. i and correction factor X s i The correction term is used. Before correcting each detection point, the position correction value is added to the position vector of the detection point (point i) in the sensor coordinate system, thereby correcting the position of each point in the stationary object point cloud through formula (21).

[0465] (Processing related to moving objects)

[0466] Next, the processing related to the moving object is described. Fig.36 In addition to replacing the stationary object (rigid body) with a moving object (rigid body), the coordinate system is the same as Fig.36 The coordinate system is the same as in , so its description is omitted.

[0467] (The detection target is a moving target and the sensor is in motion)

[0468] First, the relationship between the values ​​in each coordinate system in the case where the detection target is a moving object and the sensor is in motion will be described. Position vector _p of point i (detection position) i It is expressed by the following formula (22).

[0469] p i =p m +u i …(twenty two)

[0470] The velocity vector of point i in the rigid body coordinate system is _p i It is expressed by the following formula (23). Note that the velocity vector ·_p in the sensor coordinate system of point i i This is similar to the above formula (2), so its description will be omitted here.

[0471]

[0472] Using the vector constant equation of formula (6) above and the velocity vector ·_p i The sensor coordinate system is at point i in formula (2), and formula (23) is transformed to obtain the following formula (24).

[0473]

[0474] Formula (24) is further converted into the following formula (25) to rearrange the relationship between the respective values.

[0475]

[0476] (Speed ​​discrimination between stationary and moving objects)

[0477] Next, the speed discrimination of a stationary object and a moving object by the stationary object / moving object discrimination processing 401 is described. By correcting the Doppler speed w according to the motion of the point i i , the corrected Doppler velocity w is obtained by the following formula (26): i '.

[0478]

[0479] The stationary object / moving object discrimination process 401 is performed by performing the correction of the Doppler velocity w i 'Perform threshold determination to determine whether point i is a moving object or a stationary object. For example, set the moving object determination threshold v for determining a moving object th1 and a stationary object determination threshold v for determining a stationary object th2 . After the corrected Doppler velocity wi The absolute value of ' is less than the stationary object judgment threshold v th2 (|w i '| <v th2 ), it is determined that point i is a stationary object. i The absolute value of ' exceeds the moving object determination threshold v th1 (|w i '|>v th1 ), it is determined that point i is a moving object.

[0480] (Use Doppler velocity to estimate sensor velocity)

[0481] Next, a method of estimating the sensor velocity using the Doppler velocity by the sensor velocity estimation process 402 in the case where the sensor is in motion and the point i is a moving object will be described. i , the relative speed of the sensor to the moving object is expressed by the following equation (27).

[0482]

[0483] Use sensor direction R s Transform formula (27) to obtain the following formula (28).

[0484]

[0485] Here, the value _η is defined as follows: i .

[0486]

[0487] Vector_u in formula (29) i It is obtained by the following formula (30).

[0488] u i =p s +r i -p m …(30)

[0489] Using the determinant, equation (29) is expressed as the following equation (31).

[0490]

[0491] The above formula (28) is converted using formula (29) to formula (31) to obtain the following formula (32).

[0492]

[0493] Here, the value ξi is defined as in the following formula (33).

[0494]

[0495] The value ξi defined using equation (33) and the value e defined using equation (14) i To transform the above formula (32) to obtain the speed of the moving object _v m The following formula (34).

[0496]

[0497] (Point cloud correction processing for moving objects)

[0498] Next, the moving object point cloud correction process performed by the point cloud correction process 404 in the case where the sensor moves will be described below.

[0499] The velocity vector _v of point i in the sensor coordinate system i It is expressed by the following formula (35).

[0500]

[0501] The velocity vector _v of point i in the rigid body coordinate system i It is expressed by the following formula (36).

[0502]

[0503] Assume that equations (35) and (36) are equal and use the sensor orientation R s Transform formula (35) to obtain the following formula (37).

[0504]

[0505] Substituting the above formula (30) into the vector_u of formula (37) i To obtain the following formula (38).

[0506]

[0507] Transform equation (38) into a discrete equation and apply the measured differential time Δt i To obtain the following formula (39).

[0508]

[0509] Based on Expression 39, the correction coefficient σ s i Defined in the following formula (40).

[0510]

[0511] Using the measured differential time Δt shown in equation (40) i and correction factor σ s i , the position of each point in the moving object point cloud is corrected by the following formula (41).

[0512]

[0513] In formula (41), the left side represents the position vector of each corrected detection point (point i) in the sensor coordinate system. The last term on the right side represents the position vector of each detection point in the sensor coordinate system before correction. In point cloud correction processing 404, by using the measured differential time Δt i and the correction coefficient σ at point i s i Before correcting each detection point, the position correction value is added to the position vector of the detection point (point i) in the sensor coordinate system, and the position of each point in the moving object point cloud is corrected by formula (41).

[0514] (2-2-5. Effects of the First Embodiment)

[0515] In measurements using direct time-of-flight (dToF)-LiDAR according to the prior art, there are very few methods for calculating the moving speed of the surrounding environment. Specifically, a method is used to grasp the movement of the surrounding environment structure from the difference in matching of structural shapes of multiple frames, and the movement amount is divided by the frame period, etc.

[0516] In contrast, by performing measurements using the FMCW-LiDAR technology applied to the present disclosure, information called a velocity point cloud can be acquired. The velocity point cloud is information that is a collection of distances, direction angles, and Doppler velocities from a sensor to a detection point of an object.

[0517] The measurement of FMCW-LiDAR generally performs beam scanning and distance measurement in sequence, and thereby outputs a velocity point cloud at a constant cycle. Here, it is assumed that the environment around a moving subject and a velocity point cloud obtained by measuring a moving object using an FMCW-LiDAR sensor mounted on the moving subject and performing measurement using FMCW-LiDAR are acquired.

[0518] The entire surrounding structure can be regarded as a rigid body, and the motion of the rigid body is self-motion when viewed from the reference coordinate system of the surrounding structure. The relationship between the Doppler velocity, the scanning direction vector, the rigid body velocity vector, and the rigid body angular velocity vector is derived from the kinematic equations related to the velocity vector and the angular velocity vector at a point on the rigid body and the relationship expression between the velocity vector and the Doppler velocity at the point.

[0519] Therefore, by using the position vector, angular velocity vector, and velocity point cloud acquired by the FMCW-LiDAR sensor, it is considered to calculate the velocity vector of the rigid body reference point. Therefore, using the measurement method according to the first embodiment, even if only a single frame of the velocity point cloud acquired by measurement using the FMCW-LiDAR sensor mounted on the mobile body is used, the motion of the surrounding environment structure can be estimated. That is, the self-motion can be estimated by changing the reference coordinate system.

[0520] Since the measurement method according to the first embodiment of the present disclosure is not a method using frame differences, speed estimation can be continued even under conditions where the degradation problem occurs. In addition, using the measurement method according to the first embodiment, motion that cannot be estimated by frame differences can be measured not only in self-motion estimation but also in motion estimation of surrounding moving objects.

[0521] In addition, with the measurement method according to the first embodiment, even in the case where there is a dynamic environment around, dynamic objects with high accuracy in a single frame can be excluded by performing velocity discrimination using the velocity point cloud. That is, with the measurement method according to the first embodiment, by excluding the portion corresponding to the dynamic object from the acquired velocity point cloud, and by estimating motion estimation using only the portion of the velocity point cloud corresponding to the surrounding environment, high-precision self-motion estimation can be achieved.

[0522] (2-3. First Modification Example of First Embodiment)

[0523] Next, a first modification of the first embodiment is described. The first modification of the first embodiment is to add a configuration for generating motion meta information related to the motion of the moving object to the Fig.17 An example of the configuration of the recognition system 1a of the first embodiment is shown in FIG.

[0524] Fig.37 : is a block diagram showing a configuration of an example of a recognition system according to a first modification example of the first embodiment. Fig.37 In the recognition system 1b according to the first modification example of the first embodiment, the signal processing unit 20b is a combination unit 224 and a motion recognition unit 225 have been added to the recognition system 1b according to the first modification example of the first embodiment. Fig.17 The signal processing unit 20a of the recognition system 1a of the first embodiment shown in FIG.

[0525] The combining unit 224 receives the moving object velocity point cloud passed from the moving object point cloud correction unit 214, and receives the image data of the region of interest passed from the ROI extraction unit 223. The combining unit 224 combines the moving object velocity point cloud with the image data of the region of interest. For example, the combining unit 224 may combine the moving object velocity point cloud with the image data of the region of interest. In this case, the combining unit 224 may associate the velocity information of each point included in the moving object velocity point cloud with each pixel data in the image data of the region of interest. The image obtained by combining the velocity information with each pixel of the image data of the region of interest is called a composite image.

[0526] The combination is not limited thereto, and the combination unit 224 may combine the image data of the region of interest with the moving object speed point cloud. In this case, the combination unit 224 may associate information of corresponding pixel data in the region of interest with each point of the moving object speed point cloud. The point cloud obtained by combining each pixel data of the region of interest with each point of the moving object speed point cloud is referred to as a combined point cloud.

[0527] The combining unit 224 transfers the combined image or the combined point cloud to the transmission unit 201 and the motion recognition unit 225 .

[0528] The motion recognition unit 225 recognizes the motion of the moving object based on the combined image or the combined point cloud transmitted from the combination unit 224. The motion recognition unit 225 can obtain speed distribution information based on the speed information of each point included in the composite image or the combined point cloud. The motion recognition unit 225 can estimate the motion of the moving object more accurately by using the distribution information of the speed. For example, the motion recognition unit 225 can perform motion recognition using a learning model trained based on known speed distribution information by machine learning. The motion recognition unit 225 recognizes the motion of the moving object and outputs meta information (walking, running, etc.) related to the motion. The motion meta information output from the motion recognition unit 225 is transmitted to the transmission unit 201.

[0529] The transmission unit 201 transmits the combined image or the combined point cloud transmitted from the combination unit 224 and the motion meta information transmitted from the motion recognition unit 225 to the information processing unit 30 .

[0530] (2-4. Second Modification of First Embodiment)

[0531] Next, a second modification of the first embodiment is described. The second modification of the first embodiment is based on Fig.17 The illustrated embodiment is an example in which the image sensor 120 and a configuration for performing processing related to image data are omitted in the recognition system 1 a .

[0532] Fig.38: is a block diagram showing the configuration of an example of a recognition system according to a second modification example of the first embodiment. Fig.38 In the identification system 1c, a sensor unit 10b is provided, wherein Fig.17 Compared with the sensor unit 10 shown in FIG. 1 , the image sensor 120 is omitted. In addition, the recognition system 1c has a signal processing unit 20c, wherein the image sensor 120 is omitted. Fig.17 Compared with the signal processing unit 20a shown in FIG. 1 , components related to the processing of image data are omitted, specifically, the 3D / 2D converter 222, the ROI extraction unit 223, the combination unit 224, the motion recognition unit 225, and the image recognition unit 226 are omitted.

[0533] Even in the structure of the recognition system 1c according to the second variant of the present embodiment, it is possible to output map information and output a stationary object speed point cloud and a moving object speed point cloud. When it is not necessary to have motion meta information obtained by motion recognition of a moving object or image meta information obtained by image recognition, cost reduction can be achieved by applying the configuration of the recognition system 1c according to the second variant of the embodiment.

[0534] (2-5. Third Modification of First Embodiment)

[0535] Next, a third modification of the embodiment of the present invention will be described. The third modification of the embodiment is one in which a configuration for controlling the scanning of the light detection and ranging sensor 110 has been added to the configuration according to Fig.17 An example of an identification system 1a of the embodiment is shown in FIG.

[0536] Fig.39 : is a block diagram showing a configuration of an example of a signal processing system according to a third modification example of the first embodiment. Fig.39 In the recognition system 1d, the signal processing unit 20d has a signal processing unit added to Fig.17 The additional units of the signal processing unit 20 a in the recognition system 1 a in FIG. 1 , specifically, the control communication unit 230 , the scanning controller 231 , and the parameter setting unit 232 .

[0537] Based on the coordinate reference information and the ROI designation information transmitted from the information processing unit 30 , the control communication unit 230 generates scanning control information for controlling scanning in the light detection and ranging sensor 110 .

[0538] For example, the control communication unit 230 transfers the received coordinate reference information to the parameter setting unit 232. For example, based on the received coordinate reference information, the parameter setting unit 232 sets initial values ​​related to the scanning of the light detection and ranging sensor 110. The control communication unit 230 transfers the initial values ​​set by the parameter setting unit 232 as scanning control information to the light detection and ranging sensor 110. The light detection and ranging sensor 110 initializes the scanning mechanism and the like according to the scanning control information transferred from the control communication unit 230.

[0539] In addition, for example, the control communication unit 230 transmits the received ROI designation information to the scanning controller 231. The ROI designation information may include, for example, coordinate information of a region of interest set as image data. For example, based on the received ROI designation information, the scanning controller 231 converts the coordinate information into information of horizontal and vertical angle ranges for controlling the scanning range in the light detection and ranging sensor 110, and transmits the information to the control communication unit 230. The control communication unit 230 transmits the information of the horizontal angle range and the vertical angle range transmitted from the scanning controller 231 to the light detection and ranging sensor 110 as scanning control information. The light detection and ranging sensor 110 controls the scanning range according to the received scanning control information.

[0540] This makes it possible to control the light detection and ranging sensor 110 to exclusively scan the specified area of ​​interest, resulting in a reduction in the time required for scanning. In addition, the light detection and ranging sensor 110 can also be controlled to perform exclusive and high-density scanning on the specified area of ​​interest, resulting in more accurate operation recognition and image recognition in the area of ​​interest.

[0541] Without being limited thereto, the ROI designation information may be transferred from the ROI extraction unit 223 to the control communication unit 230. That is, based on the point cloud by the 2D coordinates transferred from the 3D / 2D converter 222, the ROI extraction unit 223 transfers information indicating the region of interest extracted from the image data transferred by the image sensor 120 as the ROI designation information to the control communication unit 230 via the receiving unit 200. The control communication unit 230 transfers the ROI designation information transferred from the ROI extraction unit 223 to the scanning controller 231.

[0542] (3. According to the second embodiment of the present disclosure)

[0543] Next, a second embodiment according to the present disclosure will be described. The second embodiment according to the present disclosure is an example in which the processing in the SLAM processing unit 220 further uses position information obtained by a global navigation satellite system (GNSS).

[0544] In a SLAM system that is installed on a mobile body and estimates its own position orientation and surrounding environment at the same time, GNSS can be used together to reduce the cumulative error of its own position. However, under environmental conditions where it is difficult to receive GNSS signals, the cumulative error cannot be reduced, and there is a problem of reduced accuracy of the absolute position of the self. For example, in an indoor environment where a SLAM system using an external environmental sensor is frequently used, it is difficult for GNSS to receive radio waves from satellites, resulting in GNSS operation failure due to accuracy degradation expressed as dilution of precision (DOP). In addition, in indoor environment structures, there is a high probability that smooth walls, floors, etc. mainly have a field of view, which leads to the occurrence of the regression problem in SLAM.

[0545] Even in a degraded scenario, the recognition system using the light detection and ranging sensor 110 through FMCW-LiDAR according to the present disclosure can estimate its own speed and angular velocity with high accuracy. This makes it possible to stably continue SLAM processing even under conditions where it is difficult to receive GNSS signals.

[0546] Fig.40 : is a block diagram showing the configuration of an example of the recognition system according to the second embodiment. It is to be noted that Fig.40 The identification system 1e in is shown as an example, in which the Fig.38 In the second variation of the first embodiment shown in FIG, a GNSS receiver 140 is added to the identification system 1 c.

[0547] exist Fig.40 In the identification system 1e shown, the sensor unit 10c includes an IMU 100, a light detection and ranging sensor 110, and a synchronization signal generator 130, and also includes a GNSS receiver 140. Based on the GNSS signal received from the GNSS satellite, the GNSS receiver 140 acquires position information (referred to as a GNSS absolute position), which indicates the position of the GNSS receiver 140 on the earth. The GNSS receiver 140 outputs the acquired GNSS absolute position and a value indicating the DOP of the received GNSS signal (DOP value).

[0548] This example shows the GNSS receiver 140 operating independently of the synchronization signal generated by the synchronization signal generator 130, but the operation is not limited to this example. For example, the GNSS receiver 140 may receive the GNSS signal in synchronization with the synchronization signal.

[0549] The position information and DOP value output from the GNSS receiver 140 are transmitted to the SLAM processing unit 220a in the signal processing unit 20e. The SLAM processing unit 220a further includes a determination unit, which is similar to the one shown in FIG. Figure 4The configuration of the described SLAM processing unit 5220 performs determination based on the DOP value of the GNSS signal. The SLAM processing unit 220a can perform SLAM processing by combining the position information transmitted from the GNSS receiver 140 with the stationary object speed point cloud corrected by the stationary object point cloud correction unit 213 according to the determination result of the determination unit.

[0550] Fig.41 is a flowchart showing an example of processing performed by the signal processing unit 20e according to the second embodiment. Fig.29 The processing of the different parts of the flowchart (steps S300 to S311) will be described, and the description of other processing will be omitted.

[0551] In step S120, when the stationary object velocity point cloud is acquired by the stationary object point cloud correction unit 213 in the signal processing unit 20e, the process proceeds to step S300. In step S300, in the recognition system 1e, the GNSS receiver 140 receives the GNSS signal to acquire the GNSS signal. Based on the received GNSS signal, the GNSS receiver 140 calculates the GNSS absolute position and obtains the DOP value of the GNSS signal.

[0552] In the next step S301, the signal processing unit 20e uses the SLAM processing unit 220a to determine whether the DOP value of the GNSS signal obtained in step S300 is greater than a specific value. When it is determined in step S301 that the DOP value of the GNSS signal is greater than a specific value (step S301, "Yes"), the signal processing unit 20e proceeds to the process of step S302.

[0553] In step S302, the signal processing unit 20e uses the SLAM processing unit 220a to perform SLAM processing by combining the following: the GNSS absolute position, the sensor position, the orientation, and the angular velocity transmitted from the sensor position / orientation estimation unit 210; the sensor velocity; and the stationary object velocity point cloud passed from the stationary object point cloud correction unit 213. For example, the SLAM processing unit 220a may correct the sensor position transmitted from the sensor position / orientation estimation unit 210 based on the GNSS absolute position. In addition, the SLAM processing unit 220a may correct the map information based on the GNSS absolute position.

[0554] In the next step S303 , the signal processing unit 20 e uses the transmission unit 201 to output its own position, orientation, speed, and angular velocity acquired by the SLAM processing unit 220 a in step S302 .

[0555] In contrast, when it is determined in step S301 that the DOP value of the GNSS signal is smaller than a certain value (step S301 , “No”), the signal processing unit 20 e proceeds to the process of step S302 .

[0556] In step S302, similar to Fig.29 In the processing in step S121 in the flowchart, the signal processing unit 20e uses the SLAM processing unit 220a to perform SLAM processing, which uses the stationary object velocity point cloud acquired in step S120 and the sensor position, orientation and angular velocity transmitted from the sensor position / orientation estimation unit 210 in step S101.

[0557] In the next step S311 , the signal processing unit 20 e uses the transmission unit 201 to output its own position, orientation, speed, and angular speed acquired by the SLAM processing unit 220 a in step S310 .

[0558] In this way, in the second embodiment of the present disclosure, the GNSS absolute position is used to estimate the own position, etc., when the GNSS signal reception is good, and the FMCW-LiDAR data and IMU data are used to estimate the own position, when it is difficult to receive the GNSS signal. Therefore, even in a state where it is difficult to receive the GNSS signal, the own position, etc. can be estimated with high accuracy.

[0559] (4. According to the third embodiment of the present disclosure)

[0560] Next, a third embodiment according to the present disclosure will be described. The third embodiment according to the present disclosure is an example of performing SLAM processing by applying Visual-SLAM that performs SLAM processing based on image data.

[0561] In a SLAM system that is installed on a mobile subject and estimates both its own position / orientation and the surrounding environment at the same time, Visual-SLAM using camera images can be used. For example, in environmental conditions with few structures within the measurement range (deserts, etc.), there are cases where Visual-SLAM is more stable than LiDAR-SLAM using LiDAR measurement results.

[0562] Visual-SLAM extracts feature points from images obtained by capturing the environment, and estimates the change in its own position from the inter-frame correlation of the feature point cloud. At this time, it is assumed that feature points are extracted from the surrounding static environment. Therefore, when there are multiple moving objects around and when feature points are extracted from moving objects, the SLAM operation may become unstable.

[0563] The recognition system using the FMCW-LiDAR according to the present disclosure can instantly distinguish between moving objects and stationary objects using velocity point cloud information, so that only a portion of the image corresponding to the static environment structure can be extracted from the image acquired by the camera. Therefore, by performing feature point extraction processing on the image, a stable visual SLAM operation can be achieved even when there are multiple moving objects in the surrounding environment.

[0564] Fig.42 is a block diagram showing a configuration of an example of a recognition system according to the third embodiment. Fig.42 The recognition system 1f shown in FIG. 1 has a configuration in which the ROI extraction unit 223 has been replaced by the moving object / stationary object image separation unit 240, and Fig.17 In the recognition system 1a of the first embodiment shown in FIG. 1 , the image recognition unit 226 is omitted.

[0565] exist Fig.42 , the receiving unit 200 transmits the image data received from the image sensor 120 to the moving object / stationary object image separation unit 240. The moving object / stationary object image separation unit 240 uses the moving object state information to extract a portion of the image of the stationary environment from the overall image based on the image data. More specifically, the moving object / stationary object image separation unit 240 extracts the area other than the moving object in the image data as a partial image of the stationary environment based on the 2D data obtained by transforming the moving object velocity point cloud into 2D data by the 3D / 2D converter 222. The moving object / stationary object image separation unit 240 passes the extracted partial image of the stationary environment to the Visual-SLAM processing unit 220b as a stationary object image.

[0566] The visual-SLAM processing unit 220b performs SLAM processing based on the sensor position, orientation, and angular velocity transmitted from the sensor position / orientation estimation unit 210 and the sensor velocity transmitted from the sensor velocity estimation unit 212. At the same time, the visual-SLAM processing unit 220b performs visual SLAM processing based on the still image transmitted from the moving object / still object image separation unit 240. For example, the visual-SLAM processing unit 220b can acquire the current position of the sensor unit 10a, etc. by extracting feature points from the still object image and comparing the extracted feature points with the map information.

[0567] Fig.43 : is a flowchart showing an example of processing by a signal processing unit according to the third embodiment. Fig.43 In the process of using IMU data and using FMCW-LiDAR data in steps S100 to S134, Fig.29The processing in steps S100 to S134 of the flowchart is similar, so their description is omitted here.

[0568] In step S140, the signal processing unit 20f acquires image data from the image sensor 120 using the receiving unit 200, and proceeds to the process of step S400. In addition, the signal processing unit 20f converts the moving object state information estimated by the moving object state estimation unit 221 in step S132 into 2D data through the 3D / 2D converter 222, and passes the 2D data to the process in step S400.

[0569] In step S400, the signal processing unit 20f extracts a partial image of a stationary environment from the entire image based on the image data by using the operation state information of each mobile object using the moving object / stationary object image separation unit 240. The moving object / stationary object image separation unit 240 passes the extracted partial image to the Visual-SLAM processing unit 220b.

[0570] In the next step S401, the signal processing unit 20f uses the Visual-SLAM processing unit 220b to extract feature points from the partial image of the static environment transmitted by the moving object / static object image separation unit 240. In the next step S402, the Visual-SLAM processing unit 220b associates the feature points extracted in step S401 with the map information and updates the map information. In addition, the Visual-SLAM processing unit 220b estimates its own position based on the extracted feature points and the updated map information. The signal processing unit 20f uses the transmission unit 201 to output the own position estimated by the Visual-SLAM processing unit 220b.

[0571] In this way, the third embodiment performs combined use of both SLAM processing based on FMCW-LiDAR data and IMU data and visual SLAM processing using image data, thereby enabling the acquisition of self-position and map information with higher accuracy even in environmental conditions with almost no structure.

[0572] In addition, the Visual-SLAM processing unit 220b according to the third embodiment extracts a partial image formed using a stationary object image from a moving object speed point cloud obtained by speed differentiation of FMCW-LiDAR data of one frame of image data. Therefore, Visual-SLAM processing can be performed based on one frame of image data.

[0573] The effects described in this specification are merely examples, and therefore, there may be other effects not limited to the exemplary effects.

[0574] It should be noted that the present technology may also have the following configurations.

[0575] (1) An identification system comprising:

[0576] A sensor unit, comprising a first sensor and a second sensor, the first sensor being configured to output velocity point cloud data comprising a plurality of points, each point having velocity information and time point information, and the second sensor being configured to output inertial measurement data;

[0577] a receiving unit configured to receive the velocity point cloud data and the inertial measurement data output from the sensor unit; and

[0578] The output unit is configured to output sensor status information and moving object status information, the sensor status information includes information indicating the position and orientation of the sensor unit, and the moving object status information includes information indicating the position, orientation and speed of the moving object. The sensor status information and the moving object status information are information generated based on the velocity point cloud data and the inertial measurement data.

[0579] (2) The recognition system according to (1), further comprising: a map creation unit configured to update map information based on the velocity point cloud data and the inertial measurement data, wherein:

[0580] The output unit further outputs map information.

[0581] (3) The identification system according to (2), wherein:

[0582] The output unit outputs the map information at a lower output rate than an output rate of the sensor state information and the moving object state information.

[0583] (4) The identification system according to (3), wherein:

[0584] The output unit outputs the sensor state information and the moving object state information at output rates corresponding to each other.

[0585] (5) The identification system according to any one of (2) to (4), wherein:

[0586] Map creation unit,

[0587] When the accuracy of the absolute position of the sensor unit acquired from the fourth sensor is a certain value or more, the map information is updated using the velocity point cloud data, the inertial measurement data, and the absolute position.

[0588] (6) The identification system according to any one of (2) to (5), wherein:

[0589] The receiving unit further receives image data output from a third sensor included in the sensor unit, and

[0590] The map creation unit creates and updates map information based on the image data and the sensor status information.

[0591] (7) The identification system according to (6), wherein:

[0592] The map creation unit updates the map information using image data of a partial image extracted from the image data based on the moving object state information.

[0593] (8) The identification system according to any one of (2) to (7), further comprising:

[0594] a correction unit configured to correct, for each frame of the detection operation performed by the first sensor, a stationary object velocity point cloud based on an estimated value at a predetermined time point given as at least one predetermined time point, the stationary object velocity point cloud being a velocity point cloud of a stationary object and included in the velocity point cloud data, wherein

[0595] The map creation unit updates the map information using the stationary object speed point cloud corrected by the correction unit.

[0596] (9) The identification system according to (8), wherein:

[0597] The correction unit further corrects a moving object speed point cloud, which is a speed point cloud of the moving object and is included in the speed point cloud data, based on the estimated value at a predetermined time point, and

[0598] The output unit outputs the moving object state information generated using the moving object speed point cloud corrected by the correction unit.

[0599] (10) The identification system according to any one of (1) to (9), wherein:

[0600] The receiving unit further receives image data output from a third sensor included in the sensor unit, and

[0601] The output unit further outputs image meta information obtained by performing image recognition on the image of the region of interest extracted from the image data based on the moving object state information.

[0602] (11) The identification system according to any one of (1) to (10), wherein:

[0603] The output unit adds time point information, which is information indicating a time point at which each of the plurality of points is acquired on the first sensor, to the sensor state information and outputs the obtained information.

[0604] (12) A recognition method executed by a processor, the method comprising:

[0605] receiving velocity point cloud data and inertial measurement data, the velocity point cloud data being data output from a first sensor included in the sensor unit and including a plurality of points, each point having velocity information and time point information, and the inertial measurement data being data output from a second sensor included in the sensor unit; and

[0606] Output sensor status information and moving object status information, the sensor status information includes information indicating the position and orientation of the sensor unit, the moving object status information includes information indicating the position, orientation and speed of the moving object, and the sensor status information and the moving object status information are information generated based on the velocity point cloud data and the inertial measurement data.

[0607] (13) An identification device comprising:

[0608] a receiving unit configured to receive velocity point cloud data which is data output from a first sensor included in the sensor unit and includes a plurality of points, each point having velocity information and time point information, and inertial measurement data which is data output from a second sensor included in the sensor unit; and

[0609] The output unit is configured to output sensor status information and moving object status information, the sensor status information includes information indicating the position and orientation of the sensor unit, and the moving object status information includes information indicating the position, orientation and speed of the moving object. The sensor status information and the moving object status information are information generated based on the velocity point cloud data and the inertial measurement data.

[0610] Reference Numbers List

[0611] 1. 1a, 1b, 1c, 1d, 1e, 1f identification system

[0612] 10, 10a, 10b, 10c sensor unit

[0613] 20, 20a, 20b, 20c, 20d, 20e, 20f signal processing unit

[0614] 30 Information Processing Unit

[0615] 40 scan lines

[0616] 41, 41a, 41b, 41 st Launch Point

[0617] 421,422,42 N Scan Line

[0618] 45° Angle Range

[0619] 46 Scan range

[0620] Objects 50a, 50b, 52

[0621] 60a, 60b, 60c frames

[0622] 61 pixels

[0623] 100IMU

[0624] 110 Light Detection and Distance Sensor

[0625] 111 Optical scanning unit

[0626] 112 optical transmission / receiving unit

[0627] 113 Receive signal processing unit

[0628] 114 Optical Scanning Controller

[0629] 115, 201, 5201 transmission units

[0630] 120 Image Sensor

[0631] 130 Synchronous signal generator

[0632] 140GNSS Receiver

[0633] 200 receiving units

[0634] 210 sensor position / orientation estimation unit

[0635] 211 Moving object / stationary object separation unit

[0636] 212 Sensor speed estimation unit

[0637] 213 Stationary Object Point Cloud Correction Unit

[0638] 214 Moving Target Point Cloud Correction Unit

[0639] 220, 220a, 5220 SLAM processing unit

[0640] 220b visual SLAM processing unit

[0641] 221 Moving Object State Estimation Unit

[0642] 222 3D / 2D converter

[0643] 223 ROI extraction unit

[0644] 224 combination units

[0645] 225 Motion Recognition Unit

[0646] 226 Image Recognition Unit

[0647] 230 Control communication unit

[0648] 231 Scan Controller

[0649] 232 Parameter setting unit

[0650] 240 Moving object / stationary object image separation unit

[0651] 400 Sensor position / orientation estimation processing

[0652] 401 Stationary object / moving object identification processing

[0653] 402 Sensor speed estimation processing

[0654] 403, 404 Point cloud correction processing

[0655] 405 Moving Object Detection Processing

[0656] 1000 Sensor Group

[0657] 1010 Firmware

[0658] 2000, 3000 processors

[0659] 2010, 3010 library part

[0660] 2020, 3020 application part

[0661] 2030OS

[0662] 2040 API Call Unit

[0663] 2041 API processing unit.

Claims

1. An identification system, comprising: A sensor unit, comprising a first sensor and a second sensor, wherein the first sensor is configured to output velocity point cloud data including a plurality of points, each point having velocity information and time point information, and the second sensor is configured to output inertial measurement data; a receiving unit configured to receive the velocity point cloud data and the inertial measurement data output from the sensor unit; as well as An output unit is configured to output sensor status information and moving object status information, wherein the sensor status information includes information indicating the position and orientation of the sensor unit, and the moving object status information includes information indicating the position, orientation and speed of the moving object, and the sensor status information and the moving object status information are information generated based on the velocity point cloud data and the inertial measurement data.

2. The identification system according to claim 1, further comprising: A map creation unit, the map creation unit being configured to update map information based on the velocity point cloud data and the inertial measurement data, wherein: The output unit: The map information is further outputted.

3. The identification system according to claim 2, wherein: The output unit: The map information is output at a lower output rate than output rates of the sensor state information and the moving object state information.

4. The identification system according to claim 3, wherein: The output unit: The sensor state information and the moving object state information are output at output rates corresponding to each other.

5. The identification system according to claim 2, wherein: The map creation unit: When the accuracy of the absolute position of the sensor unit acquired from the fourth sensor is equal to or greater than a certain value, the map information is updated using the velocity point cloud data, the inertial measurement data, and the absolute position.

6. The identification system according to claim 2, wherein: The receiving unit: further receiving image data output from a third sensor included in the sensor unit, and The map creation unit: The map information is created and updated based on the image data and the sensor status information.

7. The identification system according to claim 6, wherein: The map creation unit: The map information is updated using image data of a partial image extracted from the image data based on the moving object state information.

8. The identification system according to claim 2, further comprising: a correction unit configured to correct, for each frame of the detection operation performed by the first sensor, a stationary object velocity point cloud based on an estimated value at a predetermined time point given as at least one predetermined time point, the stationary object velocity point cloud being a velocity point cloud of a stationary object and included in the velocity point cloud data, wherein The map creation unit: The map information is updated using the stationary object speed point cloud corrected by the correction unit.

9. The identification system according to claim 8, wherein: The correction unit: further correcting a moving object speed point cloud, which is a speed point cloud of a moving object and is included in the speed point cloud data, based on the estimated value at the predetermined time point, and The output unit: The moving object state information generated using the moving object speed point cloud corrected by the correction unit is output.

10. The identification system according to claim 1, wherein: The receiving unit: further receiving image data output from a third sensor included in the sensor unit, and The output unit: Image meta information obtained by performing image recognition on an image of a region of interest extracted from the image data based on the moving object state information is further outputted.

11. The identification system according to claim 1, wherein: The output unit: Time point information is added to the sensor state information and the obtained information is output, the time point information being information indicating a time point at which each of the plurality of points is acquired on the first sensor.

12. A recognition method executed by a processor, the method comprising: receiving velocity point cloud data and inertial measurement data, the velocity point cloud data being data output from a first sensor included in the sensor unit and including a plurality of points, each point having velocity information and time point information, and the inertial measurement data being data output from a second sensor included in the sensor unit; as well as Output sensor status information and moving object status information, wherein the sensor status information includes information indicating the position and orientation of the sensor unit, and the moving object status information includes information indicating the position, orientation and speed of the moving object, and the sensor status information and the moving object status information are information generated based on the velocity point cloud data and the inertial measurement data.

13. An identification device, comprising: a receiving unit configured to receive velocity point cloud data, which is data output from a first sensor included in the sensor unit and includes a plurality of points, each point having velocity information and time point information, and inertial measurement data, which is data output from a second sensor included in the sensor unit; as well as An output unit is configured to output sensor status information and moving object status information, wherein the sensor status information includes information indicating the position and orientation of the sensor unit, and the moving object status information includes information indicating the position, orientation and speed of the moving object, and the sensor status information and the moving object status information are information generated based on the velocity point cloud data and the inertial measurement data.

Images