Motion monitor using laser radar registration

Abstract

A motion monitor includes a lidar device, a controller, and circuitry. The lidar device is configured to perform scans of an environment in the vicinity of a ego vehicle to generate a lidar point cloud related to one or more objects in the environment. The controller is configured to transform the lidar point cloud with a position transformation matrix to generate a position point set, aggregate the position point set over a duration of interest to generate an aggregated point set, register the position point set to the aggregated point set to generate a correction transformation matrix, update the position transformation matrix with the correction transformation matrix to generate an updated position transformation matrix, and generate a motion set based on the updated position transformation matrix. The circuitry is configured to receive the motion set.

Description

Technical Field

[0001] This disclosure relates to a system and / or method for motion monitoring of a self-driving vehicle, and more particularly to motion estimation of a self-driving vehicle using lidar registration. Background Technology

[0002] Self-vehicle motion estimation is used in advanced driver assistance systems and autonomous vehicle features. Self-motion estimation is typically achieved using a Global Positioning System (GPS) receiver, an Inertial Measurement Unit (IMU), and / or a Vehicle Speed ​​Sensor (VSS) and / or a Steering Angle Sensor. GPS systems are susceptible to errors in urban environments and tunnels. IMU systems are susceptible to drift. VSS and steering angle sensors have low accuracy. Both types of systems are expensive. LiDAR-based self-vehicle motion estimation methods using scan-to-scan registration are less costly than GPS / IMU methods, but have low accuracy and are prone to drift.

[0003] Therefore, those skilled in the art continue to conduct research and development in the field of self-vehicle motion estimation using lidar sensors. Summary of the Invention

[0004] This document provides a motion monitor. The motion monitor includes a lidar device, a controller, and circuitry. The lidar device is disposed in a vehicle and configured to perform multiple scans of the environment near the vehicle to generate multiple lidar point clouds associated with one or more objects in the environment. The controller communicates with the lidar device and is configured to transform the multiple lidar point clouds using multiple position transformation matrices to generate multiple sets of position points, aggregate the multiple sets of position points over a duration of interest to generate multiple aggregated point sets, register the multiple sets of position points to the multiple aggregated point sets to generate multiple correction transformation matrices, update the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position transformation matrices, and generate multiple motion sets based on the multiple updated position transformation matrices. The circuitry communicates with the controller and is configured to receive the multiple motion sets.

[0005] In one or more embodiments of a motion monitor, registration of multiple sets of location points includes noncausal registration, which utilizes one or more future sets of location points in the multiple sets of location points to noncausally register one or more current sets of location points in the multiple sets of location points.

[0006] In one or more embodiments of a motion monitor, the registration of multiple sets of location points is causal registration, which uses only past sets of location points in the multiple sets of location points to causally register each current set of location points in the multiple sets of location points.

[0007] In one or more embodiments of a motion monitor, the aggregation of multiple sets of location points includes: identifying the number of points aggregated in the neighborhood of each current location point in the multiple aggregated sets, and removing the neighborhood in response to the number of aggregated points being less than a lower threshold number.

[0008] In one or more embodiments of the motion monitor, the aggregation of multiple location point sets further includes: reducing the neighborhood to a random sample of aggregated points within the neighborhood in response to the number of aggregated points exceeding an upper limit threshold.

[0009] In one or more embodiments of the motion monitor, the aggregation of multiple sets of location points further includes: calculating singular value decompositions of the aggregated points in the neighborhood to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, wherein the first and second singular vectors define a best-fit plane for the aggregated points, and the third singular vector is normal to the best-fit plane, and filtering the neighborhood in response to a ratio of the third singular value to the first singular value being greater than a ratio threshold value.

[0010] In one or more embodiments of the motion monitor, the least mean square technique is used to compute the generation of multiple correction transformation matrices under the low-angle correction assumption.

[0011] In one or more embodiments of the motion monitor, the circuitry is a storage device configured to record multiple motion sets, and the multiple motion sets form a training dataset for an autonomous driving training system.

[0012] In one or more embodiments of the motion monitor, the circuitry is a drive system located in the autonomous vehicle and configured to control the motion of the autonomous vehicle in response to a plurality of motion sets.

[0013] This paper presents a method for motion estimation of an autonomous vehicle. The method includes: repeatedly scanning the environment near the autonomous vehicle using a lidar device to generate multiple lidar point clouds associated with one or more objects in the environment; transforming the multiple lidar point clouds using multiple position transformation matrices to generate multiple sets of position points; aggregating the multiple sets of position points within a duration of interest to generate multiple aggregated point sets; registering the multiple sets of position points to the multiple aggregated point sets to generate multiple correction transformation matrices; updating the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position transformation matrices; generating multiple motion sets based on the multiple updated position transformation matrices; and receiving the multiple motion sets at a circuit.

[0014] In one or more embodiments of the method, the registration of each of the plurality of location point sets includes noncausal registration, which utilizes one or more future location point sets in the plurality of location point sets to noncausally register one or more current location point sets in the plurality of location point sets.

[0015] In one or more embodiments of the method, the registration of each of the plurality of location point sets includes causal registration, which uses only past location point sets in the plurality of location point sets to causally register each current location point set in the plurality of location point sets.

[0016] In one or more embodiments of the method, aggregating multiple sets of location points includes: identifying the number of aggregating points in the neighborhood of each current location point in the multiple aggregating point sets, and removing the neighborhood in response to the number of aggregating points being less than a lower threshold number.

