Robot scene perception method and system based on multi-sensor fusion

Abstract

The present application relates to the field of measurement and navigation technology, and particularly relates to a robot scene perception method and system based on multi-sensor fusion. First, the starting exposure time of the current frame image is taken as the reference, the robot body motion displacement in the laser radar measurement points is removed by using the inertial data, and the sensor sampling time difference is obtained; further, the vertical edge point set is filtered and the candidate velocity grid is constructed, for each candidate velocity vector, the body motion compensation point is projected to the image plane to obtain the deduced pixel coordinates, and the expected tilt angle is calculated; further, the optimal velocity vector is selected by using the position projection and the double constraints of the rolling shutter shear slope; finally, the corresponding body motion compensation point is extrapolated to the preset prediction target time by using the optimal velocity vector, the space-time aligned scene point cloud data is generated, the dynamic distortion caused by the non-synchronous sampling is eliminated, and the zero-delay accurate scene perception is realized.

Description

Technical Field

[0001] This invention relates to the field of measurement and navigation technology, specifically to a robot scene perception method and system based on multi-sensor fusion. Background Technology

[0002] Industrial mobile robots are often equipped with mechanical rotating LiDAR and rolling shutter cameras as sensing devices for operation in dynamic environments. Mechanical LiDAR performs omnidirectional scanning through continuous rotation, resulting in time differences between measurement points at different angles. Rolling shutter cameras use a line-by-line exposure mechanism, leading to variations in the exposure times of different image lines. When moving obstacles (such as forklifts or AGVs) exist around the robot, this asynchronous sampling mechanism causes the position of the same object to not coincide with the position in the point cloud and the image, and the vertical edges of the object to exhibit tilting deformation in the image due to relative motion.

[0003] Existing technologies often assume instantaneous synchronization of sensor data or only compensate for the robot's own motion, ignoring observational distortions caused by the motion of external objects. This results in a spatiotemporal misalignment between the obstacle positions analyzed from the point cloud data received by the navigation system and their actual positions, leading to the need for excessively large safety margins during path planning or the occurrence of obstacle avoidance misjudgments. Summary of the Invention

[0004] To address the technical problem of distortion in existing robot point cloud data affecting the accuracy of path planning during navigation, this invention aims to provide a robot scene perception method and system based on multi-sensor fusion. The specific technical solution adopted is as follows:

[0005] A robot scene perception method based on multi-sensor fusion, the method comprising:

[0006] Acquire observation data from a mechanical rotating lidar and a rolling shutter camera; using the initial exposure time of the current frame image as a reference, use inertial data to remove the robot body motion displacement in the lidar measurement points, generate body motion compensation points, and calculate the sensor sampling time difference between the laser acquisition time of each body motion compensation point and the exposure time of the corresponding pixel of the camera exposure.

[0007] Based on the spatial distribution of the body motion compensation points, a set of vertical edge points is selected. For each set of vertical edge points, a candidate velocity grid is constructed. For each candidate velocity vector, the body motion compensation points are projected onto the image plane in combination with the sensor sampling time difference to obtain the inferred pixel coordinates. The expected tilt angle is calculated based on the hardware parameters of the rolling shutter and the depth coordinates of the body motion compensation points. Based on the image edge features, the spatial distance error of the inferred pixel coordinates and the difference between the expected tilt angle and the edge tangent direction are analyzed. The optimal velocity vector corresponding to the set of vertical edge points is selected from the candidate velocity grid.

[0008] The corresponding body motion compensation point is extrapolated to the preset prediction target time using the optimal velocity vector to generate spatiotemporally aligned scene point cloud data.

[0009] Furthermore, the method for obtaining the body motion compensation points includes:

[0010] The inertial data includes angular velocity and acceleration data of the lidar collected by the inertial measurement unit;

[0011] For each measurement point acquired by the lidar, the sampling time offset relative to the initial exposure time is obtained; based on the sampling time offset, the angular velocity and acceleration data of the lidar in the corresponding time period are integrated over time to obtain the relative pose transformation matrix; based on the original spatial coordinates of the measurement point, the relative pose transformation matrix and the rigid body transformation matrix of the lidar coordinate system relative to the camera coordinate system are used to perform an inverse transformation to obtain the body motion compensation point.

[0012] Furthermore, the method for obtaining the sensor sampling time difference includes:

[0013] For each of the body motion compensation points, the exposure time of the corresponding pixel is obtained based on the camera's intrinsic parameter matrix and row readout time constant; the sensor sampling time difference is obtained based on the difference between the laser acquisition time and the corresponding exposure time.

[0014] Furthermore, the method for obtaining the vertical edge point set includes:

[0015] Within a preset spatial neighborhood of each of the body motion compensation points, the local normal vector is calculated using covariance matrix decomposition based on the coordinates of each point in the spatial nearest point set. The local normal vector is compared with the gravity direction vector to select vertical edge points from the body motion compensation points and construct a set of vertical edge points.

[0016] Furthermore, the method for obtaining the derived pixel coordinates includes:

[0017] Within the currently analyzed set of vertical edge points, target motion compensation points are selected one by one. For each candidate velocity vector, the candidate velocity vector and the sensor sampling time difference are fused to obtain a displacement compensation vector. The displacement compensation vector and the spatial coordinates of the target motion compensation point are fused, and combined with the camera's intrinsic parameter matrix, the target motion compensation point is projected onto the image plane to obtain the deduced pixel coordinates.

[0018] Furthermore, the method for obtaining the expected tilt angle includes:

