An angle-based point cloud data rectification method

Abstract

The application discloses an angle-based point cloud data correction method, which comprises the following steps: A, sampling the motion distance measured by a code disc according to a preset sampling period, and then converting the motion distance sampled in each preset sampling period into a period sampling distance and a period sampling angle; B, accumulating the period sampling distance saved in each preset sampling period according to the same angle index value to obtain a corresponding point cloud distance correction value, and accumulating the period sampling angle saved in each preset sampling period according to the same angle index value to obtain a corresponding point cloud angle correction value; and C, whenever a point with a pose angle equal to the angle index value is traversed from a frame of point cloud, calculating a point cloud coordinate offset correction value based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value, and then correcting the coordinates of the traversed point by using the point cloud coordinate offset correction value.

Description

Technical Field

[0001] This application relates to the technical field of sensor data correction, and is particularly applicable to an angle-based point cloud data correction method. Background Technology

[0002] In rotating point cloud acquisition systems, such as the lidar of robots, which are specifically designed to collect environmental point cloud data for positioning and navigation, the acquired point cloud data often suffers from coordinate offsets due to mechanical vibration or motion errors. Traditional methods control the robot in a static state to calibrate the point cloud data, but they cannot adapt to dynamic error changes.

[0003] Chinese invention patent application number 202111453891.2 discloses an inertial navigation error elimination method based on point cloud feature points. To correct the accumulated error of a robot's inertial navigation system (INS), this method performs feature point matching on the collected point cloud during the robot's movement. This matching process requires establishing descriptors and employing the PNP (Perspective-N-Point) algorithm to obtain the pose transformation of the feature points. However, the pose transformation of the INS is subsequently compared with the pose transformation of the 3D-TOF point cloud feature points, and the resulting deviation is then converted... The error is shifted to the inertial navigation system, and the error is dynamically corrected. However, the original point cloud data collected by the 3D-TOF sensor is not directly corrected. As a result, it is impossible to track and locate the positional relationship between obstacles and the robot in the original point cloud in real time. If the obstacle is detected using the real-time collected point cloud data, the robot may only accurately detect the obstacle after it has already touched it. This process consumes a lot of time and computing power to process the real-time collected point cloud data, resulting in collision behaviors such as the robot continuously hitting the obstacle or the robot being in contact with the obstacle for a long time, which brings unknown dangers to the robot. Summary of the Invention

[0004] This application aims to provide an angle-based point cloud data correction method, the specific technical solution of which is as follows:

[0005] A point cloud data correction method based on angles is disclosed. The main body executing the point cloud data correction method is a robot equipped with an encoder and a laser sensor. After the robot acquires a frame of point cloud data using the laser sensor, the robot walks towards the location distributed in the frame of point cloud data and measures the movement distance using the encoder during the movement. The point cloud data correction method is executed during the robot's movement towards the location distributed in the frame of point cloud data. The point cloud data correction method includes: Step A, sampling the movement distance measured by the encoder according to a preset sampling period, then converting the movement distance sampled in each preset sampling period into a periodic sampling distance and a periodic sampling angle, and storing the periodic sampling distance and periodic sampling angle in the target memory space for each preset sampling period; then executing Step B; Step B, setting the angle index traversal range corresponding to the robot within the preset sampling period according to the ratio of the preset sampling period to the unit angle sampling time; then within the corresponding angle index traversal range, adjusting the point cloud data according to the same angle index value. The periodic sampling distances stored in each preset sampling period are accumulated until all periodic sampling distances in the target memory space are accumulated, resulting in a point cloud distance correction value corresponding to the same angle index value; and the periodic sampling angles stored in each preset sampling period are accumulated according to the same angle index value until all periodic sampling angles in the target memory space are accumulated, resulting in a point cloud angle correction value corresponding to the same angle index value; then step C is executed; wherein, one preset sampling period corresponds to one angle index traversal range, and one angle index traversal range can be divided into multiple angle index values; step C: traverse the pose angle of the point cloud in a frame, and whenever a point in the point cloud in a frame has a pose angle equal to the angle index value, calculate the point cloud coordinate offset correction value based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value, and then use the point cloud coordinate offset correction value to correct the coordinates of the traversed points, so as to correct the coordinates of each point in the point cloud in a frame.

[0006] In summary, this application calculates the point cloud distance correction value and point cloud angle correction value accumulated over multiple preset sampling periods for points with corresponding angle index values ​​in a frame of point cloud by executing steps A to C. Then, based on the point cloud distance correction value and point cloud angle correction value corresponding to the angle index value, the coordinates of the points with corresponding matching angle index values ​​in the frame of point cloud are corrected. This ultimately achieves motion correction of the frame of point cloud, which is equivalent to the robot compensating for the point cloud by accumulating the point cloud distance correction value and point cloud angle correction value within the motion time (greater than or equal to the multiple preset sampling periods) spent by the robot moving towards the obstacle distributed in the frame of point cloud. This helps predict the relative motion position of the frame of point cloud within the same time period, thereby providing the robot with a possible collision area with the obstacle. This makes the collision detection real-time, preventing the robot from continuously hitting the obstacle before detecting a collision. Attached Figure Description

[0007] Figure 1 This is a schematic flowchart of an angle-based point cloud data correction method disclosed in one embodiment of this application.

[0008] Figure 2 This is a schematic flowchart of the method for calculating the point cloud distance correction value and the point cloud angle correction value in step B of another embodiment of this application.

[0009] Figure 3 This is a flowchart illustrating the method for correcting the coordinates of the traversed points in step C, which is disclosed in another embodiment of this application. Detailed Implementation

[0010] The technical solutions of the embodiments of this application will be described in detail below with reference to the accompanying drawings. To further illustrate the embodiments, this application provides accompanying drawings. These drawings are part of the disclosure of this application and are mainly used to illustrate the embodiments, and can be used in conjunction with the relevant descriptions in the specification to explain the operating principles of the embodiments. With reference to these drawings, those skilled in the art should be able to understand other possible implementation methods and the advantages of this application.

[0011] In scenarios where robots collide with obstacles during their movement, the robot first collects point cloud data from the obstacle at its starting point. Then, it begins moving towards the obstacle from that starting point, with the physical location corresponding to the first collected point cloud data gradually moving closer to the robot. Current technology allows the robot to use real-time collected laser point clouds to detect the minimum distance between itself and the obstacle, calculating collision markers and angles. However, due to the large scale of the batch point cloud data collected during movement, the computational load from feature point matching and coordinate system transformation operations is substantial. This results in a lack of real-time tracking and localization of the same obstacle. Consequently, the robot only accurately detects the obstacle upon contact, failing to determine the obstacle's pose in advance. This leads to a poorer response to obstacle collisions, posing unforeseen dangers to the robot.

[0012] To predict the positional relationship of the same obstacle relative to the robot using the first collected point cloud without waiting for real-time point cloud acquisition, this application discloses an angle-based point cloud data correction method. The main body executing the point cloud data correction method is a robot equipped with an encoder and a laser sensor. Schematically, the robot includes the following structure: a robot body capable of autonomous movement with a left drive wheel and a right drive wheel; a laser sensor installed on the top of the robot body; the laser sensor can be a lidar to scan a circle of point cloud; and encoders on the left and right drive wheels for detecting the travel distance of the drive wheels. The encoders can measure the robot's movement distance, including the straight-line distance traveled by the robot relative to its original starting position.