[0017] In one or more embodiments of the method, the aggregation of the plurality of location point sets further includes: in response to the number of aggregated points exceeding an upper limit threshold, reducing the neighborhood to a random sample of aggregated points within the neighborhood.

[0018] In one or more embodiments of the method, the aggregation of the plurality of location point sets further includes: calculating the singular value decomposition of the aggregated points in the neighborhood to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, wherein the first singular vector and the second singular vector define a best-fit plane for the aggregated points, and the third singular vector is normal to the best-fit plane; and filtering the neighborhood in response to a ratio of the third singular value to the first singular value being greater than a ratio threshold value.

[0019] In one or more embodiments of the method, the least mean square technique is used to compute the generation of multiple correction transformation matrices under the low-angle correction assumption.

[0020] This document provides a self-driving vehicle. The self-driving vehicle includes a lidar device, a controller, circuitry, and a drive system. The lidar device is configured to perform multiple scans of the environment near the self-driving vehicle to generate multiple lidar point clouds associated with one or more objects in the environment. The controller communicates with the lidar device and is configured to transform the multiple lidar point clouds using multiple position transformation matrices to generate multiple sets of position points, aggregate the multiple sets of position points for a duration of interest to generate multiple aggregated point sets, register the multiple sets of position points to the multiple aggregated point sets to generate multiple correction transformation matrices, update the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position matrices, and generate multiple motion sets based on the multiple updated position transformation matrices. The circuitry communicates with the controller and is configured to receive the multiple motion sets and generate multiple commands in response to the multiple motion sets. The drive system is configured to control the motion of the self-driving vehicle in response to the multiple commands.

[0021] In one or more embodiments of the autonomous vehicle, the registration of multiple location point sets is causal registration, which uses only past location point sets in the multiple location point sets to causally register each current location point set in the multiple location point sets.

[0022] In one or more embodiments, the autonomous vehicle also includes a camera configured to sense the environment near the autonomous vehicle to generate multiple images.

[0023] In one or more embodiments of the autonomous vehicle, multiple position transformation matrices align multiple motion sets and multiple images.

[0024] The present invention may also include the following solutions.

[0025] 1. A motion monitor, comprising: A lidar device, wherein the lidar device is disposed in a self-driving vehicle and is configured to perform multiple scans of the environment near the self-driving vehicle to generate multiple lidar point clouds associated with one or more objects in the environment. A controller, communicating with the lidar device and configured to transform the plurality of lidar point clouds using a plurality of position transformation matrices to generate a plurality of position point sets, aggregate the plurality of position point sets during a duration of interest to generate a plurality of aggregated point sets, register the plurality of position point sets to the plurality of aggregated point sets to generate a plurality of correction transformation matrices, update the plurality of position transformation matrices using the plurality of correction transformation matrices to generate a plurality of updated position transformation matrices, and generate a plurality of motion sets based on the plurality of updated position transformation matrices; and A circuit that communicates with the controller and is configured to receive the plurality of motion sets.

[0026] 2. The motion monitor according to Scheme 1, wherein the registration of the plurality of location point sets includes noncausal registration, the noncausal registration utilizing one or more future location point sets in the plurality of location point sets to noncausally register one or more current location point sets in the plurality of location point sets.

[0027] 3. The motion monitor according to Scheme 1, wherein the registration of the plurality of location point sets is causal registration, and the causal registration uses only the past location point sets in the plurality of location point sets to causally register each current location point set in the plurality of location point sets.

[0028] 4. The motion monitor according to Scheme 1, wherein the aggregation of the plurality of location point sets includes: Identify the number of cluster points in the plurality of cluster point sets within the neighborhood of each current location point in the plurality of location point sets; and The neighborhood is removed in response to the number of clustered points being less than a lower threshold number.

[0029] 5. The motion monitor according to Scheme 4, wherein the aggregation of the plurality of location point sets further includes: In response to the number of cluster points exceeding an upper limit threshold, the neighborhood is reduced to a random sample of cluster points within the neighborhood.

[0030] 6. The motion monitor according to Scheme 5, wherein the aggregation of the plurality of location point sets further includes: The singular value decomposition of the cluster points in the neighborhood is computed to determine the first singular vector, the first singular value, the second singular vector, the third singular vector, and the third singular value, wherein the first and second singular vectors define the best-fit plane of the cluster points, and the third singular vector is normal to the best-fit plane; and In response to the ratio of the third singular value to the first singular value being greater than a ratio threshold, the neighborhood is filtered.

[0031] 7. The motion monitor according to Scheme 1, wherein the least mean square technique is used to calculate the generation of the plurality of correction transformation matrices under the low-angle correction assumption.

[0032] 8. The motion monitor according to claim 1, wherein the circuit is a storage device configured to record the plurality of motion sets, and the plurality of motion sets form a training dataset for an autonomous driving training system.

[0033] 9. The motion monitor according to claim 1, wherein the circuit is a drive system arranged in the self-vehicle and configured to control the motion of the self-vehicle in response to the plurality of motion sets.