[0019] The hardware parameters include at least the camera's focal length and line readout time constant. The product of the lateral velocity component corresponding to the candidate velocity vector, the focal length, and the line readout time constant is used as the numerator, and the absolute value of the depth coordinate of the body motion compensation point in the camera's coordinate system is used as the denominator. The expected tilt angle is obtained by performing an arctangent transformation on the ratio of the fractions.

[0020] Furthermore, the method for obtaining the optimal velocity vector includes:

[0021] Extract the edge distance index map and edge angle map of the current frame image;

[0022] For each of the body motion compensation points, the actual edge distance and actual edge angle are obtained by querying the edge distance index map and the edge angle map based on the inferred pixel coordinates; the cost factor corresponding to the body motion compensation point is obtained based on the difference between the actual edge angle and the expected tilt angle, combined with the actual edge distance.

[0023] By fusing the cost factors of all the body motion compensation points under the same candidate velocity vector, a matching cost value is obtained, and the candidate velocity vector with the smallest matching cost value is selected as the optimal velocity vector.

[0024] Furthermore, the method for acquiring the scene point cloud data includes:

[0025] The preset prediction target time is later than the starting exposure time and all the laser acquisition times; for each body motion compensation point, the time span from the original sampling time to the prediction target time is calculated; based on the spatial coordinates of the body motion compensation point, the time span and the corresponding optimal velocity vector are fused to obtain the spatial coordinates of the corresponding body motion compensation point at the prediction target time;

[0026] The spatial coordinates of all the aforementioned body motion compensation points at the predicted target time are used to construct scene point cloud data.

[0027] Furthermore, the method for obtaining the candidate velocity grid includes:

[0028] Based on the preset nearest obstacle distance, camera focal length, and the absolute value of the sensor sampling time difference with the largest amplitude, a search speed step size is set, and candidate speed vectors are generated within a preset speed search range to form a candidate speed grid.

[0029] The present invention also proposes a robot scene perception system based on multi-sensor fusion. The system includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements any of the steps of the robot scene perception method based on multi-sensor fusion described above.

[0030] The present invention has the following beneficial effects:

[0031] This invention first decouples the robot's motion based on the image exposure time and quantifies the minute sampling time difference between sensors. Second, utilizing the rolling shutter effect, it constructs dual constraints based on positional consistency (based on inferred pixel coordinates) and morphological consistency (based on expected tilt angles), selecting the optimal velocity vector within a single frame of data, overcoming the limitation of traditional methods that cannot measure velocity instantaneously. Finally, this velocity vector is used to extrapolate the lagging point cloud data to the future predicted target time, generating a spatiotemporally strictly aligned scene point cloud. This process effectively eliminates perception delays and geometric distortions in dynamic environments, ensuring that the robot can perform path planning based on accurate and realistic obstacle locations, significantly improving obstacle avoidance safety and efficiency. Attached Figure Description

[0032] To more clearly illustrate the technical solutions and advantages in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0033] Figure 1 A flowchart illustrating a robot scene perception method based on multi-sensor fusion, as provided in one embodiment of the present invention;

[0034] Figure 2 This is a flowchart illustrating a method for obtaining an optimal velocity vector according to an embodiment of the present invention. Detailed Implementation

[0035] To further illustrate the technical means and effects adopted by the present invention to achieve its intended purpose, the following, in conjunction with the accompanying drawings and preferred embodiments, details the specific implementation, structure, features, and effects of a robot scene perception method and system based on multi-sensor fusion proposed according to the present invention. In the following description, different "one embodiment" or "another embodiment" do not necessarily refer to the same embodiment. Furthermore, specific features, structures, or characteristics in one or more embodiments can be combined in any suitable form.

[0036] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0037] The following description, in conjunction with the accompanying drawings, details a specific scheme for a robot scene perception method and system based on multi-sensor fusion provided by the present invention.

[0038] Please see Figure 1 The diagram illustrates a flowchart of a robot scene perception method based on multi-sensor fusion according to an embodiment of the present invention, specifically including:

[0039] Step S1: Acquire observation data from the mechanical rotating lidar and the rolling shutter camera; using the initial exposure time of the current frame image as a reference, use inertial data to remove the robot body motion displacement in the lidar measurement points, generate body motion compensation points, and calculate the sensor sampling time difference between the laser acquisition time of each body motion compensation point and the exposure time of the corresponding pixel of the camera exposure.

[0040] This invention is applicable to robots equipped with mechanical lidar and rolling shutter cameras. It aims to establish a precise spatiotemporal mapping relationship between lidar measurement data and camera image data, and to uniformly extrapolate preset target times to generate spatiotemporally aligned scene point cloud data, providing accurate data support for path planning during navigation.

[0041] In one embodiment of the present invention, observation data from a mechanical rotating lidar and a rolling shutter camera are first acquired to provide a data basis. In order to unify the time and space reference system of the multi-source sensors, the system first defines the start exposure time of the current frame image as a reference to provide a time reference, representing the zero point of time.

[0042] Since the point cloud data of the LiDAR is collected sequentially at different times, the robot's own movement causes spatial distortion of the original point cloud. Therefore, inertial data is used to remove the robot's own motion displacement in the LiDAR measurement points, generate body motion compensation points, transform the LiDAR measurement points to the camera coordinate system, and remove the background displacement introduced by the robot's own movement, thus completing the unification of the time and space reference system of the multi-source sensors.

[0043] Preferably, in one embodiment of the present invention, since the lidar is omnidirectional (360 degrees) while the camera only has a forward field of view (e.g., -45 to +45 degrees), only the laser points falling within the camera's field of view can be fused with the image. Therefore, before the initial analysis, the system reads pre-calibrated hardware parameters, including the rigid transformation matrix of the lidar coordinate system relative to the camera coordinate system. Camera intrinsic parameter matrix and the camera's line readout time constant Among them, the row readout time constant It is an inherent hardware property of rolling shutter cameras, representing the physical time interval required for the camera sensor to read a line of pixels, which is used to subsequently calculate the pixel-level exposure time.