[0013] To facilitate the explanation of the environmental characteristics of the point cloud data correction method, this application uses a scenario where a robot is about to collide with an obstacle during its movement as an illustration. The robot starts walking from its original starting position. At the original starting position, the robot uses a laser sensor to scan for obstacles at the target location in its direction of movement (which can be considered as the front of the robot's head) and collects a frame of point cloud data. That is, the robot uses a laser sensor to collect a frame of point cloud data from the obstacle, which serves as the first frame of point cloud data. Then, the robot walks towards the location distributed in the first frame of point cloud data and uses an encoder to measure the distance traveled during the movement. That is, the robot walks towards the target location, and the encoder continues to perform measurement work. The location distributed in the first frame of point cloud data can form an area, including the target location. Preferably, the target location can be the location of the first point collected in the first frame of point cloud data. The robot can plan an effective path from the original starting position to the target location using a heuristic search algorithm, and then walk along the effective path to shorten the distance between the robot and the obstacle. Of course, when the target location is selected as the location of other points in the first frame of point cloud data, an effective path to the target location can also be planned. Therefore, the walking direction of the robot is not fixed during the process of walking towards the location distributed in the first frame of point cloud data.

[0014] In order to correct the coordinates of the point cloud frame by combining the motion distance measured by the encoder, the point cloud data correction method disclosed in this application is executed during the process of the robot walking towards the location where the point cloud frame is distributed. That is, the point cloud data correction method is executed during the process of the robot walking towards the location where the point cloud frame is distributed, so as to perform motion compensation on the point cloud frame and predict risks in advance.

[0015] As the robot closes the distance to the obstacle, a segment of the movement distance within each sampling period can be measured sequentially using an encoder. This distance can be accumulated to represent the total movement distance the robot has traveled from its original starting position. The original starting position is the starting point where the robot moves towards the obstacle, and it is also the location where the first frame of point cloud data from the obstacle is acquired. The first frame of point cloud data is considered as a circle of point cloud data scanned by the robot at the original starting position. It can be regarded as acquiring one frame of point cloud data, which then serves as the original point cloud data that needs to be corrected during the robot's movement towards the obstacle. This allows the robot to determine the relative positional characteristics of the obstacle in advance using the corrected point cloud data before it touches the obstacle.

[0016] like Figure 1 As shown, the point cloud data correction method includes:

[0017] Step A: The robot samples the motion distance measured by the encoder according to the preset sampling period, and then converts the motion distance sampled in each preset sampling period into periodic sampling distance and periodic sampling angle, and saves the periodic sampling distance and periodic sampling angle in the target memory space for each preset sampling period; then proceed to step B.

[0018] It should be noted that the point cloud sampled by the LiDAR or 3D scanning equipment mounted on the robot can be considered as a series of discrete points arranged in a 360-degree circle. These points are sampled sequentially. As the robot moves, these points undergo relative motion with respect to the robot, shifting a certain distance or rotating an angle relative to the original location area occupied by the point cloud (or the original point cloud). The robot can use data sensed by inertial sensors to describe the relative translation and rotation of the first point cloud sampled.

[0019] In step A, the robot subtracts the distance values ​​sampled by the encoder at the start and end times of the same preset sampling period to obtain the motion distance measured within the same preset sampling period. This motion distance is considered as the distance traveled by the robot within the same preset sampling period. If the motion distance is divided according to the left and right drive wheels, it is divided into left drive wheel motion distance and right drive motion distance. These are recorded as the left drive wheel motion distance and right drive motion distance sampled in each preset sampling period. Based on this, the left drive wheel motion distance and right drive motion distance are converted into periodic sampling distance and periodic sampling angle according to kinematic formulas. Thus, a periodic sampling distance and a periodic sampling angle are calculated in each preset sampling period. The periodic sampling distance represents the straight-line distance actually traveled by the robot within a preset sampling period, and the periodic sampling angle represents the angle actually turned by the robot within a preset sampling period.

[0020] To obtain reliable motion compensation by accumulating a sufficient number of periodic sampling distances and angles, step A saves the periodic sampling distances and angles to the target memory space for each preset sampling period. Therefore, the target memory space needs to be configured with a sufficiently large capacity to match the number of preset sampling periods required by the encoder. If the number of preset sampling periods is a preset target number, the target memory space can hold a maximum of a preset target number of periodic sampling distances and angles, and these can be accessed via index values ​​within the target memory space.

[0021] Since the encoder is considered a reliable measurement result for measuring a small distance the drive wheel has rotated within a short period of time, the preset target number of times is determined by the measurement accuracy of the encoder. The distance traveled by the robot's left or right drive wheel within the preset target number of preset sampling periods can be accurately measured by the encoder, eliminating the interference of slippage error.

[0022] Step B: Set the angle index traversal range for the robot within the preset sampling period based on the ratio of the preset sampling period to the unit angle sampling time. Step B uses the angle as the index value for calling / traversing the aforementioned periodic sampling distance, the aforementioned reference, and the point cloud of a frame. Specifically, one preset sampling period corresponds to one angle index traversal range, and one angle index traversal range is divided into multiple index values, ensuring that each index value indexes the allowed calculated correction value (including distance and angle) within one preset sampling period, matching it to each point in the point cloud corresponding to the index.

[0023] As the sampling time of the encoder increases, the preset sampling period experienced by the sampling movement distance increases, and the upper limit of the corresponding angle index traversal range increases. The corresponding angle index traversal range will expand, so it is necessary to convert it to a circular index within 0 to 359. Correspondingly, the corresponding angle index traversal range is converted into 360 angle index values ​​between 0 and 359 or between 1 and 360. Therefore, an angle index traversal range can be divided into multiple angle index values. Correspondingly, the point cloud of a frame is converted into a point cloud with a resolution of 1 degree, thereby controlling the angle index traversal range to be orderly divided into 360 angle index values ​​within a preset sampling period, so as to divide the point cloud of a frame into 360 groups of point clouds.

[0024] Then, within the corresponding angle index traversal range, the latest saved periodic sampling distance within each preset sampling period is accumulated according to the same angle index value until all periodic sampling distances within the target memory space are accumulated, obtaining the point cloud distance correction value corresponding to the same angle index value. At this time, the accumulated point cloud distance correction value is the result of accumulating the periodic sampling distances within the last preset sampling period of the encoder operation; and then, the latest saved periodic sampling angle within each preset sampling period is accumulated according to the same angle index value until all periodic sampling angles within the target memory space are accumulated, obtaining the point cloud angle correction value corresponding to the same angle index value. At this time, the accumulated point cloud angle correction value is the result of accumulating the periodic sampling angles within the last preset sampling period of the encoder operation; then, step C is executed. Therefore, based on the motion distance sampled by the encoder and the periodic sampling distance stored in the target memory space in each preset sampling period, the newly stored periodic sampling distance is added to each preset sampling period, which is equivalent to updating the motion distance compensation amount according to the preset sampling period. Similarly, based on the motion angle sampled by the encoder and the periodic sampling angle stored in the target memory space in each preset sampling period, the newly stored periodic sampling angle is added to each preset sampling period, which is equivalent to updating the motion angle compensation amount according to the preset sampling period. Thus, within the corresponding angle index traversal range, the total distance the robot has moved and the total angle the robot has turned are superimposed on the point cloud corresponding to the currently traversed angle index value.

[0025] In step B, each angle index value can be mapped one-to-one with the periodic sampling distance and periodic sampling angle within the same preset sampling period. Simultaneously, within the corresponding angle index traversal range of each preset sampling period, specifically, each point in a frame of point cloud is assigned an integer angle value (between 0 and 359 degrees), and a one-to-one index relationship is established with the correction values ​​(point cloud distance correction value and point cloud angle correction value) calculated in the last preset sampling period. This accelerates the point cloud data correction method's correction of point clouds with corresponding angle index values ​​according to the preset sampling period. The larger the motion deviation (corresponding correction value) required for the correction / compensation of the first set of point clouds acquired, the faster the correction method can perform point cloud corrections according to the preset sampling period.

[0026] Based on this, this application dynamically superimposes the periodic sampling distance according to a preset sampling period to obtain the point cloud distance correction value, and dynamically superimposes the periodic sampling angle according to a preset sampling period to obtain the point cloud angle correction value. Thus, by using the periodic sampling distance and periodic sampling angle calculated in each preset sampling period to accumulate and calculate the allowable movement distance (point cloud distance correction value) and movement angle (point cloud angle correction value) of the corresponding point cloud correction, each point in the point cloud can be indexed to obtain the corresponding point cloud distance correction value and point cloud angle correction value to compensate for the offset caused by the robot's movement relative to the obstacle, thereby improving the real-time correction accuracy. The robot can then predict the real-time position of the obstacle and avoid obstacle collisions in advance. Moreover, the straight line formed by the corrected points is straighter, which to a certain extent avoids the distortion of the laser detection straight wall.

[0027] Step C: Traverse the pose angles of the point cloud frame. At this time, the robot only traverses the point cloud data of the laser sensor collected at the original starting position. The pose angles in the point cloud data frame are pre-added. Whenever a point in the point cloud frame is traversed to a position angle equal to the angle index value, a point cloud coordinate offset correction value is calculated based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value. This application uses the point cloud distance correction value and the point cloud angle correction value as physical quantities to calculate the point cloud coordinate offset correction value, following trigonometric geometric relationships, including but not limited to substituting the point cloud distance correction value and the point cloud angle correction value into trigonometric function relationships to calculate the point cloud coordinate offset correction value on the corresponding coordinate axis. One point cloud coordinate offset correction value corresponds to one angle index value. This process is repeated for all points with a position angle equal to the angle index value. The point cloud distance correction value and the point cloud angle correction value corresponding to all angle index values ​​traversed in the point cloud frame that are equal to the position angle are sequentially converted into point cloud coordinate offset correction values ​​corresponding to each angle index value that is equal to the position angle, thus constituting all motion parameters / motion offsets allowed for the correction of the point cloud frame.

[0028] In step C, the point cloud distance correction value and point cloud angle correction value corresponding to the same angle index value are both calculated in step B and indexed in step C by traversing the pose angle of the point cloud. At this time, the pose angle of the point cloud is matched one-to-one with the point cloud distance correction value and the point cloud angle correction value. Preferably, the time for a frame of point cloud to be traversed and indexed in step C, as well as the time for indexing the point cloud distance correction value and the point cloud angle correction value and calculating the point cloud coordinate offset correction value, are shortened, thereby speeding up the calculation of the point cloud coordinate offset correction value required for each point.

[0029] Then, the coordinates of the traversed points are corrected using point cloud coordinate offset correction values ​​to correct the coordinates of all points in a frame of point cloud. The corrected coordinates of the frame of point cloud are then configured as the latest relative position between the obstacle at the target location and the robot's current position, enabling the prediction of the actual distribution position of the first frame of point cloud traversed by the robot using point cloud distance correction values ​​and point cloud angle correction values. Therefore, whenever a point in a frame of point cloud is traversed to a pose angle equal to the angle index value, it is considered a point with a matching angle index value, and also a point requiring correction (indexed using the pose angle or the same value of the angle index value). Its coordinates are then corrected using a correspondingly calculated point cloud coordinate offset correction value.

[0030] Step C corrects the coordinates of the traversed points using point cloud coordinate offset correction values. The coordinates of points corresponding to the angle index values ​​equal to the pose angles traversed in each step C are used as the correction subject. By using point cloud coordinate offset correction values ​​to offset the correction subject, the distance between the correction subject and the robot's current position is reduced. This compensates for the motion offset caused by the robot's relative movement in a given point cloud frame. Thus, the corrected point coordinates predict the most likely physical location of the point cloud frame after multiple preset sampling periods before the robot actually collides with obstacles distributed at that point cloud location. As the robot moves towards the location of the point cloud frame, the point cloud frame gets closer to the robot's body, and the result of correction using point cloud coordinate offset correction values ​​is closer to the actual situation.

[0031] By executing steps A to C, point cloud distance correction values ​​and point cloud angle correction values ​​accumulated over multiple preset sampling periods are calculated for points in the point cloud corresponding to angle index values. Then, based on the point cloud distance correction values ​​and point cloud angle correction values ​​corresponding to the angle index values, the coordinates of points in the point cloud corresponding to the matching angle index values ​​are corrected. This ultimately achieves motion correction for the point cloud, which is equivalent to the robot compensating for the point cloud by accumulating point cloud distance correction values ​​and point cloud angle correction values ​​within the motion time (greater than or equal to the multiple preset sampling periods) spent by the robot moving towards the obstacles distributed in the point cloud. This helps predict the relative motion position of the point cloud in the same time period, thereby providing the robot with a possible collision area with the obstacle. This makes the collision detection real-time, preventing the robot from continuously hitting the obstacle before detecting a collision.

[0032] In step B, the method for setting the angle index traversal range of the robot within the preset sampling period based on the ratio of the preset sampling period to the unit angle sampling time includes:

[0033] The ratio of the preset sampling period to the unit angle sampling time is calculated to obtain the predicted angle value of a single preset sampling period. The span of the angle index value that the robot is allowed to traverse within the current preset sampling period is planned. It should be noted that the robot has already scanned the point cloud frame before executing step A, and then enters the first preset sampling period. The angle of the point cloud that the laser sensor can scan within the first preset sampling period is predicted by calculating the ratio of the preset sampling period to the unit angle sampling time. The predicted angle value of a single preset sampling period is then used in step B. If the point cloud frame is counted with a resolution of 1 degree, the predicted angle value of a single preset sampling period is counted as the number of point cloud samples within a single preset sampling period.

[0034] The preset sampling period and unit angle sampling time are both preset time values. The unit angle sampling time is the time it takes for the robot to collect a point cloud of 1 degree using a laser sensor, denoted as per_angle_times, in milliseconds. A frame of point cloud is the point cloud collected by the robot during a complete scan using the laser sensor. If the frame of point cloud is counted at a resolution of 1 degree, points between 0 and 359 degrees or between 1 and 360 degrees are collected to form a point cloud distributed within a 360-degree range. The number of points in a frame of point cloud is generally greater than or equal to 360. The number of points in each degree of a frame of point cloud is not necessarily fixed. The more points in each degree, the greater the point cloud coordinate offset correction value required for point coordinate compensation, and correspondingly, the greater the required accumulated periodic sampling distance and periodic sampling angle. The number of points in a frame of point cloud is denoted as cloudPointRawCount. The sampling time of a frame of point cloud is considered the time taken for the laser sensor to scan the surrounding environment in one complete scan, denoted as cloudPointDuration, in microseconds. Based on this, the unit angle sampling time per_angle_times equals...

[0035] The starting angle index value is added to the predicted angle value of a single preset sampling period scan to obtain the ending angle index value. This ending angle index value is calculated based on the predicted angle value of a single preset sampling period scan, thus pre-defining the ending index value of the point cloud within a preset sampling period. When using the starting angle index value in the first preset sampling period, it can be configured as the angle index value of the first point in a frame of point cloud acquired by the laser sensor within a preset sampling period, preferably 0 degrees. By adding the starting angle index value to the predicted angle value of a single preset sampling period scan, the point cloud is traversed starting from 0 degrees. Within the current preset sampling period, the periodic sampling distance and periodic sampling angle described in step B are also traversed starting from 0 degrees until the ending angle index value is reached. Then, the ending angle index value can be updated to the first angle index value of the point cloud traversed in the next preset sampling period (360 degrees), i.e., updated to the starting angle index value of the next preset sampling period. This makes the predicted angle value of a single preset sampling period scan the offset between the starting angle index values ​​of two adjacent preset sampling periods.

[0036] Then, the target upper limit index value is calculated based on the end angle index value, and then the target upper limit index value is configured as the upper limit value of the angle index traversal range. The target upper limit index value is used to represent the maximum number of point clouds that the laser sensor is allowed to collect in a single preset sampling period. Since the end angle index value is an index value calculated based on the predicted angle value scanned in a single preset sampling period, the corresponding calculated target upper limit index value can predict the maximum number of point clouds that can be corrected in a single preset sampling period. The maximum number of point clouds that can be corrected in a single preset sampling period is less than or equal to the number of point clouds that the laser sensor can collect in the same preset sampling period. At this time, in step B, the robot does not use the laser sensor to scan the point cloud of the surrounding environment, but calculates the target upper limit index value as the predicted value of the number of point clouds based on the end angle index value.

[0037] Specifically, the method for calculating the target upper limit index value based on the end angle index value includes: detecting the size relationship between the point cloud angle start index value and the end angle index value to determine whether the point cloud angle start index value or the end angle index value crosses the 360-degree angle range.

[0038] When the starting index value of the point cloud angle is detected to be less than the ending index value, the starting index value of the point cloud angle is subtracted from the ending index value to obtain the target upper limit index value, which is greater than 0. This allows the calculation of the maximum number of point clouds that can be collected within a single preset sampling period, and the traversal range of the angle index is limited by 0 and the target upper limit index value. The starting index value and the ending index value of the point cloud angle can be set within the same preset sampling period.

[0039] When the starting index value of the point cloud angle is detected to be greater than or equal to the ending index value, the starting index value of the point cloud angle is subtracted from 360, and then the difference obtained by subtraction is added to the ending index value to obtain the target upper limit index value. Schematic, the starting index value of the point cloud angle is set to 120 within the current preset sampling period. If the traversal reaches the ending angle index value of 119 within the current preset sampling period, then the starting index value of the point cloud angle is greater than the ending angle index value. At this time, the number of points traversed within the current preset sampling period has reached 360, and the maximum angle index value of 359 has also been crossed during the traversal. In order to perform index analysis on the point cloud distributed between the angle of the last point traversed within the current preset sampling period (a point indexed by the ending angle index value) and the starting index value of the point cloud angle within the positive integer angle range, this application controls 360 minus 120 and adds 119 to obtain 359 when the starting index value of the point cloud angle is greater than the ending angle index value of 119. Then, 359 is used as the target upper limit index value; thereby setting the angle index traversal range to 0 to 359.

[0040] In summary, this application calculates the target upper limit index value based on the relationship between the starting and ending angle index values ​​of the point cloud and configures it as the upper limit value of the angle index traversal range. Thus, for point cloud scanning crossing the 0-degree / 360-degree boundary, it utilizes the cyclic characteristics of the point cloud in the 360-degree scan to calculate the target upper limit index value, keeping the upper limit value of the angle index traversal range positive and avoiding negative values ​​caused by the cyclic boundary, thereby ensuring the continuity and correctness of the angle index value calculation.

[0041] Furthermore, the aforementioned point cloud angle starting index value is used to represent the angle index value of the first point in the point cloud frame that is acquired by the laser sensor, and is configured to be equal to the starting angle index value, forming the angle index value of the first point that needs to be corrected in the first preset sampling period of the point cloud frame.

[0042] If indexing is performed within the current preset sampling period, the starting angle index value of the point cloud can represent the angle index value of the first point in the point cloud that needs to be corrected within the current preset sampling period; correspondingly, the ending angle index value is used to represent the starting angle index value within the next preset sampling period.

[0043] In the physical environment, the closer a point is to the location indexed by the starting point cloud angle value, the more the point indexed by the starting point cloud angle value receives the cumulative operation of the periodic sampling distance and periodic sampling angle in the preset sampling period. The larger the point cloud distance correction value and point cloud angle correction value required for correction, the more motion deviation is accumulated for the first sampled point as the preset sampling period increases. When the robot approaches the distribution location of the point cloud frame at a certain speed, the more correction value is compensated for for the point indexed by the starting point cloud angle value, and the closer the corrected coordinates of the point cloud frame are to the robot. The accuracy of the corrected coordinates of the point cloud frame is improved, and the possibility of collision with obstacles at the distribution location of the point cloud frame is predicted in advance, thus improving the real-time performance of collision behavior detection.

[0044] As one embodiment, the iterative calculation logic of accumulating the periodic sampling distances stored in each preset sampling period according to the same angle index value and accumulating the periodic sampling angles stored in each preset sampling period according to the same angle index value is described in detail in step B, as follows: Figure 2 As shown, after acquiring the point cloud frame and saving the periodic sampling distance and periodic sampling angle calculated for the current preset sampling period to the target memory space in step A, step B specifically includes:

[0045] Step B1: Starting from the first address of the target memory space, traverse the periodic sampling distance and periodic sampling angle stored in the target memory space in Step A. Specifically, when Step B1 is executed for the first time, the pre-set traversal pointer points to the object at the first address in the target memory space to traverse the periodic sampling distance and periodic sampling angle corresponding to the first preset sampling period stored in the target memory space. Here, one preset sampling period corresponds to one periodic sampling distance and one periodic sampling angle. Step A controls the periodic sampling distance and periodic sampling angle to be stored starting from the end address of the target memory space. It is then determined whether the traversal has reached the end address of the target memory space. In the case of storing the periodic sampling distance and periodic sampling angle from the end of the target memory space for each preset sampling period, it is essentially determining whether the periodic sampling distance and periodic sampling angle corresponding to the last preset sampling period stored in the target memory space have been traversed. If yes, proceed to Step B8; otherwise, proceed to Step B2. Therefore, the closer the address currently being traversed in step B1 is to the end address of the target memory space, the newer the periodic sampling distance and periodic sampling angle will be, that is, the more likely it is to traverse the periodic sampling distance and periodic sampling angle stored in the most recent preset sampling period.

[0046] Step B2: Calculate the target upper limit index value by executing the method described above, which sets the robot's angle index traversal range within the preset sampling period based on the ratio of the preset sampling period to the unit angle sampling time. Simultaneously, set the current index value to 0 within the current preset sampling period, and then execute step B3. For the current preset sampling period, during step B2's process of setting the robot's angle index traversal range within the current preset sampling period based on the ratio of the preset sampling period to the unit angle sampling time, the upper limit value of the robot's angle index traversal range within the current preset sampling period is set as the target upper limit index value. This target upper limit index value belongs to the current preset sampling period, and one preset sampling period corresponds to one angle index traversal range. In addition, in order to accumulate the periodic sampling distance and periodic sampling angle according to various angle index values, and to index out the distance periodic correction value and angle periodic correction value corresponding to each angle index value, the current index value is set to increment from 0, so that it traverses from 0 to the corresponding target upper limit index value in each preset sampling period. Therefore, the current index value is defined as the index value for traversing each integer angle covered by the angle index traversal range, so as to count the point cloud corresponding to each angle index value that needs to be corrected in the current preset sampling period, and to realize the distance periodic correction value and angle periodic correction value corresponding to each angle index value that needs to be accumulated in the current preset sampling period.

[0047] Step B3: Determine whether the current index value mentioned in step B2 is less than the target upper limit index value mentioned in step B2. If yes, proceed to step B4; otherwise, proceed to step B7. Within the current preset sampling period, determine whether the current index value is less than the target upper limit index value. Here, the target upper limit index value represents the number of points whose corresponding angle index value (or corresponding pose angle) needs to be corrected after determining the periodic sampling distance and periodic sampling angle corresponding to the current preset sampling period. This is counted as the maximum number of point clouds that the laser sensor is allowed to collect within the current preset sampling period. Preferably, the pose angle of the point cloud that needs to be corrected covers 0 to 359 from 0 to the target upper limit index value (the angle index traversal range corresponding to the current preset sampling period).

[0048] Based on the aforementioned embodiments, starting from the point cloud angle starting index value, the distance period correction value and angle period correction value corresponding to each of the target upper limit index values ​​need to be used for accumulation. This is equivalent to controlling all the distance period correction values ​​and angle period correction values ​​corresponding to the first index value (the current index value with a value of 0) in the first preset sampling period and the first index value (the current index value with a value of 0) in the next preset sampling period to participate in the accumulation.

[0049] Step B4: Determine that the current index value mentioned in Step B2 is within the angle index traversal range. Then, control the sum of the current index value mentioned in Step B2 and the starting point cloud angle index value to be modulo 360 (i.e., perform a modulo 360 operation) to generate an angle index value. This angle index value is then identified as the pose angle of a point required to accumulate the point cloud distance correction value and / or the point cloud angle correction value. Finally, execute Step B5. This divides the point cloud frame according to angles between 0 and 360 degrees, and uses the angle index value to limit the cyclic boundary of the 360-degree scan.

[0050] In step B4, the sum of the current index value and the starting index value of the point cloud angle, as described in step B2, is modulo 360. The generated angle index value ensures that the subsequent indexing of the distance period correction value and the angle period correction value cyclically cycles within the angle range of 0 to 359. Although the traversal range of the angle index can be between 0 and 359 or between 1 and 360, or it can expand beyond 360 degrees as the target upper limit index value increases, the angle offset range of the point cloud that needs to be corrected is defined as being between 0 and 359 based on the generated angle index value. This can be understood as the angle index value changing from the starting index value of the point cloud angle to the ending angle index value in the incremental change of the current index value. If the angle index value exceeds 359, the angle index value changes from 0 to the correction value corresponding to the ending angle index value, satisfying the circular index.

[0051] It should be noted that this application sets a starting index value for the point cloud angle in step B, which is applicable to a circular index within a 360-degree range. Within a single preset sampling period, the starting index value can be set to 1 degree, 2 degrees, or a larger integer angle, or it can be equal to the starting angle index value. It is important to note that if the ending angle index value obtained by adding the starting angle index value to the predicted angle value scanned in a single preset sampling period is greater than 359 degrees (i.e., greater than the starting index value), then the ending angle index value cannot be used to index a frame of point cloud (the actually acquired point cloud, not the predicted acquired point cloud) whose pose angle (angle index value) range is between 0 and 359. Therefore, this application needs to calculate a target upper limit index value based on the ending angle index value. Then, when the current index value is less than the target upper limit index value, the sum of the current index value and the starting index value of the point cloud angle is modulo 360 to generate an angle index value for circular indexing between 0 and 359.

[0052] In summary, this method ensures that all point clouds corresponding to index values ​​from 0 to the target upper limit (the angle index traversal range corresponding to the current preset sampling period) fall within the point cloud angle range that needs correction. This allows the target upper limit index value to also define the effective angle coverage range within the current preset sampling period. Specifically, when the angle is smaller than the target upper limit index value, the current index value is combined with the current index value to generate the angle index value. This ensures that all point clouds corresponding to index values ​​traversed within the current preset sampling period can be converted into the corresponding angle index values, avoiding distortion or error accumulation caused by discontinuous scanning.

[0053] Step B5: Add the periodic sampling distance stored in the target memory space within the current preset sampling period to the pre-set distance period correction value corresponding to the same angle index value to obtain the distance period correction value corresponding to the same angle index value. Update the distance period correction value corresponding to the same angle index value to the pre-set distance period correction value corresponding to the same angle index value. This is done so that the periodic sampling distance stored in the next preset sampling period is accumulated when step B5 is executed next time. In addition, when the same angle index value is updated, step B5 is also executed to add the corresponding distance period correction value to the periodic sampling distance stored in the current preset sampling period. This achieves the accumulation of the periodic sampling distance stored in each preset sampling period according to the same angle index value within the corresponding angle index traversal range, and continuously updates the distance period correction value.

[0054] Furthermore, the periodic sampling angles stored in the target memory space within the current preset sampling period are added to the pre-set angle period correction value corresponding to the same angle index value to obtain the angle period correction value corresponding to the same angle index value. This angle period correction value is then updated to the pre-set angle period correction value corresponding to the same angle index value. This is used to accumulate the periodic sampling angles stored in the next preset sampling period when step B5 is executed next time. Additionally, when the same angle index value is updated, step B5 is also executed to add the corresponding angle period correction value to the periodic sampling angles stored in the current preset sampling period. Then, step B6 is executed. This process accumulates the periodic sampling angles stored in each preset sampling period according to the same angle index value within the corresponding angle index traversal range and continuously updates the angle period correction value.

[0055] Preferably, when step B5 is executed for the first time, the distance cycle correction value corresponding to the same angle index value is 0; the angle cycle correction value corresponding to the same angle index value is 0. The distance cycle correction value corresponding to the same angle index value can be stored as an array structure in the target memory space to store the accumulated value of the distance cycle correction value corresponding to each angle index value; the angle cycle correction value corresponding to the same angle index value can be stored as an array structure in the target memory space to store the accumulated value of the angle cycle correction value corresponding to each angle index value; thus, the accumulated motion parameter correction amount corresponding to each angle (0 to 359) is stored through the distance cycle correction value and the angle cycle correction value corresponding to the same angle index value.

[0056] Step B6: Control the current index value mentioned in step B2 to automatically increment by one, then update the automatically incremented current index value to the current index value mentioned in step B2, and then execute step B3 to continue indexing within the range of the target upper limit index value mentioned in step B2, unless it exceeds the angular index traversal range.

[0057] While continuing indexing within the range where the current index value is less than the target upper limit index value described in step B2, each time the current index value described in step B2 is incremented by one, the sum of the incremented current index value and the starting index value of the point cloud angle is modulo 360 in step B4 to generate a new angle index value. Then, by executing step B5, the distance period correction value and angle period correction value corresponding to the new angle index value are calculated. Thus, while indexing within the range where the current index value is less than the target upper limit index value described in step B2, steps B3 to B6 are executed multiple times to accumulate the periodic sampling distance and accumulated the periodic sampling angle for different angle index values, thereby obtaining the distance period correction value and angle period correction value corresponding to different angle index values.

[0058] Step B7: If the distance period correction value and angle period correction value corresponding to each angle index value are accumulated within the current preset sampling period, then the end angle index value is updated to the start angle index value, and the next preset sampling period is updated to the current preset sampling period. Then, step B1 is executed to traverse the period sampling distance and period sampling angle stored in the next preset sampling period from the target memory space, and the next preset sampling period begins.

[0059] Each time the next preset sampling period is updated to the current preset sampling period, the periodic sampling distance and periodic sampling angle stored in the current preset sampling period are read from the target memory space. Then, without traversing to the end address of the target memory space, the method of setting the traversal range of the robot's corresponding angle index in the preset sampling period according to the ratio of the preset sampling period to the unit angle sampling time is executed to update the target upper limit index value, and the current index value is controlled to be restored to 0 in the current preset sampling period.

[0060] In some embodiments, each time step B7 is executed, the updated starting angle index value is increased by one, and then steps B1 and B2 are executed. In step 1, if the periodic sampling distance and periodic sampling angle stored in the current preset sampling period have not been traversed to the end address of the target memory space, step B2 is executed, which increases the target upper limit index value calculated in step B2 by one, thereby expanding the traversal range of the corresponding angle index in the current preset sampling period by one. This also realizes the correction of more point cloud angle regions by adding the latest periodic sampling distance and periodic sampling angle stored in the target memory space every preset sampling period, which is suitable for real-time processing.

[0061] Based on this, in a scenario where the robot approaches a straight wall at a certain speed, the closer the angle index value is to the starting angle index value of the point cloud, the more times the corresponding distance periodic correction value and angle periodic correction value are accumulated. This makes the straight wall at the location of the point corresponding to the starting angle index value of the point cloud straighter, and the point cloud gets closer to the robot. Therefore, the amount of motion (including periodic sampling distance and periodic sampling angle) collected first in the point cloud correction is more, and it is closer to the robot's real-time movement position, thus improving the accuracy of point cloud correction.

[0062] Step B8: Determine that all periodic sampling distances and all periodic sampling angles within the target memory space have been accumulated, i.e., the end address of the target memory space has been reached. Set the distance periodic correction value corresponding to the latest updated angle index value as the point cloud distance correction value corresponding to the same angle index value, and set the angle periodic correction value corresponding to the latest updated angle index value as the point cloud angle correction value corresponding to the same angle index value. At this point, the periodic sampling distances stored in each preset sampling period are accumulated within the corresponding angle index traversal range according to the same angle index value, and all periodic sampling distances within the target memory space have been accumulated, resulting in the point cloud distance correction value corresponding to the same angle index value. Similarly, the periodic sampling angles stored in each preset sampling period are accumulated within the corresponding angle index traversal range according to the same angle index value, and all periodic sampling angles within the target memory space have been accumulated, resulting in the point cloud angle correction value corresponding to the same angle index value. Then, execute the aforementioned step C.

[0063] In some embodiments, when the number of executions of steps B1 to B7 reaches a preset target number, step B8 is executed. This determines that the robot has experienced a preset target number of preset sampling periods, and that the point cloud distance correction value corresponding to the same angle index value is equal to the cumulative value of the periodic sampling distances within the preset target number of preset sampling periods. Simultaneously, it determines that the point cloud angle correction value corresponding to the same angle index value is equal to the cumulative value of the periodic sampling angles within the preset target number of preset sampling periods. The preset target number is determined by the measurement accuracy of the encoder. Thus, by executing steps B1 to B8, the distance periodic correction value and the angle periodic correction value are dynamically updated, ensuring that the finally set point cloud distance correction value and point cloud angle correction value maintain a real-time correction advantage over multiple preset sampling periods.

[0064] Regarding the measurement accuracy of the encoder, the point cloud frame is first considered as a series of discrete points arranged in a 360-degree circle. These points are sampled sequentially. Then, step A saves the periodic sampling distance and periodic sampling angle in the target memory space for each preset sampling period. Steps B and C then process these points based on the periodic sampling distance and periodic sampling angle saved in each preset sampling period. During the processing, the robot moves towards the point cloud, and time has passed. That is, these points undergo relative motion with respect to the robot, resulting in a distance offset or a certain angular deflection. Since measuring a small distance traveled by the drive wheel within a short period of time is considered a reliable measurement result, the preset target number is determined by the measurement accuracy of the encoder. The distance traveled by the robot's left or right drive wheel within the preset target number of preset sampling periods can be accurately measured by the encoder, eliminating slippage error interference. Therefore, the number of preset sampling periods is the preset target number, and the target memory space is configured to hold a maximum of the preset target number of periodic sampling distances and preset target number of periodic sampling angles, supporting index retrieval within the target memory space using index values.

[0065] In summary, by dynamically updating the distance periodic correction value and the angle periodic correction value through steps B1 to B8, the point cloud distance correction value required for each angle index value is obtained by continuously superimposing the periodic sampling distance stored within different preset sampling periods on the distance periodic correction value. Similarly, the point cloud angle correction value required for each angle index value is obtained by continuously superimposing the periodic sampling angle stored within different preset sampling periods on the angle periodic correction value. These values ​​describe the translation and rotation of each point in the point cloud relative to the moving robot, which is equivalent to predicting the motion of the point cloud and achieving cumulative error compensation. This is superior to fixed parameter correction and facilitates subsequent compensation of the corresponding distance and angle.

[0066] As one embodiment, specifically set within a preset sampling period, the method for calculating the periodic sampling distance and periodic sampling angle in step A includes:

[0067] The difference between the distance traveled by the encoder mounted on the robot's left drive wheel at the current moment and the distance traveled at the reference moment is used to obtain the distance traveled by the left drive wheel. The distance traveled by the encoder mounted on the left drive wheel at the current moment can be converted from the pulse count value currently measured by the encoder mounted on the left drive wheel. The distance traveled by the encoder mounted on the left drive wheel at the current moment and the current moment (a sampling timestamp) can be encapsulated in a data structure (e.g., assigned to the corresponding field in an object) and stored. The distance traveled by the encoder mounted on the robot's left drive wheel at the reference moment is a pre-stored motion parameter, so that the object indexed by each angle index value from memory contains motion information within a preset sampling period.

[0068] The difference between the distance traveled by the encoder mounted on the robot's right drive wheel at the current moment and the distance traveled at the reference moment is used to obtain the distance traveled by the right drive wheel. The distance traveled by the encoder mounted on the right drive wheel at the current moment can be converted from the pulse count value currently measured by the encoder mounted on the right drive wheel. The distance traveled by the encoder mounted on the right drive wheel at the current moment and the current moment (a sampling timestamp) can be encapsulated in a data structure (e.g., assigned to the corresponding field in an object) and stored. The distance traveled by the encoder mounted on the robot's right drive wheel at the reference moment is a pre-stored motion parameter, so that the object indexed by each angle index value from memory contains motion information within a preset sampling period.

[0069] Calculate half the sum of the movement distances of the left and right drive wheels to obtain the periodic sampling distance. Control the periodic sampling distance to be stored starting from the end address of the target memory space. In step B1, as the periodic sampling distances are stored in the target memory space sequentially from the first address of the target memory space for each preset sampling period, the closer the traversed address is to the end address of the target memory space, the newer the periodic sampling distance will be. That is, it will be able to traverse the periodic sampling distance stored in the most recent preset sampling period to ensure the real-time nature of the sampling distance information.

[0070] The ratio of the difference between the travel distance of the left and right drive wheels to the wheelbase is calculated to obtain the periodic sampling angle. The periodic sampling angle is then stored starting from the end address of the target memory space. The periodic sampling angle and periodic sampling distance can be stored in the same data structure, but they occupy different addresses. In step B1, as the periodic sampling angles are stored sequentially for each preset sampling period starting from the first address of the target memory space, the closer the traversed address is to the end address of the target memory space, the newer the periodic sampling angle is, meaning it can access the periodic sampling angle stored in the most recent preset sampling period, ensuring the real-time nature of the sampling angle information. Therefore, the aforementioned step B1, which determines whether the end address of the target memory space has been reached, essentially determines whether the periodic sampling distance and periodic sampling angle corresponding to the last preset sampling period stored in the target memory space have been completely traversed.

[0071] It should be noted that the start time of the preset sampling period is the reference time, and the end time of the preset sampling period is the current time. Understandably, if multiple preset sampling periods exist, the start time of the current preset sampling period equals the end time of the previous preset sampling period, and the end time of the current preset sampling period equals the start time of the next preset sampling period. The distance between the robot's left and right drive wheels is equal to the wheelbase. The motion distance sampled within the preset sampling period described in step A is divided into the motion distance of the left drive wheel and the motion distance of the right drive wheel. To calculate the motion distance measured by the encoder mounted on the robot's left drive wheel at the current time, the initial value of the reference time is set to the time when the robot was stationary before walking. Therefore, the motion distance measured by the encoder at the reference time is equal to 0, facilitating the measurement of the motion distance at the current time based on the encoder pulse count value measured at the reference time, thereby calculating the motion distance of the left and right drive wheels.

[0072] This application samples the code disk according to a preset sampling period T, which is generally T = 10ms. Of course, it can be set to other values ​​according to different needs. Setting it to 10 milliseconds is more appropriate. If the time is too long, it will affect the detection results; if the time is too short, the performance requirements of the sensor and processor will be too high.

[0073] Schematic diagram: The robot's wheelbase is H, the speed of the left drive wheel is v1, the speed of the right drive wheel is v2, and the robot's travel speed is... The angular velocity of the robot during rotation is Integrating v over the sampling period from reference time 0 to the current time t yields the robot's movement distance, which is: Integrating w yields the robot's rotation angle, which is... If this application calculates the periodic sampling distance and periodic sampling angle according to step A, and the reference time is 0 (the moment when the robot is stationary before walking), then v2t is the movement distance of the left drive wheel, v1t is the movement distance of the right drive wheel, and the pulse count value generated by the wheel rotation during the time interval 0 to t is multiplied by the number of millimeters corresponding to each pulse, and then divided by 1000 to obtain the total number of meters moved during the time interval 0 to t; where the number of millimeters corresponding to each pulse is equal to the circumference of the wheel (left drive wheel or right drive wheel) divided by the preset encoder ratio. Then according to... Calculate the periodic sampling distance and record it as the periodic sampling distance corresponding to the current preset sampling period; and follow the... Calculate the periodic sampling angle and record it as the periodic sampling angle corresponding to the current preset sampling period.

[0074] Based on the above embodiments, after calculating the periodic sampling distance and periodic sampling angle, to begin transitioning from the current preset sampling period to the next preset sampling period, the current time needs to be updated to the reference time. Simultaneously, the motion distance measured by the encoder mounted on the robot's left drive wheel at the current time is updated to the motion distance measured by the encoder mounted on the robot's left drive wheel at the reference time, and the motion distance measured by the encoder mounted on the robot's right drive wheel at the current time is updated to the motion distance measured by the encoder mounted on the robot's right drive wheel at the reference time. This facilitates the execution of steps A and B within the transition from the current preset sampling period to the next preset sampling period. Therefore, when executing step A again, the motion distance measured by the encoder mounted on the robot's left drive wheel at the current time is directly subtracted from the motion distance measured at the reference time to obtain the left drive wheel motion distance corresponding to the next preset sampling period; and the motion distance measured by the encoder mounted on the robot's right drive wheel at the current time is directly subtracted from the motion distance measured at the reference time to obtain the right drive wheel motion distance corresponding to the next preset sampling period. Since each preset sampling period is a time period experienced by the robot as it walks toward the distribution position of the point cloud in a frame, each point in the point cloud in a frame can correspond to the left drive wheel movement distance and the right drive wheel movement distance of a preset sampling period. Only then can the left drive wheel movement distance and the right drive wheel movement distance be used for motion pose prediction, which is convenient for point cloud correction.

[0075] As one embodiment, before step C traverses the pose angles of the point cloud frame, specifically before executing step A, each point in the point cloud frame is distributed relative to the robot at various physical locations within a 360-degree angular coverage area. This can be divided into 360 points corresponding to 1-degree angle index values, with each angle index value corresponding to a pose angle. Therefore, starting from 0 degrees, the pose angles of the point cloud frame are incrementally set at 1-degree intervals. Since the point cloud data correction method is executed as the robot moves towards the locations distributed in the point cloud frame, the pose angles of each point in the point cloud frame are set sequentially before executing step A.

[0076] It should be noted that the coordinates of each point in a frame of point cloud can form a two-dimensional matrix. Starting from 0 degrees, corresponding integer angles can be added to each point in the frame of point cloud at 1-degree intervals to serve as pose angles.

[0077] Therefore, for each pose angle set, a pose angle is added to the coordinates of the corresponding point until the added pose angle increases to 359. At this point, the pose angle range of a point cloud frame is 0 to 359. It is determined that 360 pose angles are added to the point cloud frame to avoid using inverse trigonometric functions to calculate angles, thereby speeding up the algorithm execution and reducing calculation errors. It also converts the point cloud frame into a set of points with a resolution of 1 degree according to the angle index value. The sequentially added pose angles form the index order of each point, and each pose angle can be mapped one-to-one with the aforementioned distance period correction value, angle period correction value, and angle index value, so that the point cloud data correction method can quickly perform point cloud correction.

[0078] In a scenario where the coordinates and pose angles of a frame of point cloud are stored in a point cloud container (which can be set within the target memory space), for each set pose angle, the coordinates and pose angle of the corresponding point are stored in the point cloud container so that the pose angle of the corresponding point is equal to the angle index value, until the pose angle increases from 0 degrees to 359 degrees, thus determining that the frame of point cloud is sorted into the point cloud container in ascending order of pose angle; then, by executing step C, the pose angles of the frame of point cloud are traversed in the point cloud container, wherein, after the frame of point cloud is stored in ascending order of pose angle, the absolute value of the difference between the pose angles of points stored in two adjacent storage addresses in the point cloud container is equal to 1.

[0079] Based on the above embodiments, in step C, whenever a point with a pose angle equal to the angle index value is traversed from the point cloud in a frame, a point cloud coordinate offset correction value is calculated based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value. Then, the point cloud coordinate offset correction value is used to correct the coordinates of the traversed points. Figure 3As shown, it specifically includes:

[0080] Step C1: Configure the angle index value to equal 0; and traverse the pose angles of each point in the point cloud frame according to the address storage order, and then execute step C2; so that: the angle index value can be traversed from 0 in the subsequent traversal of the pose angles of the point cloud frame. If a matching pose angle cannot be found in the point cloud container through the angle index value, the angle index value is incremented; otherwise, the next pose angle in the point cloud frame is traversed and the angle index value is incremented.

[0081] Here, an iterator can be defined to point to the starting address of the point cloud container, pointing to the pose angle of each point in a frame of point cloud, and the pose angle can be searched once within the range of 0 to 359 using the angle index value, because it is necessary to correct all points in the frame of point cloud once.

[0082] Step C2: Determine if the currently incremented angle index value is less than 360. If yes, proceed to step C3; otherwise, after confirming that the coordinates of all points in the point cloud frame have been corrected, step C can be executed, or the point cloud data correction method can be stopped. Step C2 sets the index boundary of the angle index value to 360 based on the index value range of the previously added pose angle, so that the currently incremented angle index value traverses each pose angle of the point cloud frame without exceeding the boundary.

[0083] Step C3: Determine whether the currently incremented angle index value is less than the pose angle currently being traversed. If yes, proceed to step C4; otherwise, proceed to step C5. Step C3 first assumes that the address indexed by the currently incremented angle index value is less than the end address of the point cloud container before comparing the currently incremented angle index value with the pose angle stored in the point cloud container to determine whether the currently incremented angle index value falls into the matching pose angle.

[0084] Step C5: Determine whether the currently incremented angle index value is equal to the pose angle of the current traversal. If yes, proceed to step C6; otherwise, proceed to step C7. Step C5 determines whether the currently incremented angle index value is a matching index value by comparing it with the pose angle of the current traversal. Then, use the currently incremented angle index value to index the point cloud distance correction value and point cloud angle correction value required for the point correction corresponding to the pose angle of the current traversal.

[0085] Step C7: Traverse to the pose angles adjacent to the currently traversed pose angle at the storage address. After excluding cases where the currently incremented angle index value is not less than the currently traversed pose angle, if the currently incremented angle index value is not equal to the currently traversed pose angle, determine that the currently incremented angle index value is greater than the currently traversed pose angle. It is possible that the currently traversed pose angle has been matched by the previously incremented angle index value. Continue traversing the pose angles adjacent to the currently traversed pose angle at the storage address within the point cloud container, and update the pose angles adjacent to the currently traversed pose angle at the storage address to the currently traversed pose angle. This is equivalent to incrementing the iterator to point to the new pose angle in the point cloud container so as to index the corresponding point. Then, execute step C3, and compare the new pose angle with the currently incremented angle index value at the level of pose angle iteration update.

[0086] Step C6: If the currently incremented angle index value is equal to the pose angle of the current traversal, then the currently incremented angle index value is determined to be a matching point cloud pose angle, completing one angle matching, which is also the matching of an angle of a point; and based on the currently incremented angle index value, the point cloud distance correction value and the point cloud angle correction value mentioned in step B are indexed, and then the point cloud coordinate offset correction value is calculated based on the point cloud distance correction value and the point cloud angle correction value, and then the point cloud coordinate offset correction value is used to correct the coordinates of the point corresponding to the pose angle of the current traversal; then step C8 is executed.

[0087] In step C, the method of calculating the point cloud coordinate offset correction value based on the point cloud distance correction value and the point cloud angle correction value, and then using the point cloud coordinate offset correction value to correct the coordinates of the point corresponding to the pose angle of the current traversal, includes: whenever a point cloud distance correction value and a point cloud angle correction value are indexed using the angle index value, the cosine value of the point cloud distance correction value and the point cloud angle correction value are multiplied to obtain the point cloud horizontal coordinate offset correction value, which is recorded as the coordinate offset in the x-axis direction of the map coordinate system; the sine value of the point cloud distance correction value and the sine value of the point cloud angle correction value are multiplied to obtain the point cloud vertical coordinate offset correction value, which is recorded as the coordinate offset in the y-axis direction of the map coordinate system. Then, the point cloud horizontal coordinate offset correction value and the point cloud vertical coordinate offset correction value are combined to form the point cloud coordinate offset correction value corresponding to the angle index value, so that the closer the robot is to a point indexed from the point cloud of the current pose angle, the larger the point cloud coordinate offset correction value.

[0088] Then, the x-coordinate of the point corresponding to the pose angle of the current traversal (from the point currently traversed within the point cloud container) is subtracted from the point cloud x-coordinate offset correction value to obtain the corrected x-coordinate of the point corresponding to the angle index value; and the y-coordinate of the point corresponding to the pose angle of the current traversal (from the point currently traversed within the point cloud container) is subtracted from the point cloud y-coordinate offset correction value to obtain the corrected y-coordinate of the point corresponding to the angle index value; therefore, the corrected x-coordinate and corrected y-coordinate of the point corresponding to the angle index value predict the angle index position that the point corresponding to the angle index value is most likely to reach after multiple preset sampling cycles through step B. When the robot walks, the point corresponding to the angle index value is closer to the robot body and closer to the obstacle distribution in the actual environment, which can predict the collision location in advance.

[0089] To distinguish between corrected and uncorrected points in the point cloud frame, after obtaining the corrected x-coordinate and y-coordinate of a point corresponding to the angle index value, the corrected x-coordinate of the point corresponding to the angle index value is updated to the x-coordinate of the point corresponding to the pose angle of the currently traversed point, and the corrected y-coordinate of the point corresponding to the angle index value is updated to the y-coordinate of the point corresponding to the pose angle of the currently traversed point. Then, step C8 is executed.

[0090] Step C8: Traverse to the pose angles adjacent to the currently traversed pose angle at the storage address. After determining that the currently incremented angle index value is equal to the currently traversed pose angle, and indexing the corresponding point cloud distance correction value and point cloud angle correction value, and correcting the coordinates of the point corresponding to the currently traversed pose angle based on the point cloud distance correction value and point cloud angle correction value, traverse the pose angles adjacent to the currently traversed pose angle at the storage address within the point cloud container, update the pose angles adjacent to the currently traversed pose angle at the storage address to the currently traversed pose angle, iterate through the pose angles, which is equivalent to incrementing the iterator by one, shifting to the next array element, pointing to the new pose angle of the point cloud container so as to index the corresponding point, and then execute step C4 to continue traversing to find the angle index value equal to the new pose angle.

[0091] Step C4: Increment the angle index value by one, so that the angle index value continues to increase by 1 degree angle resolution, while keeping the pose angle of the current traversal unchanged. Only update the angle index value to track the solution angle of the same point cloud. Then update the automatically incremented angle index value to the currently incremented angle index value, and then execute step C2 to traverse the pose angle of each point in the point cloud frame according to the angle index value.

[0092] In summary, whenever a point in the point cloud frame reaches a pose angle equal to the angle index value, it is considered a point with a matching angle index value, and also a point that needs correction (indexed using the pose angle or the angle index value of the same value). Then, a correspondingly calculated point cloud coordinate offset correction value is used to correct its coordinates. The corrected coordinates predict the most likely physical location of the point cloud frame after multiple preset sampling periods before the robot actually collides with the obstacles distributed at the point cloud frame. As the robot moves towards the location of the point cloud frame, the point cloud frame gets closer to the robot's body, and the result of correction using the point cloud coordinate offset correction value is closer to the actual situation.

[0093] Based on this, by iteratively executing steps C1 to C8 until step C2 determines that the currently incremented angle index value is greater than or equal to 360, the coordinates of all points in the point cloud frame are corrected. Then, the corrected coordinates of the point cloud frame are configured as the latest relative positional relationship between the obstacle at the target location and the robot's current position. This enables the robot to accurately and comprehensively predict the actual distribution position of the first point cloud frame traversed by the robot using the point cloud distance correction value and point cloud angle correction value. This allows the robot to predict collisions in advance and anticipate future risks. For example, when the robot is about to collide with or just touches an obstacle during its movement, the corrected coordinates predict the location of the obstacle, allowing the robot to detect the collision in advance (i.e., the obstacle point cloud has not yet been detected by the laser sensor) and trigger a collision warning signal. This ensures the robot's safety, reduces the occurrence of continuous obstacle-hitting, shortens the robot's obstacle contact time, solves the problems of the robot continuously hitting obstacles and the robot having long obstacle contact time, and avoids potential future safety risks.

[0094] The above embodiments are only for illustrating the technical concept and features of this application, and are intended to enable those skilled in the art to understand the content of this application and implement it accordingly. They should not be used to limit the scope of protection of this application. All equivalent changes or modifications made in accordance with the spirit and essence of this application should be included within the scope of protection of this application.

Claims

1. An angle-based point cloud data correction method, wherein the main body executing the point cloud data correction method is a robot equipped with an encoder and a laser sensor; After the robot uses a laser sensor to collect a frame of point cloud, the robot walks towards the location where the frame of point cloud is distributed and uses an encoder to measure the distance of movement during the walking process. Its features are, The robot performs a point cloud data correction method while walking toward the location where the point cloud in the frame is distributed; The point cloud data correction method includes: Step A: Sample the motion distance measured by the encoder according to the preset sampling period, then convert the motion distance sampled in each preset sampling period into periodic sampling distance and periodic sampling angle, and save the periodic sampling distance and periodic sampling angle in the target memory space for each preset sampling period; then execute Step B; Step B: Set the angle index traversal range corresponding to the robot within the preset sampling period according to the ratio of the preset sampling period to the unit angle sampling time; then, within the corresponding angle index traversal range, accumulate the periodic sampling distances stored in each preset sampling period according to the same angle index value, until all periodic sampling distances in the target memory space are accumulated, to obtain the point cloud distance correction value corresponding to the same angle index value; and accumulate the periodic sampling angles stored in each preset sampling period according to the same angle index value, until all periodic sampling angles in the target memory space are accumulated, to obtain the point cloud angle correction value corresponding to the same angle index value; then execute step C; wherein, one preset sampling period corresponds to one angle index traversal range, and one angle index traversal range can be divided into multiple angle index values; Step C: Traverse the pose angle of the point cloud frame. Whenever a point in the point cloud frame is traversed to a pose angle equal to the angle index value, calculate the point cloud coordinate offset correction value based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value. Then, use the point cloud coordinate offset correction value to correct the coordinates of the traversed points to correct the coordinates of each point in the point cloud frame.

2. The point cloud data correction method according to claim 1, characterized in that, In step B, the method for setting the angle index traversal range of the robot within the preset sampling period based on the ratio of the preset sampling period to the unit angle sampling time includes: Calculate the ratio of the preset sampling period to the unit angle sampling time to obtain the predicted angle value of a single preset sampling period scan; The starting angle index value is added to the predicted angle value of a single preset sampling period scan to obtain the ending angle index value; Calculate the target upper limit index value based on the ending angle index value, and then configure the target upper limit index value as the upper limit value of the angle index traversal range; The point cloud frame mentioned above is a point cloud collected by the robot during a scan using a laser sensor. The target upper limit index value is used to represent the maximum number of point clouds that the laser sensor is allowed to collect within a single preset sampling period.

3. The point cloud data correction method according to claim 2, characterized in that, The method for calculating the target upper limit index value based on the end angle index value includes: Detect the relationship between the starting and ending angle index values ​​of the point cloud; When the starting index value of the point cloud angle is detected to be less than the ending index value, the starting index value of the point cloud angle is subtracted from the ending index value to obtain the target upper limit index value. When the starting index value of the point cloud angle is detected to be greater than or equal to the ending index value, the starting index value of the point cloud angle is subtracted from 360, and then the difference obtained by subtraction is added to the ending index value to obtain the target upper limit index value. The point cloud angle starting index value is used to represent the angle index value of the first point in a frame of point cloud that is acquired by the laser sensor, and is configured to be equal to the starting angle index value.

4. The point cloud data correction method according to claim 3, characterized in that, Step B specifically includes: Step B1: Starting from the first address of the target memory space, traverse the periodic sampling distance and periodic sampling angle stored in the target memory space in step A, and determine whether the traversal has reached the last address of the target memory space. If yes, execute step B8; otherwise, execute step B2. In step A, the periodic sampling distance and periodic sampling angle are both stored starting from the last address of the target memory space. Step B2: Calculate the target upper limit index value by executing the method of setting the robot's angle index traversal range within the preset sampling period according to the ratio of the preset sampling period to the unit angle sampling time, and set the current index value to be equal to 0 within the current preset sampling period, and then execute step B3. Step B3: Determine whether the current index value mentioned in step B2 is less than the target upper limit index value mentioned in step B2. If yes, proceed to step B4; otherwise, proceed to step B7. Step B4: Determine that the current index value mentioned in step B2 is within the angle index traversal range, and control the sum of the current index value mentioned in step B2 and the starting index value of the point cloud angle to take the remainder of 360 to generate the angle index value; then execute step B5; Step B5: Add the periodic sampling distance stored in the target memory space within the current preset sampling period to the pre-set distance period correction value corresponding to the same angle index value to obtain the distance period correction value corresponding to the same angle index value, and update the distance period correction value corresponding to the same angle index value to the pre-set distance period correction value corresponding to the same angle index value; and add the periodic sampling angle stored in the target memory space within the current preset sampling period to the pre-set angle period correction value corresponding to the same angle index value to obtain the angle period correction value corresponding to the same angle index value, and update the angle period correction value corresponding to the same angle index value to the pre-set angle period correction value corresponding to the same angle index value; then execute step B6; Step B6: Control the current index value described in step B2 to automatically increment by one, then update the current index value after automatic increment to the current index value described in step B2, and then execute step B3; Step B7: Update the end angle index value to the start angle index value, and update the next preset sampling period to the current preset sampling period. Then execute step B1 to traverse the periodic sampling distance and periodic sampling angle stored in the next preset sampling period from the target memory space. Step B8: Determine all periodic sampling distances and all periodic sampling angles within the target memory space, and set the distance periodic correction value corresponding to the latest updated angle index value to the point cloud distance correction value corresponding to the same angle index value, and set the angle periodic correction value corresponding to the latest updated angle index value to the point cloud angle correction value corresponding to the same angle index value; then execute step C.

5. The point cloud data correction method according to claim 4, characterized in that, The first execution of step B5 also includes: The distance cycle correction value corresponding to the same preset angle index value is 0; the angle cycle correction value corresponding to the same preset angle index value is 0.

6. The point cloud data correction method according to claim 4, characterized in that, Each time step B7 is executed, the updated starting angle index value is increased by one. Then, steps B1 and B2 are executed, causing the target upper limit index value calculated in step B2 to increase by one, thereby expanding the traversal range of the corresponding angle index within the current preset sampling period by one.

7. The point cloud data correction method according to claim 4, characterized in that, In step A, the method for converting the periodic sampling distance and the periodic sampling angle includes: The difference between the distance traveled by the encoder mounted on the robot's left drive wheel at the current moment and the distance traveled at the reference moment is used to obtain the distance traveled by the left drive wheel. The difference between the distance traveled by the encoder mounted on the robot's right drive wheel at the current moment and the distance traveled at the reference moment is used to obtain the distance traveled by the right drive wheel. Calculate half the sum of the distances traveled by the left drive wheel and the right drive wheel to obtain the periodic sampling distance; The ratio of the difference between the travel distance of the left drive wheel and the travel distance of the right drive wheel to the wheelbase is calculated to obtain the periodic sampling angle; The start time of the preset sampling period is the reference time, and the end time of the preset sampling period is the current time; the distance between the robot's left drive wheel and its right drive wheel is equal to the wheel track; the motion distance sampled within the preset sampling period is divided into the motion distance of the left drive wheel and the motion distance of the right drive wheel.

8. The point cloud data correction method according to claim 7, characterized in that, After calculating the periodic sampling distance and periodic sampling angle, the motion distance measured by the encoder mounted on the robot's left drive wheel at the current moment is updated to the motion distance measured by the encoder mounted on the robot's left drive wheel at the reference moment; and the motion distance measured by the encoder mounted on the robot's right drive wheel at the current moment is updated to the motion distance measured by the encoder mounted on the robot's right drive wheel at the reference moment.

9. The point cloud data correction method according to claim 1, characterized in that, Before step C traverses the pose angles of the point cloud frame, the following steps are taken: starting from 0 degrees, the pose angles of the point cloud frame are incremented by 1 degree intervals; each time a pose angle is set, the pose angle is added to the coordinates of the corresponding point, until the added pose angles increase to 359, thus determining that 360 pose angles are added to the point cloud frame and forming the index order of each point, so that the point cloud frame is converted into a set of points with a resolution of 1 degree according to the angle index value. Wherein, the coordinates of each point in the point cloud frame are distributed within a 360-degree angular range relative to the robot before step A is executed; Wherein, after a frame of point cloud is stored in order of increasing pose angle, the absolute value of the difference between the pose angles of points stored in two adjacent storage addresses is equal to 1.

10. The point cloud data correction method according to claim 9, characterized in that, In step C, whenever a point with a pose angle equal to the angle index value is traversed from the point cloud in a frame, a point cloud coordinate offset correction value is calculated based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value. The method of then using the point cloud coordinate offset correction value to correct the coordinates of the traversed points includes: Step C1: Configure the angle index value to equal 0, and traverse the pose angle of each point in the point cloud frame according to the address storage order; then execute step C2. Step C2: Determine whether the currently incremented angle index value is less than 360. If yes, proceed to step C3; otherwise, determine the coordinates of all points in the point cloud of the frame after correction. Step C3: Determine whether the currently incremented angle index value is less than the pose angle of the current traversal. If yes, proceed to step C4; otherwise, proceed to step C5. Step C5: Determine whether the currently incremented angle index value is equal to the pose angle of the current traversal. If yes, proceed to step C6; otherwise, proceed to step C7. Step C7: Traverse to the pose angle adjacent to the current pose angle at the storage address, update the pose angle adjacent to the current pose angle at the storage address to the current pose angle, and then execute step C3. Step C6: Determine that the currently incremented angle index value matches a point cloud pose angle; and index the point cloud distance correction value and point cloud angle correction value mentioned in Step B based on the currently incremented angle index value. Then, calculate the point cloud coordinate offset correction value based on the point cloud distance correction value and point cloud angle correction value, and then use the point cloud coordinate offset correction value to correct the coordinates of the points corresponding to the pose angles currently being traversed; then execute Step C8. Step C8: Traverse to the pose angles adjacent to the currently traversed pose angles at the storage address, and then update the pose angles adjacent to the currently traversed pose angles at the storage address to the currently traversed pose angles; then execute step C4. Step C4: Increment the angle index value by one, then update the incremented angle index value to the currently incremented angle index value, and then execute step C2.

11. The point cloud data correction method according to claim 10, characterized in that, In step C, the method of calculating the point cloud coordinate offset correction value based on the point cloud distance correction value and the point cloud angle correction value corresponding to the angle index value, and then using the point cloud coordinate offset correction value to correct the coordinates of the traversed points includes: Whenever a point cloud distance correction value and a point cloud angle correction value are indexed using the angle index value, the cosine value of the point cloud distance correction value and the point cloud angle correction value are multiplied together to obtain the point cloud horizontal coordinate offset correction value; the sine value of the point cloud distance correction value and the sine value of the point cloud angle correction value are multiplied together to obtain the point cloud vertical coordinate offset correction value; wherein, the point cloud horizontal coordinate offset correction value and the point cloud vertical coordinate offset correction value constitute the point cloud coordinate offset correction value; The corrected x-coordinate of a point corresponding to the current pose angle is obtained by subtracting the point cloud x-coordinate offset correction value from the x-coordinate of the point; and the corrected y-coordinate of a point corresponding to the current pose angle is obtained by subtracting the point cloud y-coordinate offset correction value from the y-coordinate of the point; and the corrected y-coordinate of a point corresponding to the angle index value is obtained by subtracting the point cloud y-coordinate offset correction value from the y-coordinate of the point.

12. The point cloud data correction method according to claim 11, characterized in that, After obtaining the corrected x-coordinate and corrected y-coordinate of a point corresponding to the angle index value, the corrected x-coordinate of the point corresponding to the angle index value is updated to the x-coordinate of the point corresponding to the pose angle of the current traversal, and the corrected y-coordinate of the point corresponding to the angle index value is updated to the y-coordinate of the point corresponding to the pose angle of the current traversal. Then proceed to step C8.

Images