[0034] 10. A method for motion estimation of an autonomous vehicle, comprising: The environment near the vehicle is scanned multiple times using a lidar device to generate multiple lidar point clouds associated with one or more objects in the environment. Multiple position transformation matrices are used to transform the multiple lidar point clouds to generate multiple position point sets; Aggregate the multiple location point sets within the duration of interest to generate multiple aggregation point sets; Register the plurality of location point sets to the plurality of aggregation point sets to generate a plurality of correction transformation matrices; The plurality of position transformation matrices are updated using the plurality of correction transformation matrices to generate a plurality of updated position transformation matrices; Multiple motion sets are generated based on the multiple updated position transformation matrices; and The plurality of motion sets are received at the circuit.

[0035] 11. The method according to Scheme 10, wherein the registration of each of the plurality of location point sets includes noncausal registration, the noncausal registration utilizing one or more future location point sets in the plurality of location point sets to noncausally register one or more current location point sets in the plurality of location point sets.

[0036] 12. The method according to Scheme 10, wherein the registration of each of the plurality of location point sets includes causal registration, the causal registration using only past location point sets in the plurality of location point sets to causally register each current location point set in the plurality of location point sets.

[0037] 13. The method according to Scheme 10, wherein the aggregation of the plurality of location point sets includes: Identify the number of cluster points in the neighborhood of each current location point in the plurality of cluster point sets; and The neighborhood is removed in response to the number of clustered points being less than a lower threshold number.

[0038] 14. The method according to Scheme 13, wherein the aggregation of the plurality of location point sets further includes: In response to the number of cluster points exceeding an upper limit threshold, the neighborhood is reduced to a random sample of cluster points within the neighborhood.

[0039] 15. The method according to Scheme 14, wherein the aggregation of the plurality of location point sets further includes: The singular value decomposition of the cluster points in the neighborhood is calculated to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, wherein the first singular vector and the second singular vector define a best-fit plane for the cluster points, and the third singular vector is normal to the best-fit plane; and In response to the ratio of the third singular value to the first singular value being greater than a ratio threshold, the neighborhood is filtered.

[0040] 16. The method according to Scheme 10, wherein the least mean square technique is used to calculate the generation of the plurality of correction transformation matrices under the low-angle correction assumption.

[0041] 17. A self-driving vehicle, comprising: A lidar device configured to perform multiple scans of the environment near the self-vehicle to generate multiple lidar point clouds associated with one or more objects in the environment; A controller, which communicates with the lidar device and is configured to transform the multiple lidar point clouds using multiple position transformation matrices to generate multiple position point sets, aggregate the multiple position point sets during a duration of interest to generate multiple aggregate point sets, register the multiple position point sets to the multiple aggregate point sets to generate multiple correction transformation matrices, update the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position matrices, and generate multiple motion sets based on the multiple updated position transformation matrices. A circuit that communicates with a controller and is configured to receive multiple motion sets and generate multiple commands in response to the multiple motion sets; and A drive system configured to control the movement of the self-driving vehicle in response to the plurality of commands.

[0042] 18. The self-vehicle according to Scheme 17, wherein the registration of the plurality of location point sets is a causal registration of each current location point set in the plurality of location point sets causally registered only using past location point sets in the plurality of location point sets.

[0043] 19. The self-vehicle according to claim 17 further includes a camera configured to sense the environment near the self-vehicle to generate multiple images.

[0044] 20. The self-vehicle according to Scheme 19, wherein the plurality of position transformation matrices align the plurality of motion sets and the plurality of images.

[0045] The foregoing features and advantages, as well as other features and advantages, of this disclosure will become apparent from the following detailed description of the best mode for carrying out this disclosure, taken in conjunction with the accompanying drawings. Attached Figure Description

[0046] Figure 1 This is a schematic diagram illustrating the system's context.

[0047] Figure 2 This is a schematic diagram illustrating a self-driving vehicle in another system according to one or more exemplary embodiments.

[0048] Figure 3 This is a flowchart of a motion estimation method for a self-vehicle according to one or more exemplary embodiments.

[0049] Figure 4 This is a schematic diagram of noncausal aggregation according to one or more exemplary embodiments.

[0050] Figure 5 This is a schematic diagram of informal registration according to one or more exemplary embodiments.

[0051] Figure 6 This is a schematic diagram of causal aggregation according to one or more exemplary embodiments.

[0052] Figure 7 This is a schematic diagram illustrating causal registration according to one or more exemplary embodiments.

[0053] Figure 8 This is a flowchart of registration steps according to one or more exemplary embodiments.

[0054] Figure 9 This is a schematic diagram of a set of aggregation points according to one or more exemplary embodiments.

[0055] Figure 10 It is a schematic diagram of a rapidly aggregated set of points according to one or more exemplary embodiments.

[0056] Figure 11 It is a schematic diagram of the best-fit plane according to one or more exemplary embodiments. Detailed Implementation

[0057] Embodiments of this disclosure provide a system and / or method for lidar-based ego vehicle motion estimation. In various embodiments, the method achieves high ego vehicle motion accuracy by performing non-causal registration between lidar device scans and convergence scans before and after the current scan. Non-causal means that convergence scans are not labeled with cause and effect. Therefore, convergence scans can be recorded during the journey, and non-causal registration of the current scan can be performed after the journey is complete. By iteratively performing this process, estimation errors and drift errors are minimized, thereby achieving high accuracy. In other embodiments, registration of the current scan can be performed in real time using only past convergence scans (e.g., causal registration).