[0044] Based on the rigid body transformation matrix of the lidar to the camera, the horizontal field of view of the camera (e.g., -45 to +45 degrees) is reverse-mapped from the camera coordinate system to the lidar coordinate system, thereby dynamically determining an effective horizontal angle preservation interval. Only measurement points within the current scanning cycle whose horizontal azimuth angle falls within the horizontal angle preservation interval are selected for subsequent processes.

[0045] The current scan cycle refers to the scan cycle that includes the start exposure time of the current frame image. When the start exposure time is exactly between two adjacent scan cycles, one of them can be selected, for example, the scan cycle with the shortest timing sequence can be selected.

[0046] Inertial data includes angular velocity and acceleration data of the lidar collected by the inertial measurement unit (IMU). The inertial measurement unit (IMU) is rigidly connected to the lidar to ensure data availability.

[0047] The analysis process is the same for each measurement point acquired by the lidar; only one example will be described here.

[0048] The rotation azimuth angle and firing frequency of the lidar determine the absolute acquisition time of each measurement point. By comparing this with the initial exposure time (the reference point or zero point), the sampling time offset relative to the initial exposure time is obtained (the difference between the absolute acquisition time and the initial exposure time).

[0049] By integrating the angular velocity and acceleration data of the lidar in the corresponding time period based on the sampling time offset, the relative pose change of the lidar in the corresponding time period can be solved, and the relative pose transformation matrix can be obtained to provide a basis for coordinate alignment.

[0050] Among them, the minimum value between the absolute acquisition time and the initial exposure time is used as the lower limit of time integration, and the maximum value between the absolute acquisition time and the initial exposure time is used as the upper limit of time integration. When the sampling time offset is 0, it means that the absolute acquisition time and the initial exposure time are exactly the same, and the relative pose transformation matrix is ​​an identity matrix. The method of obtaining the relative pose transformation matrix belongs to the basic derivation of inertial navigation and will not be elaborated further.

[0051] The original spatial coordinates of the measurement points are the coordinates in the LiDAR coordinate system. The relative pose transformation matrix provides the relative motion information of the LiDAR body coordinate system, and the rigid body transformation matrix provides the fixed spatial transformation relationship between the LiDAR coordinate system and the camera coordinate system. Therefore, based on the original spatial coordinates of the measurement points, the relative pose transformation matrix and the rigid body transformation matrix of the LiDAR coordinate system relative to the camera coordinate system are used to perform inverse transformation to obtain the body motion compensation points. The point cloud is uniformly transformed to the camera coordinate system, which establishes a unified coordinate reference for subsequent spatiotemporal alignment and feature fusion with image data, and removes the robot body motion displacement in the LiDAR measurement points. The candidate velocity vectors, position coordinates, etc. in the subsequent analysis are all performed in the camera coordinate system.

[0052] As an example, the coordinate transformation equations include: In the formula, This represents the original spatial coordinates of the i-th measurement point; i is the index (serial number) of the measurement point. This represents the relative pose transformation matrix of the i-th measurement point; This represents the rigid body transformation matrix, which uses coordinate transformation equations to transform each measurement point to the coordinate system of the current frame image. This represents the spatial coordinates of the i-th measurement point in the camera coordinate system.

[0053] After transformation, the motion compensation point of the body inherits the index of the measurement point, and the two represent different observation states of the same physical point in space.

[0054] The discrepancy between the time when the lidar acquires the motion compensation point of the object and the exposure time of the corresponding pixel of the camera is the root cause of the misalignment of the dynamic object in the image and point cloud. Therefore, the sensor sampling time difference between the laser acquisition time of each motion compensation point and the exposure time of the corresponding pixel of the camera is calculated to provide a basis for subsequent analysis.

[0055] Preferably, in one embodiment of the present invention, for each body motion compensation point, the exposure time of the corresponding pixel is first obtained based on the camera's intrinsic parameter matrix and row readout time constant;

[0056] As an example, the projection row number on the image is obtained by projecting the camera's intrinsic parameter matrix onto the image plane. In this process, the projection row number of the body motion compensation point is used as a first-order approximation, ignoring the minute changes in the row number caused by the object's own displacement within a very short sampling time. Due to the camera's rolling shutter exposure mechanism, the row number determines the corresponding pixel exposure time.

[0057] The sensor sampling time difference is obtained based on the difference between the laser acquisition time and the corresponding exposure time.

[0058] As an example, the formula for calculating the sensor sampling time difference includes: ;

[0059] in, The sensor sampling time difference corresponding to the i-th body motion compensation point; This indicates the acquisition time of the i-th body motion compensation point by the lidar; Indicates the projection row number corresponding to the i-th body motion compensation point; This represents the camera's line readout time constant.

[0060] It should be noted that the rigid body transformation matrix is ​​determined by the actual installation situation. The method of obtaining the transformation matrix between the two coordinate systems to obtain the rigid body transformation matrix required in this embodiment is a well-known technology. The intrinsic parameter matrix describes the mapping relationship of the camera from the pixel coordinate system to the image coordinate system. The camera's intrinsic parameter matrix and row readout time constant are common knowledge. The acquisition settings of the camera and LiDAR can be set by the implementer as needed and are no longer limited.