[0058] LiDAR devices typically measure the environment around an autonomous vehicle at constant intervals (referred to as scans). For real-time methods, scan-to-scan registration is used to generate real-time autonomous vehicle motion estimates suitable for real-time driver assistance. The accuracy of the autonomous motion estimates suitable for the training dataset can be higher than that offered by scan-to-scan registration methods. Therefore, a non-real-time method is provided for generating datasets suitable for training autonomous driving systems. The non-real-time method uses a non-causal solution for an offline implementation with improved accuracy, which mitigates random walk errors and further improves the registration process.

[0059] refer to Figure 1 The diagram illustrates the scenario of system 80a. System 80a may include an environment 82, one or more objects (one shown) 84, a self-vehicle 90a, and a navigation monitor 100a. The self-vehicle 90a generally includes one or more cameras (one shown) 92, a lidar device 102, and a transmitter 104. The navigation monitor 100a includes a receiver 108, a controller 110, circuitry 112, a storage device 114, and an autonomous driving training system 116. The self-vehicle 90a and the navigation monitor 100a are configured to record and non-real-time (e.g., non-causal) analysis of the motion of the self-vehicle 90a.

[0060] The transmitted signal (Tx) can be generated and presented by the lidar device 102. The received signal (Rx) can be received by the lidar device 102. The received signal Rx is the transmitted signal Tx reflected by the object 84. The lidar point cloud signal (LPC) is typically generated by the lidar device 102 and provided to the transmitter 104. The signal LPC transmits the point cloud of the object 84 measured by the lidar device 102. The image signal (IMG) is generated by the camera 92 and provided to the transmitter 104. The signal IMG transmits a series of visual images of the surrounding environment 82 recorded by the camera 92.

[0061] The data signal (DT) is generated by transmitter 104, received by receiver 108, and transmitted to controller 110. Signal DT is a wired or wireless signal that transmits LiDAR point cloud data from vehicle 90a to navigation monitor 100a. The motion set signal (MS) is generated by controller 110 and provided to storage device 114 in circuitry 112. Signal MS carries the motion dataset derived from the LiDAR point cloud data. The stored data signal (SD) is generated by storage device 114 and transmitted to autonomous driving training system 116. Signal SD is the motion dataset generated by controller 110 and cached in storage device 114.

[0062] Object 84 may include, but is not limited to, moving objects (or targets), such as bicycles, handcarts, cars, trucks, motorcycles, boats, trains, and / or airplanes. In some embodiments, object 84 may include stationary objects (or targets), such as signs, pillars, buildings, mailboxes, lane markings, sidewalks, curbs, and / or walls. Other types of objects 84 capable of reflecting signal Tx may be implemented to meet the design criteria of a specific application.

[0063] Self-vehicle 90a includes, but is not limited to, moving vehicles (or objects), such as cars, trucks, motorcycles, boats, trains, and / or airplanes. Other types of self-vehicles can be implemented to meet the design criteria for specific applications.

[0064] Camera 92 is implemented as one or more optical cameras. Camera 92 is operable to capture images of the environment 82. The images (e.g., video) are presented in a signal IMG to transmitter 104 for subsequent use by autonomous driving training system 116.

[0065] The navigation monitor (or system) 100a is implemented as a navigation measurement, recording, and analysis system. In various embodiments, the navigation monitor 100a is divided between the autonomous vehicle 90a and the analysis system 106. The navigation monitor 100a is operable to repeatedly scan the environment 82 near the autonomous vehicle 90a using a lidar device 102 to generate a lidar point cloud associated with objects 84 in the environment 82. The navigation monitor 100a is also operable to transform the lidar point cloud using multiple position transformation matrices to generate multiple sets of position points, aggregate the position points within a duration of interest to generate multiple aggregated point sets, and register the position points to the aggregated point sets to generate multiple correction transformation matrices. The navigation monitor 100a can then update the position transformation matrices using the correction transformation matrices to generate multiple updated position transformation matrices, generate multiple motion sets based on the updated position transformation matrices, and store the motion sets in circuitry 112 (e.g., in storage device 114). From circuitry 112 / storage device 114, the motion sets are used as a learning database by the autonomous driving training system 116.

[0066] The lidar device 102 is implemented as a light detection and ranging device. The lidar device 102 uses pulsed laser waves to map the distances to objects 84 surrounding the vehicle 90a. The lidar device 102 is operable to measure distances to objects 84 from a few centimeters to up to 60 meters. When the vehicle 90a first passes through the environment 82, the lidar device 102 generates a lidar point cloud that accurately describes the motion of the environment 82 and the vehicle 90a. In the case of the vehicle 90a returning to the environment 82, the returning vehicle 90a is able to navigate the environment 82 in a predictable manner.

[0067] Transmitter 104 is implemented as a wireless transmitter. Transmitter 104 is operable to transmit lidar point cloud data from lidar device 102 and image data from camera 92 to analysis system 106.

[0068] The analysis system 106 is implemented as one or more computers (or processors). The analysis system 106 is operable to record and analyze lidar point cloud data and image data received from the autonomous vehicle 90a to determine the motion of the autonomous vehicle 90a. The lidar point cloud data and image data collectively form the basis of the autonomous vehicle's training dataset.

[0069] Receiver 108 is implemented as a wireless receiver. Receiver 108 is operable to transmit lidar point cloud data and image data from transmitter 104 to controller 110.