[0061] Step S2: Based on the spatial distribution of the body motion compensation points, select the vertical edge point set. For each vertical edge point set, construct a candidate velocity grid. For each candidate velocity vector, combine the sensor sampling time difference to project the body motion compensation points onto the image plane to obtain the inferred pixel coordinates. Calculate the expected tilt angle based on the hardware parameters of the rolling shutter and the depth coordinates of the body motion compensation points. Analyze the spatial distance error of the inferred pixel coordinates and the difference between the expected tilt angle and the edge tangent direction based on image edge features. Select the optimal velocity vector for the corresponding vertical edge point set from the candidate velocity grid.

[0062] The geometric shear deformation caused by the rolling shutter effect is most pronounced on physical structures perpendicular to the ground (such as columns, corners, and the sides of mobile devices), while the projection characteristics of ground points or random points are not regular. To improve computational efficiency and accuracy, it is necessary to filter from the full point cloud (all body motion compensation points). Therefore, the set of vertical edge points is selected based on the spatial distribution of body motion compensation points.

[0063] Preferably, in one embodiment of the present invention, firstly, within a preset spatial neighborhood of each body motion compensation point, the local normal vector is calculated using covariance matrix decomposition based on the coordinates of each point in the spatial nearest point set, representing the spatial orientation of the local surface where the corresponding point is located; typically, the gravity direction vector is perpendicular to the ground, and by comparing the local normal vector with the gravity direction vector, vertical edge points are selected from the body motion compensation points and a set of vertical edge points is constructed.

[0064] As an example, for any body motion compensation point, the system searches for the k nearest neighbor set of the body motion compensation point in three-dimensional space to form the corresponding preset spatial neighborhood; in this example, k=10, and the spatial nearest neighbor set is obtained.

[0065] Calculating local normal vectors is a common technique, which will be briefly described here: take the coordinates of each point in the spatial nearest point set as input, calculate the centroid (mean) and centered data (mean-removed) of the spatial nearest points, construct a 3×3 covariance matrix, and finally perform eigenvalue decomposition (EVD) to take the eigenvector corresponding to the smallest eigenvalue as the local surface normal vector.

[0066] Calculate the dot product of the local normal vector and the gravity direction vector. If the dot product result is close to 0, for example, if the absolute value of the dot product result is less than the preset vertical judgment threshold of 0.1, it indicates that the local surface where the body motion compensation point is located is perpendicular to the ground. The body motion compensation point is judged as a vertical edge point and added to the vertical edge point set.

[0067] Since there may be multiple disconnected independent obstacles in the scene (such as scattered pillars or multiple mobile AGVs), there may be multiple sets of vertical edge points. For the selected vertical edge points, Euclidean distance clustering is used to group them. Specifically, a clustering distance threshold (such as 0.5m) is set, and points with a spatial distance less than the clustering distance threshold are grouped into the same set, while points with a distance greater than this threshold are grouped into different sets, thereby dividing the discrete vertical edge points into independent sets of vertical edge points.

[0068] Meanwhile, when there are too few points in a vertical edge point set, it is likely to be noise or edge debris. Therefore, an empirical lower limit threshold is also set. For example, if the voxel size is 0.5m×0.5m×1m and the laser angular resolution is 0.2 degrees, a voxel can usually accommodate 10 to 30 points at a distance of 5m. The empirical lower limit threshold is set to 5-10, and 10 is set in this example. Subsequently, only the body motion compensation points in the (screened) vertical edge point set are analyzed.

[0069] It should be noted that for unconventional scenarios with inclined ground, the reference direction vector perpendicular to the ground is obtained to replace the gravity direction vector for analysis; in other embodiments of the present invention, the implementer can adjust the empirical lower limit threshold and the parameter k of the spatial nearest neighbor set according to the actual scenario.

[0070] Because the dynamic changes in the environment faced by mobile robots are sudden and instantaneous, relying on historical trajectory tracking based on multi-frame data introduces decision-making delays, which cannot meet the high timeliness requirements of real-time obstacle avoidance. Therefore, in order to immediately solve for the instantaneous unknown velocity of dynamic obstacles from a single frame of observation data, it is necessary to transform the continuous velocity domain into a computable and searchable discrete space, and generate a corresponding geometric shape prediction that can be verified by image observation for each possible velocity hypothesis.

[0071] Therefore, a candidate velocity grid is constructed for each set of vertical edge points. Then, for each candidate velocity vector in the grid, two parallel geometric deductions are performed: First, the motion displacement of the obstacle under the candidate velocity assumption is compensated to the body motion compensation point by combining the sensor sampling time difference, and then projected onto the image plane to calculate the corresponding deduced pixel coordinates; Second, based on the hardware parameters of the rolling shutter and the depth coordinates of the body motion compensation point, the expected tilt angle that the vertical edge should present on the image is derived according to the corresponding velocity assumption.

[0072] These two deductions together constitute a complete geometric description of the velocity hypothesis. The degree of agreement between the hypothesis and the features of the real image will become the core basis for judging whether the corresponding velocity is the real solution, thus providing a basis for obtaining the optimal velocity vector in the future.

[0073] Preferably, in one embodiment of the present invention, in order to accurately lock the true motion state of the obstacle from an infinitely continuous velocity domain, it is first necessary to construct a finite and reasonably resolution discrete search space.

[0074] Considering that the shearing deformation caused by the rolling shutter in the image is more sensitive to nearby obstacles, and the system needs to ensure sufficient velocity resolution under the most unfavorable distance conditions, while avoiding excessive computation due to too small a step size, the search velocity step size is set according to the preset nearest obstacle distance, camera focal length, and the absolute value of the sensor sampling time difference with the largest amplitude. Candidate velocity vectors are generated within the preset velocity search range to form a candidate velocity grid.

[0075] As an example, considering the physical limitations of industrial mobile robot scenarios, the preset speed search range is: , .

[0076] To ensure search accuracy capable of capturing pixel-level minute displacements, a velocity resolution step size is set. During the longest integration time span, by The maximum image projection displacement caused should be less than Each pixel (pixel precision), therefore ,in This indicates the preset nearest obstacle distance, set according to the minimum detection range (blind zone) of the LiDAR or the robot's physical radius (e.g., 0.5 meters). Indicates the camera's focal length; This represents the maximum sampling time difference, corresponding to the absolute value of the sensor sampling time difference with the largest amplitude (absolute value), i.e. ;

[0077] Take the maximum value that meets the conditions as the step size. On the two-dimensional velocity plane (assuming the mobile robot moves in a plane, corresponding to the XZ plane), use this step size to perform grid sampling on the lateral velocity and longitudinal velocity respectively, generating a series of discrete two-dimensional candidate velocity vectors, which constitute the candidate velocity grid.

[0078] It should be noted that the camera focal length refers to the focal length value in pixels in the camera intrinsic parameters (i.e., the physical focal length divided by the pixel size); in other embodiments of the present invention, the implementer can adjust the preset speed search range and pixel accuracy; or it can traverse the minimum value of the depth value (Z-axis coordinate) of all points in the current vertical edge point set in S1 in the camera coordinate system and statistically obtain the real-time value.

[0079] The system verifies the geometric consistency with the observed data by traversing the candidate velocity grid and performing a forward prediction based on the physical model for each velocity hypothesis. Specifically, it analyzes each set of vertical edge points one by one.

[0080] Within the current set of vertical edge points, select target body motion compensation points one by one. For each candidate velocity vector, considering that the obstacle has undergone relative displacement during the asynchronous sampling of the sensor, fuse the candidate velocity vector and the sensor sampling time difference, calculate the displacement increment within the corresponding time difference, and obtain the displacement compensation vector.

[0081] By fusing the displacement compensation vector and the spatial coordinates of the target body motion compensation points, and combining them with the camera's intrinsic parameter matrix, the target body motion compensation points are projected onto the image plane to obtain the extrapolated pixel coordinates. This allows the "virtual image" of the 3D point cloud under different velocity assumptions to be mapped to the 2D image domain, providing a pixel benchmark for position consistency comparison.

[0082] As an example, the transformation formula for pixel coordinates includes:

[0083] ;

[0084] in, This represents the coordinates of the r-th target body motion compensation point within the currently analyzed vertical edge point set; This represents the m-th candidate velocity vector of the current set of vertical edge points being analyzed. This represents the sensor sampling time difference at the r-th target body motion compensation point; This represents the displacement compensation vector of the r-th target body motion compensation point under the m-th candidate velocity vector; This represents the estimated position of the r-th target body motion compensation point under the m-th candidate velocity vector at the camera exposure time. Finally, the pixel coordinates are estimated by projecting the camera intrinsic parameter matrix onto the image plane.

[0085] Furthermore, in order to utilize the unique geometric shearing effect of the rolling shutter to distinguish between real motion and static texture, the hardware parameters must include at least the camera's focal length and line readout time constant. Based on the principle of perspective projection and the rolling shutter exposure mechanism, when an object has lateral relative motion, the projection position of its vertical edge on different lines of the image will be horizontally displaced due to the exposure time difference, thus forming a tilt.

[0086] Therefore, the product of the lateral velocity component, focal length, and line readout time constant corresponding to the candidate velocity vector is used as the numerator, and the absolute value of the depth coordinate of the body motion compensation point in the camera's coordinate system is used as the denominator. The expected tilt angle is obtained by performing an arctangent transformation on the ratio of the fractions.

[0087] Among them, the lateral velocity component corresponding to the selected velocity vector determines the horizontal displacement per unit time, the camera's focal length determines the scaling ratio of physical displacement to pixel displacement, the row readout time constant determines the time interval between adjacent pixel rows, and the numerator represents the lateral pixel offset caused by each additional row readout time. Due to the "nearer is larger and farther is smaller" characteristic of perspective projection, the same physical displacement produces a larger pixel offset at closer distances, so it needs to be divided by depth for normalization. The resulting fractional ratio corresponds to the tangent value of the expected tilt angle, so an arctangent transformation is finally performed.

[0088] Meanwhile, it is stipulated here that the absolute value of the depth coordinates of the body motion compensation points involved in the calculation must be greater than the preset minimum observation distance; if it is less than that, the minimum observation distance is taken to avoid the denominator being zero.

[0089] The expected tilt angle quantitatively describes the specific slope generated on the roller blind image by the lateral relative motion of the vertical edge, constituting a second geometric constraint independent of positional information.

[0090] It should be noted that the depth coordinates are the coordinates of the body motion compensation point on the Z-axis of the camera coordinate system. The camera coordinate system uses the common Z-axis to represent the optical axis direction (depth direction), the X-axis to represent the horizontal direction of the image, and the Y-axis to represent the vertical direction of the image.

[0091] The derived pixel coordinates and expected tilt angle provide a complete geometric description of each candidate velocity vector from two dimensions: spatial position consistency and geometric shape consistency. At this point, the spatial distance error of the derived pixel coordinates and the difference between the expected tilt angle and the edge tangent direction, based on the image edge feature analysis, can reflect the degree of consistency between the motion state described by the candidate velocity vector and the real single-frame observation data. Thus, the optimal velocity vector corresponding to the vertical edge point set is selected from the candidate velocity grid to obtain the instantaneous velocity estimate, providing accurate input for subsequent deterministic kinematic extrapolation and generation of spatiotemporally aligned scene point cloud data.