[0070] The controller 110 is implemented as one or more computers (or processors). The controller 110 communicates with the receiver 108 and the circuit 112. The controller 110 is operable to transform lidar point cloud data using multiple position transformation matrices to generate multiple position point sets, aggregate the position point sets for a duration of interest to generate an aggregated point set, register the position point sets to the aggregated point set to generate a correction transformation matrix, update the position transformation matrix using the correction transformation matrix to generate an updated position transformation matrix, generate a motion set based on the updated position transformation matrix, and store the motion set in the storage device 114.

[0071] Circuit 112 is implemented as a processing circuit. Circuit 112 includes a storage device 114. The storage device 114 is operable to store processed lidar cloud point data and image data received from the autonomous vehicle 90a for later use by the autonomous driving training system 116. The storage device 114 communicates with the autonomous driving training system 116.

[0072] The autonomous driving training system 116 is implemented as a training system for autonomous vehicles (e.g., self-vehicle 90a). The autonomous driving training system 116 is operable to teach the self-vehicle 90a how to react in various situations using processed motion and image data recorded for the self-vehicle 90a.

[0073] Reference Figure 2A schematic diagram illustrating another self-vehicle 90b in another system 80b is shown according to one or more exemplary embodiments. System 80b may include an environment 82, an object 84, self-vehicle 90b, and a navigation monitor 100b. Self-vehicle 90b generally includes a camera 92, a power unit 94, and a navigation monitor 100b. The portion of the navigation monitor 100b on self-vehicle 90b includes a lidar device 102, a controller 110, circuitry 112, a storage device 114, and a driver system 118. Optional portions of the navigation monitor 100b outside self-vehicle 90b include an autonomous driving training system 116. Self-vehicle 90b and navigation monitor 100b are configured to record and analyze the motion of self-vehicle 90b in real time (e.g., causally). Controller 110 communicates with lidar device 102 and circuitry 112.

[0074] Command signals (CMDs) can be generated by the driver system 118 and provided to the power unit 94. The command signals CMDs carry throttle, braking, gear selection, and / or steering commands generated by the driver system 118 to help control the vehicle 90b.

[0075] Self-vehicle 90b is a variation of self-vehicle 90a. Self-vehicle 90b is operable to use causal registration of LiDAR scanning and focusing scanning to aid in the control of self-vehicle 90b.

[0076] The power unit 94 serves as the propulsion, braking, and steering system for the self-contained vehicle 90b. The power unit 94 is operable in response to command signals CMD to provide power, gear selection, braking, and / or steering control.

[0077] The driver system 118 is implemented as a driver assistance system. The driver system 118 communicates with the power unit 94. The driver system 118 is operable to provide commands to the power unit 94 to assist the driver of the vehicle 90b in real-time control of the vehicle 90b.

[0078] Storage device 114 is implemented as a removable and / or downloadable device. Storage device 114 can communicate with autonomous driving training system 116 (if implemented). Processed lidar scan and / or image data can be transferred from storage device 114 to autonomous driving training system 116.

[0079] By performing causal registration of the lidar scan and focused scan in the autonomous vehicle 90b, the navigation monitor 100b can determine the real-time motion of the autonomous vehicle 90b. Real-time motion can include the acceleration, speed, deceleration, and yaw of the autonomous vehicle 90b. This real-time motion can be provided to the driver system 118 to aid in the control of the autonomous vehicle 90b.

[0080] Reference Figure 3The following flowchart illustrates an exemplary implementation of a method 140 for motion estimation of an ego vehicle, according to one or more exemplary embodiments. Method (or process) 140 can be used in embodiments of non-causal registration and causal registration. Method 140 generally includes steps 142 to 168, as shown. The order of the steps is shown as a representative example. Other orderings of steps can be implemented to meet the criteria of a particular application.

[0081] In step 142, the lidar device 102 scans the environment 82 near the autonomous vehicle 90a / 90b to generate lidar point clouds associated with objects 84 in the environment 82. In step 142, the camera 92 may generate image data. For autonomous vehicle 90a, in step 144, the transmitter 104 transmits lidar point cloud data and image data from the autonomous vehicle. For autonomous vehicle 90b, the storage device 114 records the aggregated lidar point cloud data and image data for subsequent driving assistance and optionally for download to the autonomous driving training system 116.

[0082] In step 146, receiver 108 in motion navigation monitor 100a receives LiDAR data and image data. Controller 110 in navigation monitor 100b reads the aggregated LiDAR point cloud data and image data from storage device 114. In step 148, controller 110 reads an initial position transformation matrix from storage device 114 or another available storage device. The transformation matrix typically allows the LiDAR point cloud data to be transformed from a local coordinate system to another coordinate system, such as the coordinate system of the image data from camera 92. In step 150, the position transformation matrix is ​​used in controller 110 to transform the LiDAR scan to generate a set of position points.

[0083] In step 152, controller 110 aggregates position points over the duration of interest to create an aggregated point set. The aggregated point set is stored in storage device 114. In step 154, controller 110 obtains the current LiDAR scan (either from the history of recorded LiDAR scans stored in storage device 114 in navigation monitor 100a, or from LiDAR device 102 in the vehicle 90b). As part of step 154, controller 110 registers the current LiDAR scan to the aggregated point set to create a correction transformation matrix. In step 154, controller 110 receives the aggregated scan and returns the correction for the position transformation (Mk_opt) for each scan.

[0084] In step 156, controller 110 updates the position transformation matrix using the correction transformation matrix to generate an updated position transformation matrix. Each existing position transformation (Lk_old) is updated according to the correction transformation matrix Mk_opt, which is the output of the scan-to-aggregate registration process. The update can be performed according to Equation 1 below: (1) Where k is the scan exponent. This update can utilize the least mean square technique under the low-angle correction assumption.

[0085] Steps 152-156 can be repeated multiple times to improve the accuracy of data registration. In step 158, the number of iterations through steps 152-156 is checked. If the number of iterations is less than a threshold value, method 140 returns to step 152 to register the current LiDAR scan to the updated aggregated data. Once the number of iterations reaches the threshold value, method 140 continues with step 160.

[0086] In step 160, controller 110 generates a motion set based on the updated position transformation matrix and current LiDAR data. In step 162, the motion set can be transmitted to circuit 112. For navigation monitor 100b, in step 164, the motion set can be consumed by autonomous driving training system 116 to aid in controlling autonomous vehicle 90b. In various embodiments, the motion set generated by autonomous vehicle 90b can be stored in storage device 114 for later transmission to autonomous driving training system 116. For navigation monitor 100a, in step 166, the motion set is stored in storage device 114 for later transmission to autonomous driving training system 116. In step 168, the processed motion set is aligned with camera images to provide a clearer understanding of the environment 82 surrounding autonomous vehicle 90a as it moves through the environment 82.

[0087] refer to Figure 4A schematic diagram of an example noncausal aggregation is shown according to one or more exemplary embodiments. Noncausal aggregation 152a may represent aggregation step 152 in method 140. A list of location point sets 170 containing recorded location point sets (e.g., past location point sets, current location point sets, and one or more future location point sets) is stored in storage device 114. Controller 110 determines a duration of interest 172a around the current location point set 174. The duration of interest 172a includes one or more future location point sets 176 and one or more past location point sets 178 in time relative to the current location point set 174. Using the location point sets within the duration of interest 172a, excluding the current location point set 174, controller 110 calculates a noncausal aggregation point set 182a. The noncausal aggregation point set 182a is stored in storage device 114 as part of a list of noncausal aggregation point sets 180a. The calculation of the noncausal aggregation point set may be repeated for each current location point set 174 in the location point set 170.

[0088] refer to Figure 5 A schematic diagram of an example noncausal registration is shown according to one or more exemplary embodiments. Noncausal registration 154a may represent registration step 154 ​​in method 140. Controller 110 may register the current location point set 174 with the corresponding noncausal cluster point set 182a from the noncausal cluster point set list 180a. The difference between the current location point set 174 and the corresponding noncausal cluster point set 182a is used to determine the noncausal correction transformation matrix 192a. The noncausal correction transformation matrix 192a is stored in a list of noncausal correction transformation matrices 190a. This registration is repeated for the location point sets in the list of location point sets 170 to populate the correction transformation matrices in the list of noncausal correction transformation matrices 190a.

[0089] refer to Figure 6A schematic diagram of an example non-automatic aggregation is shown according to one or more exemplary embodiments. Non-causal aggregation 152B may represent aggregation step 152 in method 140. A list of location point sets 170 containing recorded location point sets (e.g., past location point sets, current location point sets, and possibly one or more future location point sets) is stored in storage device 114. The duration of interest 172b prior to the current location point set 174 is determined by controller 110. The duration of interest 172b includes multiple past location point sets 178 in time relative to the current location point set 174. Using the location point sets within the duration of interest 172b, excluding the current location point set 174, controller 110 calculates a causal aggregation location point set 182b. The causal aggregation point set 182b is stored in storage device 114 as part of a list of causal aggregation point sets 180b. The calculation of the causal aggregation location point set may be repeated for location point sets 174 in the list of location point sets 170.

[0090] refer to Figure 7 A schematic diagram of an example causal registration is shown according to one or more exemplary embodiments. Causal registration 154b may represent registration step 154 ​​in method 140. Controller 110 may register the current location point set 174 with a corresponding causal aggregation point set 182b from a list of causal aggregation point sets 180b. The difference between the current location point set 174 and the corresponding causal aggregation point set 182b is used to determine the current causal correction transformation matrix 192b. The registration is repeated for each location point set in the list of location point sets 170 to populate the correction transformation matrix in the list of causal correction transformation matrices 190b.

[0091] refer to Figure 8 The flowchart illustrates an example implementation of registration step 154 ​​of method 140 according to one or more exemplary embodiments. Step 154 ​​includes steps 200 to 218, as shown. The order of the steps is shown as a representative example. Other orderings of steps may be implemented to meet the criteria of a particular application.

[0092] Each scan is registered with a cluster scan excluding itself. Registration is between each point in the scan and a plane in the cluster. In step 200, controller 110 selects the current scan that defines multiple location points around the current location of interest. In step 202, multiple location points are calculated to define a neighborhood around the current location of interest. In step 204, controller 110 identifies multiple cluster points within the neighborhood. If the number of cluster points is less than a lower threshold number, the neighborhood is removed in step 206. For neighborhoods with a number greater than the lower threshold number of cluster points, in step 208, controller 110 calculates the normal of the neighborhood. This normal is a vector normal to the best-fit plane passing through the neighborhood. If the number of cluster points in the neighborhood is greater than an upper threshold value, in step 210, controller 110 randomly selects some cluster points for further processing.