[0092] In a preferred embodiment of the present invention, please refer to Figure 2 The flowchart illustrates a method for obtaining an optimal velocity vector according to an embodiment of the present invention, specifically including:

[0093] Step S201: Extract the edge distance index map and edge angle map of the current frame image.

[0094] To avoid repeatedly performing time-consuming image convolution and edge search operations during the optimization process, the system pre-constructs a distance index map based on distance transformation and an angle index map based on gradient direction, transforming complex feature matching into a less complex table lookup operation.

[0095] As an example, the Canny edge detection operator or the Sobel operator is applied to the original grayscale image of the current frame to calculate the gradient distribution of the image. An edge intensity map is generated based on the gradient magnitude, and a binarized edge mask is extracted using a preset edge threshold.

[0096] The preset edge threshold can be an adaptive method (such as the Otsu algorithm) or a fixed threshold based on empirical values ​​to ensure that the main edges can be stably extracted under different lighting conditions.

[0097] Next, an Euclidean distance transformation is performed on the edge mask to generate an edge distance index map. In the edge distance index map, the value at any pixel position represents the Euclidean distance between that pixel and the nearest edge pixel, which is used to quickly calculate the position matching cost.

[0098] Finally, based on the horizontal component of the image gradient With vertical component The gradient direction of each edge pixel is calculated to generate an edge angle map. The edge angle map stores the numerical values. It characterizes the normal direction of the image edge, which is used for subsequent evaluation of the tilt consistency of the projection points.

[0099] Edge distance index map and edge angle map provide the basis for matching, characterize the edge features of the image, and ensure that the subsequent matching is indeed the edge of a real obstacle with corresponding kinematic deformation, rather than a static structure.

[0100] It should be noted that the Canny edge detection operator or Sobel operator, Otsu algorithm, and Euclidean distance transformation are all well-known techniques in the art and will not be elaborated further.

[0101] Step S202: For each body motion compensation point, query the edge distance index map and edge angle map based on the inferred pixel coordinates to obtain the actual edge distance and actual edge angle; based on the difference between the actual edge angle and the expected tilt angle, and combined with the actual edge distance, obtain the cost factor of the corresponding body motion compensation point.

[0102] Before finding the optimal velocity vector, for each body motion compensation point in the current vertical edge point set, the actual edge distance and actual edge angle are obtained by querying the edge distance index map and edge angle map based on the inferred pixel coordinates, and the actual data is used as a benchmark.

[0103] By comparing the actual edge angle with the expected tilt angle and combining the actual edge distance, the matching error of a single body motion compensation point can be quantified, the cost factor can be obtained, and the matching cost can be characterized.

[0104] As an example, the edge angle map typically stores the gradient direction (i.e., the normal direction of the edge, perpendicular to the edge direction), and the expected tilt angle is the tangent direction of the edge (i.e., the edge direction). In an ideal match, the tangent of the edge should be perpendicular to the normal (with an angle of 90 degrees). Therefore, the absolute value of the cosine of the difference between the actual edge angle and the expected tilt angle is used as the angle error term, representing the difference between the actual edge angle and the expected tilt angle. The value of the actual edge distance is used as the position error term, representing the spatial distance error of the inferred pixel coordinates. The smaller the value, the closer the inferred position is to the real edge in the image.

[0105] The sum of the product of the preset balance weight coefficient of the body motion compensation point and the angle error term, and the sum of the product of the angle error term and the position error term, is used as the cost factor of the corresponding body motion compensation point.

[0106] The preset balance weight coefficient is used to ensure that the position error and the angle error are on the same order of magnitude, preventing a single feature from dominating the optimization process. The value is 0.01 times the length of the image diagonal pixels (e.g., 10.0).

[0107] Step S203: Fuse the cost factors of all body motion compensation points under the same candidate velocity vector to obtain the matching cost value, and select the candidate velocity vector with the smallest matching cost value as the optimal velocity vector.

[0108] The current set of vertical edge points contains multiple body motion compensation points. To obtain the most suitable candidate velocity vector, the cost factors of all body motion compensation points under the same candidate velocity vector are fused to obtain the matching cost value, which represents the overall matching cost. Finally, the optimal velocity vector is selected.

[0109] As an example, within the current set of vertical edge points, the overall characteristics are represented by the average value. The average cost factor of all body motion compensation points under the same candidate velocity vector is used as the matching cost value, and the candidate velocity vector with the smallest matching cost value is selected as the optimal velocity vector.

[0110] It should be noted that the analysis process is the same for each set of vertical edge points. Here, only one example is described. Each set of vertical edge points corresponds to an optimal velocity vector, which provides a basis for eliminating spatiotemporal distortion in subsequent steps.

[0111] It should be noted that, to avoid false motion estimations due to noise interference, the system performs a double check on the optimal velocity vector: First, it checks whether the minimum matching cost is less than a preset matching confidence threshold (e.g., 3.0 pixels). If it is higher than the matching confidence threshold, it means that there is no valid image match in the region of the corresponding vertical edge point set, and the velocity is deemed unacceptable. At this time, the system triggers a safety fallback mechanism, resetting the optimal velocity vector of the corresponding vertical edge point set to zero and treating it as a static obstacle for subsequent processing.