[0093] In step 212, controller 110 calculates the singular value decomposition (SVD) of the clustered points in the mean-removed neighborhood to determine the first singular vector, first singular value, second singular vector, second singular value, third singular value vector, and third singular value. The first and second singular vectors define the best-fit plane. The third singular vector is normal to the best-fit plane. Controller 110 then filters the neighborhood in step 214 in response to a ratio of the third singular value to the first singular value exceeding a ratio threshold to remove one or more clustered points. In step 216, under the low-angle correction assumption, the least mean square technique is used to calculate the correction transformation matrix (Mopt). This calculation can be provided by the following Equation 2: (2) in Where s is the scan point, d is the aggregation point, and n is the normal.

[0094] The dimensions of s, d, and n are 3×N, where the first dimension consists of (x, y, z), and N is the number of points. The calculation is performed for each scan.

[0095] In step 218, if more scans are to be considered, processing returns to step 200 to select another current scan. Once a scan has been processed, step 154 ​​continues with an update step 156. Figure 3 ).

[0096] refer to Figure 9 The diagram illustrates an example of a cluster set 182c according to one or more exemplary embodiments. Cluster set 182c represents a cluster set in a list of non-causal cluster sets 182a and a list of causal cluster sets 182b.

[0097] The neighborhood 230 surrounding the current location point 174 is defined as the region within a radius 232 extending from the current location point 174. Multiple cluster points 234a-234h are scattered both inside and around the neighborhood 230. For clustering purposes, cluster points 234d-234g within the neighborhood 230 are considered. If the number of cluster points 234d-234g is less than a lower threshold value, then according to... Figure 8 In step 206, neighborhood 230 is discarded. Otherwise, a set of aggregate points 182a / 182b is generated from aggregate points 234d-234g within each neighborhood 230 around each location point 174.

[0098] refer to Figure 10 The diagram illustrates an example of a sharply clustered point set 182d according to one or more exemplary embodiments. The clustered point set 182d represents a clustered point set in a list of non-causal clustered point sets 182a and a list of causal clustered point sets 182b.

[0099] If the number of clustered points within neighborhood 230 exceeds an upper limit threshold, controller 110 randomly removes some clustered points 234a-234n (e.g., removed clustered points 236a-236d) to reduce the total number below the threshold value. Figure 8 The upper limit threshold limit in step 210. Thereafter, a set of cluster points 182a / 182b is generated from the cluster points 234a-234n within each neighborhood 230 around each location point 174.

[0100] refer to Figure 11 A schematic diagram of an exemplary best-fit plane 240 is shown according to one or more exemplary embodiments. The best-fit plane 240 defines the plane closest to the fit among the aggregation points 234a-234n. A first singular vector having a first singular value is shown as vector 242. A second singular vector having a second singular value is shown as vector 244. The first singular vector 242 and the second singular vector 244 define the best-fit plane 240. A third singular vector having a third singular value is shown as vector 246. The third singular vector 246 is normal to the best-fit plane 240.

[0101] Embodiments of this disclosure may provide a navigation monitor (or system) 100a disposed in an autonomous vehicle 90a and configured to provide accurate measurements of the motion of the autonomous vehicle 90a suitable for use as a training database for the autonomous vehicle. The navigation monitor 100a includes a lidar device 102 configured to perform multiple scans of an environment 82 near the autonomous vehicle 90a to generate multiple lidar point clouds associated with one or more objects 84 in the environment 82. The navigation monitor 100a includes a controller 110 configured to transform the multiple lidar point clouds using multiple position transformation matrices to generate multiple sets of position points, aggregate the multiple sets of position points over a duration of interest to generate multiple aggregated point sets, register the multiple sets of position points to the multiple aggregated point sets to generate multiple correction transformation matrices, update the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position transformation matrices, and generate multiple accurate motion sets based on the multiple updated position transformation matrices. The navigation monitor 100a includes circuitry 112 configured to receive the multiple motion sets. The action set is stored in storage device 114 for subsequent consumption by autonomous driving training system 116 as a training dataset.

[0102] All numerical values ​​of parameters (e.g., quantities or conditions) in this specification (including the appended claims) should in all cases be understood to be modified by the term "about / approximately," regardless of whether "about / approximately" actually appears before the numerical value. "About" implies that the numerical value allows for some slight imprecision (some closeness to the exact value; approximately or reasonably close to the value; almost). If the imprecision provided by "about" cannot be understood in this ordinary sense in the art, then "about" as used herein at least indicates variations that may arise from common methods of measuring and using these parameters. Furthermore, the disclosure of ranges includes the disclosure of all values ​​and ranges further subdivided throughout the range. Each value within the range and the endpoints of the range are disclosed herein as separate embodiments.

[0103] While the best mode for carrying out this disclosure has been described in detail, those skilled in the art to which this disclosure pertains will recognize various alternative designs and embodiments for carrying out this disclosure within the scope of the appended claims.

Claims

1. A motion monitor, comprising: A lidar device, wherein the lidar device is disposed in a self-driving vehicle and is configured to perform multiple scans of the environment near the self-driving vehicle to generate multiple lidar point clouds associated with one or more objects in the environment. A controller, which communicates with the lidar device and is configured to transform the multiple lidar point clouds using multiple position transformation matrices to generate multiple position point sets, aggregate the multiple position point sets during a duration of interest to generate multiple aggregate point sets, register the multiple position point sets to the multiple aggregate point sets to generate multiple correction transformation matrices, update the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position transformation matrices, and generate multiple motion sets based on the multiple updated position transformation matrices. as well as A circuit that communicates with the controller and is configured to receive the plurality of motion sets; The registration of the multiple location point sets includes: Identify the number of cluster points in the plurality of cluster point sets within the neighborhood of each current location point in the plurality of location point sets; and The neighborhood is removed in response to the number of cluster points being less than a lower threshold number; In response to the number of cluster points exceeding an upper threshold value, the neighborhood is reduced to a random sample of cluster points within the neighborhood; The singular value decomposition of the cluster points in the neighborhood is calculated to determine the first singular vector, the first singular value, the second singular vector, the third singular vector, and the third singular value, wherein the first singular vector and the second singular vector define the best-fit plane of the cluster points, and the third singular vector is normal to the best-fit plane. In response to the ratio of the third singular value to the first singular value being greater than a ratio threshold, the neighborhood is filtered.

2. The motion monitor according to claim 1, wherein, The registration of the multiple location point sets is causal registration, which uses only the past location point sets in the multiple location point sets to causally register each current location point set in the multiple location point sets.

3. The motion monitor according to claim 1, wherein, The least mean square technique is used to calculate the generation of the plurality of correction transformation matrices under the low-angle correction assumption.

4. The motion monitor according to claim 1, wherein, The circuit is a storage device configured to record the plurality of motion sets, and the plurality of motion sets form a training dataset for an autonomous driving training system.

5. The motion monitor according to claim 1, wherein, The circuit is a drive system arranged in the self-vehicle and configured to control the movement of the self-vehicle in response to the plurality of motion sets.

6. A method for motion estimation of an autonomous vehicle, comprising: The environment near the vehicle is scanned multiple times using a lidar device to generate multiple lidar point clouds associated with one or more objects in the environment. Multiple position transformation matrices are used to transform the multiple lidar point clouds to generate multiple position point sets; Aggregate the multiple location point sets within the duration of interest to generate multiple aggregation point sets; Register the plurality of location point sets to the plurality of aggregation point sets to generate a plurality of correction transformation matrices; The plurality of position transformation matrices are updated using the plurality of correction transformation matrices to generate a plurality of updated position transformation matrices; Multiple motion sets are generated based on the multiple updated position transformation matrices; as well as The plurality of motion sets are received at the circuit; The registration of the multiple location point sets includes: Identify the number of cluster points in the neighborhood of each current location point in the plurality of cluster point sets; The neighborhood is removed in response to the number of cluster points being less than a lower threshold number; In response to the number of cluster points exceeding an upper limit threshold, the neighborhood is reduced to a random sample of cluster points within the neighborhood; The singular value decomposition of the cluster points in the neighborhood is calculated to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, wherein the first singular vector and the second singular vector define a best-fit plane for the cluster points, and the third singular vector is normal to the best-fit plane; and In response to the ratio of the third singular value to the first singular value being greater than a ratio threshold, the neighborhood is filtered.

7. The method according to claim 6, wherein, The registration of each of the plurality of location point sets includes causal registration, which uses only the past location point sets in the plurality of location point sets to causally register each current location point set in the plurality of location point sets.

8. The method according to claim 6, wherein, The least mean square technique is used to calculate the generation of the plurality of correction transformation matrices under the low-angle correction assumption.

9. A self-driving vehicle, comprising: A lidar device configured to perform multiple scans of the environment near the self-vehicle to generate multiple lidar point clouds associated with one or more objects in the environment; A controller, which communicates with the lidar device and is configured to transform the multiple lidar point clouds using multiple position transformation matrices to generate multiple position point sets, aggregate the multiple position point sets during a duration of interest to generate multiple aggregate point sets, register the multiple position point sets to the multiple aggregate point sets to generate multiple correction transformation matrices, update the multiple position transformation matrices using the multiple correction transformation matrices to generate multiple updated position matrices, and generate multiple motion sets based on the multiple updated position transformation matrices. The circuit communicates with the controller and is configured to receive multiple motion sets and generate multiple commands in response to the multiple motion sets; as well as A drive system configured to control the movement of the autonomous vehicle in response to the plurality of commands; The registration of the multiple location point sets includes: Identify the number of cluster points in the neighborhood of each current location point in the plurality of cluster point sets; The neighborhood is removed in response to the number of cluster points being less than a lower threshold number; In response to the number of cluster points exceeding an upper limit threshold, the neighborhood is reduced to a random sample of cluster points within the neighborhood; The singular value decomposition of the cluster points in the neighborhood is calculated to determine a first singular vector, a first singular value, a second singular vector, a third singular vector, and a third singular value, wherein the first singular vector and the second singular vector define a best-fit plane for the cluster points, and the third singular vector is normal to the best-fit plane; and In response to the ratio of the third singular value to the first singular value being greater than a ratio threshold, the neighborhood is filtered.

10. The self-driving vehicle according to claim 9, wherein, The registration of the multiple location point sets is a causal registration of each current location point set in the multiple location point sets by using only the past location point sets in the multiple location point sets.

11. The self-driving vehicle of claim 9, further comprising a camera configured to sense the environment near the self-driving vehicle to generate multiple images.

12. The self-driving vehicle according to claim 11, wherein, The plurality of position transformation matrices align the plurality of motion sets and the plurality of images.

Images