[0112] Next, check whether the optimal velocity modulus is greater than the preset dead zone threshold (e.g., If the value is less than the dead zone threshold, the object corresponding to the vertical edge point set is forcibly determined to be in a stationary state, and a zero vector is output.

[0113] In other embodiments of the invention, the implementer may adjust the matching confidence threshold and the quiescent dead zone threshold as needed.

[0114] Step S3: Extrapolate the corresponding body motion compensation point to the preset prediction target time using the optimal velocity vector to generate spatiotemporally aligned scene point cloud data.

[0115] The robot control system needs to plan the future motion trajectory within the current decision-making cycle. The data collected by the sensors essentially reflects the environmental state at a past moment. The velocity vector obtained in step S2 provides a precise kinematic basis for bridging this time gap. Therefore, the corresponding body motion compensation point is extrapolated to the preset prediction target time using the optimal velocity vector to generate spatiotemporally aligned scene point cloud data. This extrapolates the discrete and lagging observation data to a unified future moment, reconstructing the rigid body geometry that is aligned in both time and space.

[0116] Preferably, in one embodiment of the present invention, in order to enable the perception results to directly serve real-time obstacle avoidance decision-making, the system first determines the time anchor point for data alignment. The start time of the next control cycle of the robot control system (determined by the actual scenario and not limited here) is obtained as the preset prediction target time. At this time, the preset prediction target time is later than the start exposure time and all laser acquisition times, ensuring that the output environmental model and the robot's execution actions are strictly synchronized on the time axis.

[0117] For each body motion compensation point, the time span from the original sampling time to the predicted target time is first calculated, and extrapolation is performed in the time dimension. Extrapolation refers to the process of predicting and extrapolating the spatial state of the target object at a specific time in the future (i.e., the spatial position of the body motion compensation point) and the motion law (i.e. the calculated optimal velocity vector) known at the current observation time using the rigid body kinematics model. This is equivalent to predictive analysis.

[0118] As an example, the formula for calculating time span includes: , Indicates the preset target time for prediction. Indicates the start exposure time of the current frame image; Represents the set of all vertical edge points, the first Sampling time offset of each body motion compensation point Indicates the first The absolute acquisition time of each body motion compensation point; Indicates the first The time span of each body motion compensation point.

[0119] Furthermore, based on the spatial coordinates of the body motion compensation point, the time span and the corresponding optimal velocity vector are fused to obtain the spatial coordinates of the corresponding body motion compensation point at the predicted target time.

[0120] As an example, for the first For each body motion compensation point, the product of the time span and the corresponding optimal velocity vector is used as the predicted displacement increment. The predicted displacement increment is then combined with the first... The coordinates obtained by summing the spatial coordinates of each body motion compensation point in the camera coordinate system are used as the spatial coordinates at the time of target prediction.

[0121] The spatial coordinates of all body motion compensation points at the predicted target time are used to construct scene point cloud data.

[0122] It should be noted that since obstacle avoidance usually focuses on the edges (contours), for other body motion compensation points (such as ground points or smooth surface points) that are not selected into the vertical edge point set, this embodiment assumes that they are in a stationary state, that is, the optimal velocity vector is a zero vector. Together with the vertical edge point set whose optimal velocity vector is a zero vector, their spatial coordinates after body motion compensation are directly retained when generating scene point cloud data.

[0123] Finally, the system packages all calculated spatial coordinates of the predicted target time and the corresponding optimal velocity vectors into a standard point cloud data format and directly inputs the data stream into the robot's local path planning module. Since the spatial coordinates of the input data are calibrated to the control time, the planning module can directly map the point cloud into a cost map without requiring additional motion compensation or probability inference. The corrected point cloud geometrically presents as a compact rigid body cluster, enabling the navigation system to plan paths at distances closer to the actual boundaries of obstacles, thus improving the robot's passage efficiency while ensuring safety.

[0124] An embodiment of the present invention also provides a robot scene perception system based on multi-sensor fusion. The system includes a memory, a processor, and a computer program. The memory is used to store the corresponding computer program, and the processor is used to run the corresponding computer program. When the computer program runs in the processor, it can implement the robot scene perception method based on multi-sensor fusion described in steps S1-S3.

[0125] In summary, to address the technical problem of distortion in existing robot point cloud data affecting the accuracy of path planning during navigation, this invention provides a robot scene perception method and system based on multi-sensor fusion. This invention first acquires observation data, using the initial exposure time of the current frame image as a reference. It then uses inertial data to remove robot motion displacement from LiDAR measurement points, generating motion compensation points and acquiring the sensor sampling time difference. Further, it filters vertical edge point sets and constructs a candidate velocity grid. For each candidate velocity vector, the motion compensation points are projected onto the image plane to obtain the extrapolated pixel coordinates, and the expected tilt angle is calculated. Further, based on image edge features, it analyzes the spatial distance error of the extrapolated pixel coordinates and the difference between the expected tilt angle and the edge tangent direction to select the optimal velocity vector. Finally, it uses the optimal velocity vector to extrapolate the corresponding motion compensation points to a preset prediction target time, generating spatiotemporally aligned scene point cloud data. This invention utilizes the sampling time difference between the laser and the rolling shutter camera to construct dual constraints of position projection and rolling shutter shear slope. It solves the instantaneous velocity of obstacles in a single frame and performs spatiotemporal extrapolation, eliminating dynamic distortion caused by asynchronous sampling and achieving zero-latency accurate scene perception.

[0126] It should be noted that the order of the above embodiments of the present invention is merely for descriptive purposes and does not represent the superiority or inferiority of the embodiments. The processes depicted in the accompanying drawings do not necessarily require a specific or sequential order to achieve the desired result. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0127] The various embodiments in this specification are described in a progressive manner. The same or similar parts between the various embodiments can be referred to each other. Each embodiment focuses on describing the differences from other embodiments.

Claims

1. A robot scene perception method based on multi-sensor fusion, characterized in that, The method comprises: Obtaining observation data of a mechanical rotating laser radar and a rolling shutter camera; taking the starting exposure time of a current frame image as a reference, removing robot body motion displacement in laser radar measurement points by using inertial data, generating body motion compensation points, and calculating the sensor sampling time difference between the laser acquisition time of each body motion compensation point and the exposure time of the corresponding pixel of the camera exposure; According to the spatial distribution of the body motion compensation points, a vertical edge point set is screened, a candidate velocity grid is constructed for each vertical edge point set, for each candidate velocity vector, the body motion compensation points are projected onto the image plane to obtain deduced pixel coordinates by combining the sensor sampling time difference, and the expected tilt angle is calculated based on the hardware parameters of the rolling shutter and the depth coordinates of the body motion compensation points; based on image edge features, the spatial distance error of the deduced pixel coordinates and the difference between the expected tilt angle and the edge tangent direction are analyzed, and the optimal velocity vector corresponding to the vertical edge point set is screened from the candidate velocity grid; The optimal velocity vector is used to extrapolate the corresponding body motion compensation points to a preset prediction target time to generate spatiotemporal aligned scene point cloud data; The method for obtaining the deduced pixel coordinates comprises: Selecting a target body motion compensation point in the vertical edge point set currently analyzed one by one, for each candidate velocity vector, obtaining a displacement compensation vector by fusing the candidate velocity vector and the sensor sampling time difference, and fusing the displacement compensation vector and the spatial coordinates of the target body motion compensation point, combining the intrinsic matrix of the camera, projecting the target body motion compensation point onto the image plane to obtain the deduced pixel coordinates; The method for obtaining the optimal velocity vector comprises: Extracting an edge distance index map and an edge angle map of the current frame image; applying an edge detection operator to the original gray image of the current frame image, generating an edge intensity map according to the gradient amplitude, and extracting a binary edge mask through a preset edge threshold; the edge mask performs Euclidean distance transformation to generate an edge distance index map; according to the horizontal component and the vertical component of the image gradient, the gradient direction of each edge pixel is calculated to generate an edge angle map; For each body motion compensation point, the actual edge distance and the actual edge angle are obtained by querying the deduced pixel coordinates in the edge distance index map and the edge angle map; according to the difference between the actual edge angle and the expected tilt angle, the cost factor corresponding to the body motion compensation point is obtained by combining the actual edge distance; The cost factors of all body motion compensation points under the same candidate velocity vector are fused to obtain a matching cost value, and the candidate velocity vector with the smallest matching cost value is selected as the optimal velocity vector. 2.The robot scene perception method based on multi-sensor fusion of claim 1, wherein, The method for obtaining the body motion compensation points comprises: The inertial data comprises the angular velocity and acceleration data of the laser radar collected by the inertial measurement unit; For each measurement point collected by the laser radar, a sampling time offset relative to the initial exposure time is obtained; based on the sampling time offset, angular velocity and acceleration data of the laser radar for a corresponding period are time-integrated to obtain a relative pose transformation matrix; based on original spatial coordinates of the measurement point, inverse transformation is performed using the relative pose transformation matrix and a rigid body transformation matrix of a coordinate system of the laser radar relative to a coordinate system of the camera to obtain a body motion compensation point. 3.The robot scene perception method based on multi-sensor fusion of claim 1, wherein, The method for obtaining the sensor sampling time difference comprises: For each body motion compensation point, an exposure time of a corresponding pixel point is obtained based on an intrinsic matrix of the camera and a row readout time constant; a sensor sampling time difference is obtained according to a difference between the laser collection time and the corresponding exposure time.

4. The robot scene perception method based on multi-sensor fusion according to claim 1, characterized in that, The method for obtaining the vertical edge point set comprises: In a preset spatial neighborhood of each body motion compensation point, a local normal vector is calculated using covariance matrix decomposition according to coordinates of points in a spatial neighborhood point set; a vertical edge point is selected from the body motion compensation point by comparing the local normal vector with a gravity direction vector, and a vertical edge point set is constructed.

5. The robot scene perception method based on multi-sensor fusion according to claim 1, characterized in that, The method for obtaining the expected tilt angle comprises: The hardware parameters at least include a focal length of the camera and a row readout time constant; a product of a transverse velocity component corresponding to the candidate velocity vector, the focal length and the row readout time constant is a numerator, an absolute value of a depth coordinate of the body motion compensation point in a coordinate system of the camera is a denominator, and an inverse tangent transformation of a fraction ratio is performed to obtain an expected tilt angle.

6. The robot scene perception method based on multi-sensor fusion according to claim 1, characterized in that, The method for obtaining the scene point cloud data comprises: The preset prediction target time is later than the initial exposure time and all the laser collection times; for each body motion compensation point, a time span from an original sampling time to the prediction target time is calculated; based on spatial coordinates of the body motion compensation point, the time span and a corresponding optimal velocity vector are fused to obtain spatial coordinates of the body motion compensation point at the prediction target time; Spatial coordinates of all the body motion compensation points at the prediction target time are counted to form scene point cloud data.

7. The robot scene perception method based on multi-sensor fusion according to claim 1, characterized in that, The method for obtaining the candidate velocity grid comprises: According to a preset nearest obstacle distance, a focal length of the camera and an absolute value of the sensor sampling time difference with the largest amplitude, a search velocity step is set, candidate velocity vectors are generated in a preset velocity search range, and a candidate velocity grid is constructed.

8. A multi-sensor fusion based robot scene perception system, the system comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, The processor executes the computer program to implement the steps of the robot scene perception method based on multi-sensor fusion according to any one of claims 1-7.

Images