Obstacle avoidance method and system for mobile robot

Abstract

The application relates to the technical field of robot path planning, and provides an obstacle avoidance method and system for a mobile robot. The implementation scheme is as follows: a double-channel octree is constructed based on an obstacle point cloud, and the obstacle point cloud is clustered to obtain various target obstacles; the occupied areas of the various target obstacles in future space-time are respectively predicted to obtain various space-time occupied pipes; a space-time distance field is generated based on the double-channel octree and the various space-time occupied pipes to determine the original distances corresponding to various space points, and the various original distances are corrected in terms of uncertainty based on the confidence channel of the double-channel octree to obtain the conservative distances corresponding to the various space points; and the initial spline trajectory of the mobile robot is optimized and solved based on the various space-time occupied pipes and the various conservative distances to generate a target obstacle avoidance trajectory of the mobile robot. The embodiment of the application can reduce the collision risk of the mobile robot and realize reliable obstacle avoidance.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, and in particular to an obstacle avoidance method and system for a mobile robot. Background Technology

[0002] With the widespread application of mobile robots in scenarios such as warehousing and logistics, intelligent inspection, unmanned delivery, and service robots, the requirements for mobile robots' environmental perception and autonomous obstacle avoidance capabilities are constantly increasing. Typically, mobile robots acquire information about their surrounding environment through onboard sensors such as LiDAR, time-of-flight cameras, and depth cameras, and perform path planning and obstacle avoidance based on the environmental perception results.

[0003] However, in complex environments such as sensor noise interference, reflection errors, or partial occlusion, existing obstacle avoidance methods have limited ability to predict the future motion state of obstacles, making it difficult to determine the area occupied by dynamic obstacles in future spacetime in a timely and accurate manner. Furthermore, most existing mobile robot obstacle avoidance methods rely on the current environmental perception results for local path planning, failing to fully consider the uncertainties of these perception results. This inevitably leads to a deviation between the trajectory planning results and the actual environmental state, increasing the collision risk for the mobile robot and hindering reliable obstacle avoidance. Summary of the Invention

[0004] This invention provides an obstacle avoidance method and system for mobile robots, which can solve at least one of the above-mentioned technical problems.

[0005] In a first aspect, embodiments of the present invention provide an obstacle avoidance method for a mobile robot, comprising: A dual-channel octree is constructed based on the acquired obstacle point cloud, and the obstacle point cloud is clustered to obtain various target obstacles; The occupancy areas of each type of target obstacle in future spacetime are predicted to obtain the spacetime occupancy channels. Based on the dual-channel octree and each of the spatiotemporal occupancy channels, a spatiotemporal distance field is generated to determine the original distance corresponding to each spatial point. Based on the confidence channel of the dual-channel octree, uncertainty correction is performed on each of the original distances to obtain the conservative distance corresponding to each spatial point. Based on the spatiotemporal occupancy channels and the conservative distances, the initial spline trajectory of the mobile robot is optimized and solved to generate the target obstacle avoidance trajectory of the mobile robot.

[0006] Secondly, embodiments of the present invention provide an obstacle avoidance system for a mobile robot, comprising: The construction and clustering module is used to construct a dual-channel octree based on the acquired obstacle point cloud, and to cluster the obstacle point cloud to obtain various target obstacles; The prediction module is used to predict the occupancy area of ​​each type of target obstacle in future time and space, and obtain each time and space occupancy channel; The conservative distance calculation module is used to generate a spatiotemporal distance field based on the dual-channel octree and each of the spatiotemporal occupancy channels to determine the original distance corresponding to each spatial point, and to perform uncertainty correction on each of the original distances based on the confidence channel of the dual-channel octree to obtain the conservative distance corresponding to each spatial point. The target obstacle avoidance trajectory generation module is used to optimize and solve the initial spline trajectory of the mobile robot based on each of the spatiotemporal occupancy channels and each of the conservative distances, and generate the target obstacle avoidance trajectory of the mobile robot.

[0007] This invention employs a dual-channel octree constructed based on the acquired obstacle point cloud. Simultaneously, the obstacle point cloud is clustered to obtain various target obstacles. The future spatiotemporal occupancy areas of each type of target obstacle are predicted, resulting in spatiotemporal occupancy channels, thus enabling spatiotemporal modeling of the future movement trends of dynamic obstacles. Based on this, a spatiotemporal distance field is generated using the dual-channel octree and the spatiotemporal occupancy channels. Uncertainty correction is applied to the original distances corresponding to each spatial point based on the confidence channel in the dual-channel octree, yielding conservative distances for each spatial point. This proactively expands the safe distance to obstacles in areas with low environmental perception reliability, reducing the impact of distance estimation deviations caused by sensor noise interference, reflection errors, or partial occlusion on trajectory planning. Furthermore, the initial spline trajectory of the mobile robot is optimized using the spatiotemporal occupancy channels and conservative distances. In other words, the spatiotemporal occupancy channels serve as constraints on the future occupancy range of dynamic obstacles, and the conservative distances serve as safety constraints for trajectory obstacle avoidance, thereby optimizing the initial spline trajectory of the mobile robot. The generated obstacle avoidance trajectory not only avoids current obstacles but also anticipates the potential future occupation of areas by dynamic obstacles. Therefore, the obstacle avoidance method for mobile robots using this invention can reduce the collision risk and achieve reliable obstacle avoidance.

[0008] It should be understood that the description in this section is not intended to identify key or essential features of the embodiments of the present invention, nor is it intended to limit the scope of the invention. Other features of the invention will become readily apparent from the following description. Attached Figure Description

[0009] The accompanying drawings are provided for a better understanding of this solution and do not constitute a limitation of the invention. Wherein: Figure 1 This is a flowchart of an obstacle avoidance method for a mobile robot according to an embodiment of the present invention; Figure 2 This is a structural block diagram of an obstacle avoidance system for a mobile robot according to an embodiment of the present invention; Figure 3 This is a schematic block diagram of a computer device used to implement the methods of the embodiments of the present invention. Detailed Implementation

[0010] The following description, in conjunction with the accompanying drawings, illustrates exemplary embodiments of the present invention, including various details to aid understanding. These details should be considered merely exemplary. Therefore, those skilled in the art will recognize that various changes and modifications can be made to the embodiments described herein without departing from the scope of the invention. Similarly, for clarity and brevity, descriptions of well-known functions and structures are omitted in the following description.

[0011] This invention provides an obstacle avoidance method and system for a mobile robot. The entity executing this obstacle avoidance method can be the obstacle avoidance system provided in this invention, or a computer device integrating the obstacle avoidance system. The obstacle avoidance system can be implemented in hardware or software, and the computer device can be a terminal or a server.

[0012] In a specific application scenario, a mobile robot is equipped with a time-of-flight camera. The obstacle avoidance system of the mobile robot (hereinafter referred to as the obstacle avoidance system or the system) establishes a communication connection with the time-of-flight camera through a communication bus. The communication bus can be at least one of the following: Universal Serial Bus, Ethernet Bus, Mobile Industry Processor Interface Bus, Low Voltage Differential Signaling Bus, or Controller Area Network Bus. The obstacle avoidance system sends acquisition control commands to the time-of-flight camera through the corresponding communication protocol to control the time-of-flight camera to perform modulation and emission, phase sampling, and image transmission.

[0013] The obstacle avoidance system sends modulation parameter configuration commands to the time-of-flight camera to configure its modulation frequency, exposure duration, integration period, and phase sampling mode. Internally, the modulation control module of the time-of-flight camera, based on the modulation parameter configuration commands, drives the infrared emitting unit to emit modulated infrared light and controls the image acquisition unit to acquire corresponding reflected light intensity images with multiple different phase delays within the same modulation period, thus obtaining multiple raw phase images. Each raw phase image is used to characterize the reflected light intensity distribution information at different phase positions.

[0014] After acquiring raw phase images, the time-of-flight camera transmits the pixel matrix data corresponding to each raw phase image to the obstacle avoidance system via a communication bus. The time-of-flight camera has an internal image buffer to store each raw phase image and adds timestamp information to each raw phase image based on the acquisition time sequence. Upon receiving the raw phase images, the obstacle avoidance system synchronizes and aligns multiple raw phase images within the same sampling period based on the corresponding timestamp information to ensure data consistency in subsequent phase calculations.

[0015] To reduce data misalignment caused by system clock deviations during mobile robot movement, the obstacle avoidance system can also send a synchronization trigger signal to the time-of-flight camera. The time-of-flight camera performs synchronized exposure based on the synchronization trigger signal and returns a frame completion flag to the obstacle avoidance system after the exposure is complete, thus achieving timing synchronization between the obstacle avoidance system and the time-of-flight camera.

[0016] During environmental perception, the obstacle avoidance system also controls the time-of-flight camera to periodically switch between different modulation frequencies, and acquires corresponding raw phase images at each modulation frequency. The raw phase images corresponding to each modulation frequency are transmitted to the obstacle avoidance system via a communication bus to serve as the data basis for subsequent obstacle avoidance planning.

[0017] Figure 1 This is a flowchart of an obstacle avoidance method for a mobile robot according to an embodiment of the present invention.

[0018] like Figure 1 As shown, the obstacle avoidance method of this mobile robot may include: S110: Construct a dual-channel octree based on the acquired obstacle point cloud, and cluster the obstacle point cloud to obtain various target obstacles; S120 predicts the occupied areas of various target obstacles in future spacetime to obtain the spacetime occupied channels. S130, a spatiotemporal distance field is generated based on a dual-channel octree and various spatiotemporal occupancy pipelines to determine the original distances corresponding to each spatial point, and the uncertainty of each original distance is corrected based on the confidence channel of the dual-channel octree to obtain the conservative distances corresponding to each spatial point. S140 optimizes the initial spline trajectory of the mobile robot based on each spatiotemporal occupancy channel and each conservative distance, generating the target obstacle avoidance trajectory of the mobile robot.

[0019] For example, obstacle point cloud refers to the set of three-dimensional spatial points that represent potential collision objects at a specific height above the ground in real physical space after filtering out multipath interference noise, edge flypoints, and ground plane points from the global three-dimensional point cloud.

[0020] In this example, the Time of Flight (TOF) camera is controlled to continuously emit infrared light at a basic modulation frequency (e.g., 80 MHz), and the reflected light intensity images are acquired in each cycle with four phase delays of 0°, 90°, 180°, and 270°, which are used as the underlying data input for subsequent joint calculation of depth, amplitude, and confidence.

[0021] For example, a dual-channel octree refers to a spatially discretized data structure built around the robot's current pose, where each node independently maintains both an "occupancy channel" and a "confidence channel." The occupancy channel records the logarithmic probability of a node containing a physical entity, while the confidence channel records the time-weighted cumulative value of the Time-of-Flight (TOF) confidence score for each node. The target obstacle refers to a cluster of point clouds representing independent physical entities (such as pedestrians, shelves, or pillars) after Euclidean clustering.

[0022] In this example, the specific process of clustering obstacle point clouds to obtain various target obstacles is as follows: Based on the traditional spatial Euclidean distance, a Time-of-Flight (TOF) amplitude channel is introduced as a fourth-dimensional feature for extended Euclidean clustering. The calculation expression for the extended distance is as follows: ; In the formula, For three-dimensional points and Extended clustering distance between them; The square of the actual physical Euclidean distance between the two points in three-dimensional space; and These represent the TOF signal amplitudes corresponding to the two points; For the preset amplitude weighting coefficient (e.g.) (After dimensional normalization).

[0023] For example, suppose there are pedestrians wearing clothing that are pressed together in space (low reflectivity, amplitude) Small) and the wall behind (high reflectivity, amplitude) (Large). If clustering is performed solely based on spatial geometric distance, due to Extremely small, the two are very easy to be misjudged as the same object; while introducing After the penalty term, due to the significant difference in the reflectivity of the two, the extended distance... This will be significantly amplified. Through this mechanism, the system can accurately segment spatially adjacent points with drastically different reflective properties into different clusters, thereby improving the accuracy of target obstacle recognition.

[0024] It should be noted that in the above formula The traditional spatial Euclidean distance, as represented, can be calculated using the commonly used method in this field: the sum of squares of the differences between coordinate components in each dimension under a three-dimensional Cartesian coordinate system. Furthermore, when performing the aforementioned distance retrieval and clustering calculations on massive point clouds, to ensure real-time computation, a spatial indexing algorithm based on a KD-Tree (K-dimensional spatial partitioning tree) is typically employed to accelerate nearest neighbor search. This is a common technique in this field for implementing point cloud distance calculations and clustering, and will not be elaborated upon further here.

[0025] For example, the spatiotemporal occupancy pipeline refers to the union of a series of continuously expanding ellipsoidal region slices formed in three-dimensional space by a dynamic target obstacle within a future prediction time window due to changes in its own motion state and the diffusion of uncertainty in its state estimation.

[0026] For example, the spatiotemporal distance field refers to a four-dimensional set of spatial distance field slices that include the current time and multiple future predicted times, used to quantify the shortest geometric distance from any point in space to the surface of the nearest obstacle.

[0027] For example, the target obstacle avoidance trajectory refers to the trajectory that meets robot dynamic constraints (such as a maximum linear velocity limit of 1.5 meters per second (m / s) and a maximum acceleration limit of...). Under the premise of ), after gradient descent and active pruning of the initial spline control points, the output is a smooth sequence of movement instructions that does not interfere with any spatiotemporal occupancy pipeline.

[0028] For example, at the end of a single iteration of the Limited-memory Broyden-Fletcher-Goldfarb-Shanno (L-BFGS) optimizer, if the 3D convex hull formed by a set of control points intersects with the "spatiotemporal occupancy pipeline" of a pedestrian in front, the control point closest to the pedestrian is extracted from the control point set and forcibly projected to a safe margin position of 0.05 meters (m) outside the pipeline along the opposite direction of the conservative distance gradient. Through this pruning and projection action, the collision avoidance property of the convex hull is upgraded from "post-event verification" to "hard constraint" in the optimization process, avoiding the tragedy of collision caused by getting trapped in local optima.

[0029] According to the above implementation method, a dual-channel octree is constructed using obstacle point clouds, and the obstacle point clouds are clustered to obtain various target obstacles, realizing the organization and management of environmental space occupancy status and obstacle category information. Simultaneously, by predicting the future spatiotemporal occupancy areas of various target obstacles, spatiotemporal occupancy channels are obtained to spatiotemporally model the future movement trends of dynamic obstacles. Based on this, a spatiotemporal distance field is generated using the dual-channel octree and each spatiotemporal occupancy channel to determine the original distance corresponding to each spatial point. Uncertainty correction is then applied to each original distance based on the confidence channel in the dual-channel octree to obtain a conservative distance corresponding to each spatial point. This allows for proactive expansion of the safe distance in areas with low environmental perception reliability, reducing the impact of perception errors on trajectory planning. Furthermore, the initial spline trajectory of the mobile robot is optimized using each spatiotemporal occupancy channel and each conservative distance, enabling the generated target obstacle avoidance trajectory to simultaneously avoid both current obstacles and the potential future occupancy areas of dynamic obstacles, thereby reducing the collision risk of the mobile robot and achieving reliable obstacle avoidance.

[0030] In one implementation, before constructing a dual-channel octree based on the acquired obstacle point cloud, the method further includes: jointly solving each first pixel of each original phase image acquired by the flying camera on the mobile robot to obtain the depth, amplitude, and confidence level corresponding to each first pixel; performing interference suppression and filtering processing on each first pixel based on the depth, amplitude, and confidence level corresponding to each first pixel to obtain each second pixel; projecting each second pixel onto a preset three-dimensional coordinate system to obtain each three-dimensional point; determining the global three-dimensional point cloud based on each three-dimensional point and the confidence level of each three-dimensional point; and extracting obstacles from the global three-dimensional point cloud to obtain the obstacle point cloud.

[0031] For example, controlling the TOF camera at the fundamental modulation frequency (For example, (MHz) emits sinusoidally modulated infrared light, and in each cycle... , , , The original phase images were acquired using four phase delays. ;in, They are respectively in , , , The original phase images acquired under four phase delays.

[0032] In this example, for each original phase image, the first pixel of the original phase image is jointly solved to obtain the following expression for calculating the depth corresponding to each first pixel: ; ; ; The formulas for calculating its amplitude and confidence level are as follows: ; ; In the formula, , and The first pixel The corresponding depth (which can be understood as the first depth), amplitude, and confidence level; c is the speed of light (e.g., m / s); The basic modulation frequency; and These are the in-phase and quadrature components obtained by subtracting the original intensity maps under different phase delays, respectively. , , , They are respectively , , , The original phase image acquired with phase delay; This indicates the operator for finding the arctangent in the four quadrants; The noise variance of the sensor is read out (as specified by the actual manufacturer).

[0033] For example, confidence level The physical meaning of is its signal-to-noise ratio normalized value, which ranges from [0,1]. Assume that the number of effective photons (amplitude A) received by a certain pixel is much greater than the sensor's noise floor. If the amplitude is weak due to the light-absorbing surface, the calculated C value will approach 1; conversely, if the amplitude is weak due to the light-absorbing surface, the C value will approach 0. This formula establishes a benchmark for measuring the reliability of the measurement in all subsequent steps.

[0034] In this example, the process of performing interference suppression and filtering on each first pixel based on the depth, amplitude and confidence level of each first pixel to obtain each second pixel includes: performing phase consistency detection through a dual-frequency strategy to suppress multipath interference, using dual verification of depth gradient and amplitude gradient to remove edge flying points, and then filtering out holes and statistical outlier pixels to obtain the remaining effective pixels (i.e., the second pixels).

[0035] Specifically, firstly, besides the fundamental modulation frequency In addition, it periodically switches to the second modulation frequency. (like A set of phase maps was acquired (MHz), and the second depth was calculated using the method shown in the previous example. .

[0036] Second, define pixel phase consistency index. ,have ;like If the pixel is determined to be severely contaminated by multipath interference, it will be removed; if Then, phasor decomposition suppression is performed on the pixel depth as follows: At the same time, the confidence level of this pixel is downgraded using the following formula: In the formula, For multipath interference determination threshold (e.g.) m); For the suppression intensity coefficient (e.g.) ); This is the corrected depth value (i.e., the second depth); This represents the confidence level after the downgrade.

[0037] Third, flying points occur at the edges of objects and are characterized by low amplitude and a large depth gradient. For each pixel, its... Amplitude gradient in the neighborhood With depth gradient : ; ; When both conditions are met and At that time, the judgment These are flying points and are discarded. For depth gradient threshold (e.g.) (meters / pixel) Amplitude threshold (e.g.) ), This represents the median amplitude of the current frame.

[0038] Fourth, for the remaining pixels after the above multipath suppression and flying point removal, invalid pixels are further identified according to the following rules: if the depth value Or outside the effective measurement range Inside (e.g.) m、 m), is determined to be a hole pixel; if the confidence level is... (like ), and is judged as a low-confidence pixel; if in Within the neighborhood These were identified as statistical outliers. , In pixels The local mean of the depth values ​​of all pixels within a preset local spatial window centered on the center (such as a 3×3 pixel neighborhood); This represents the local standard deviation of the depth values ​​of all pixels within the same local spatial window. None of the above three types of pixels participate in subsequent point cloud generation; the remaining valid pixels are the second pixel.

[0039] Fifth, perform confidence-weighted bilateral filtering smoothing on the second pixel. The expression for calculating the filter weights is as follows: ; In the formula, w represents the neighboring pixels. For the center pixel The filter weights; Standard deviation of spatial distance (e.g.) (pixels) The standard deviation of depth similarity (e.g.) millimeter (mm) This represents the effective confidence level after multipath suppression downgrading. Using the aforementioned weights, a weighted average of the neighborhood depth values ​​d is calculated, i.e. This allows us to obtain the depth values ​​corresponding to each smoothed second pixel. .

[0040] For example, in the depth map smoothing stage, the three multiplication factors in the filter weight calculation manage "higher weight for spatially closer pixels," "higher weight for pixels with similar depths," and "higher confidence neighbors contribute more." If there are both high-confidence points and low-confidence points contaminated by noise around the center pixel, the calculation of w directly multiplies by... The weight of high-confidence pixels is significantly amplified. This forces high-confidence pixels to dominate the filtering result, suppressing noise pollution in low-confidence regions at the source. This is the first core step in the end-to-end propagation of confidence, and its smoothing effect is already materialized in the depth value. middle.

[0041] In this example, each second pixel is projected onto a preset 3D coordinate system, and the calculation expressions for each 3D point are as follows: ; ; In the formula, These are the three-dimensional coordinates in the camera coordinate system. For the internal parameters of the TOF camera (where, For the focal length intrinsic parameter component of the TOF camera; (The optical core intrinsic reference component of the TOF camera). This is the extrinsic transformation matrix from the camera coordinate system to the world coordinate system; That is, the coordinates of the three-dimensional point ultimately projected onto the preset three-dimensional coordinate system (world coordinate system); It is a constant.

[0042] For example, a global 3D point cloud refers to the set composed of the projected 3D points and their confidence scores inherited from the source pixels. After a complete solution, filtering, and projection process, each point in the final global 3D point cloud not only contains its 3D spatial location but also carries an additional confidence score field, forming a quadruple with confidence scores. The fourth dimension This data is directly passed to point cloud downsampling, dual-channel octree construction, Euclidean Signed Distance Field (ESDF) derivation, and finally trajectory optimization, forming the underlying data foundation of the entire confidence-aware collision avoidance algorithm.

[0043] In one implementation, obstacle extraction is performed on the global 3D point cloud to obtain an obstacle point cloud, including: voxel downsampling the global 3D point cloud to obtain a downsampled point cloud; outlier removal is performed on the downsampled point cloud based on the local curvature, local normal deviation, and local amplitude deviation corresponding to the downsampled point cloud to obtain candidate 3D points; layered ground plane fitting is performed on each candidate 3D point based on preset prior constraints and the confidence values ​​corresponding to each candidate 3D point to obtain a target ground plane; and candidate 3D points with a vertical distance greater than a preset height threshold from each candidate 3D point are extracted to obtain the obstacle point cloud.

[0044] For example, voxel downsampling is performed on the global 3D point cloud to obtain the calculation expressions for each downsampled target 3D point as follows: ; ; In the formula, r is the Euclidean distance from the sampling point to be reduced to the sensor; Basic voxel side length (e.g.) ); For distance scaling coefficients (e.g.) ); The actual voxel side length used adaptively for the local region where this point is located; and The first voxel within the same voxel Spatial coordinates and confidence level of a three-dimensional point; and These represent the centroid coordinates of the target 3D point generated after downsampling and its inherited merged confidence level, respectively.

[0045] For example, in distant areas ( (larger) It will adaptively increase the voxel side length to compress the data volume; in the near region ( For smaller voxels, a finer mesh is maintained to preserve detail. Additionally, if multiple points exist within the same voxel, the calculation... Instead of using the conventional arithmetic mean, it utilizes confidence levels. The weighted centroid is obtained by using the weights. If there are real points with high confidence and edge flying points with low confidence within the voxel, the weight of the high-confidence points will absolutely dominate the centroid position, thus effectively avoiding the contamination of the target's physical location by residual noise during the downsampling stage.

[0046] In this example, the calculation expression for outlier removal based on the local curvature, local normal deviation, and local amplitude deviation of each target 3D point is as follows: ; ; ; In the formula, For local curvature; Principal component analysis eigenvalues ​​of the covariance matrix of the k nearest neighbors (e.g., k=20) of the target 3D point p; The neighborhood average normal deviation; Let p be the set of k-neighborhood points; and These are the local normals of point p and its neighboring point q (taking the direction of the smallest eigenvector); The neighborhood average amplitude deviation; and These are the TOF amplitudes at points p and q, respectively. This represents the median amplitude of the point cloud in the current frame.

[0047] For example, setting a local curvature threshold Normal deviation threshold Amplitude deviation threshold If the calculation is performed for a certain point p, then... and and If both conditions are met, it indicates that the point is extremely discontinuous in geometric space, and its reflection intensity has a severe abrupt change from the surrounding environment. Therefore, it is identified as outlier noise and removed. This multi-dimensional joint criterion takes into account both spatial geometry and reflection intensity consistency, and possesses strong targeted identification capabilities for TOF edge flying point remnants.

[0048] For example, prior constraints refer to historical temporal smoothing conditions introduced in the coarse fitting stage to avoid parameter jumps caused by ground normal jitter between dynamic frames in the Random Sample Consensus (RANSAC) algorithm. For instance, when performing the first coarse fitting on a subset of point clouds within a 1.2m height range below the sensor, it is mandatory that the angle between the current candidate normal and the plane normal estimated in the previous frame must be less than [the angle between the current candidate normal and the plane normal estimated in the previous frame]. This is used as the initial a priori value.

[0049] In this example, based on prior constraints and confidence values, a hierarchical ground plane fitting is performed on each candidate 3D point to obtain the following expression for the target ground plane equation: In the formula, The normalized components of the ground plane normal vector satisfy... ; This represents the directed distance from the ground plane to the origin of the world coordinate system. The coordinates of any point on the ground in the world coordinate system.

[0050] For example, based on the prior normal provided by the first layer coarse fitting, in the coarse plane... A second layer of fine fitting is performed on the neighborhood candidate point cloud. During the fine fitting sampling process, the weight of the vote for each candidate intrapoint is multiplied by its confidence level C. In this way, even if there are low-confidence observation pixels on the ground (such as light-absorbing areas like black puddles), it will not cause interference or jitter in the overall ground equation fitting.

[0051] In this example, from each candidate 3D point, candidate 3D points whose vertical distance to the target ground plane equation is greater than a preset height threshold are extracted, and the calculation expression for the obstacle point cloud is obtained as follows: In the formula, The extracted obstacle point cloud; This represents the set of all remaining candidate 3D points. For a preset vertical height threshold (e.g.) ).

[0052] By using elevation spatial filtering, ground points were precisely filtered out, while suspended or protruding objects posing a substantial collision risk were retained. Furthermore, the extracted obstacle point cloud... Each point in the data still carries the original confidence field C, providing a complete data transfer chain for subsequent dual-channel octree construction and spatiotemporal Euclidean distance field (ESDF) derivation.

[0053] In one implementation, a dual-channel octree is constructed based on the acquired obstacle point cloud, including: constructing an initial octree centered on the current pose of the mobile robot; updating the occupancy probability and confidence value of each first octree node hit by the obstacle point cloud in the initial octree, obtaining updated first octree nodes; updating the occupancy probability of each second octree node penetrated by light in the initial octree, obtaining updated second octree nodes; extracting effective occupied nodes from the updated first and second octree nodes whose occupancy probability is greater than a preset occupancy threshold and whose confidence value is greater than a preset confidence threshold; and constructing a dual-channel octree based on the effective occupied nodes.

[0054] For example, the initial octree refers to a tree established with the current spatial pose of the mobile robot as the origin, and has a preset global side length (e.g., ...). ) and minimum leaf node resolution (e.g. The octree is a three-dimensional spatial discretized data structure. Each node in the initial octree independently maintains two data channels, including an occupancy channel for recording the occupancy probability in the form of Log-odds, and a confidence channel for recording the time-weighted cumulative state of the confidence of all TOF observations falling into that node.

[0055] In this example, the obstacle avoidance system acquires the mobile robot's current pose information in the world coordinate system and uses the position corresponding to the current pose as the center position of the octree to establish a three-dimensional spatial partitioning structure within a preset spatial range. The initial octree consists of multiple octree nodes, each corresponding to a different spatial region.

[0056] For example, the space surrounding the mobile robot's current location can be divided into multiple cubic regions, and then recursively divided according to spatial hierarchy to form an octree structure. Regions closer to the mobile robot use smaller leaf nodes, while regions farther away from the mobile robot use larger leaf nodes, in order to balance spatial representation accuracy and storage efficiency.

[0057] For example, the obstacle avoidance system maps each obstacle point in the obstacle point cloud to a corresponding octree node, and determines the octree node to which the obstacle point falls as the first octree node. Each first octree node corresponds to an occupied channel and a confidence channel.

[0058] In this example, the calculation expressions for updating the occupancy probability and confidence value of each first octtree node that is hit by the obstacle point cloud in the initial octtree are as follows: ; In the formula, is the log-occupancy probability of the first octree node in the occupied channel; C is the confidence value carried by the obstacle point that hits this node; These are the parameters of the hit model (representing the theoretical probability of observing occupation). Accumulate the confidence value of the first octree node over time in the confidence channel; The time smoothing coefficient for the confidence level channel; This indicates the assignment update operator.

[0059] For example, setting the hit model parameters Time smoothing coefficient When a node in the first octree is given a high confidence level (e.g., ... When an obstacle point is hit, the Log-odds increment is weighted using the confidence level C, significantly increasing the probability of that node being occupied. Simultaneously, a linear combination of the historical state weight (0.9) and the current observation weight (0.1) smoothly integrates the current high confidence level into the node's historical confidence level. This ensures the temporal stability of the observation results and avoids drastic oscillations in node states caused by sudden noise changes in a single frame.

[0060] In this example, the calculation expression for updating the occupancy probability of each second octree node that is penetrated by light in the initial octree is as follows: In the formula, C is the logarithmic occupancy probability of the second octree node in the occupied channel; C is the confidence value carried by the corresponding ray endpoint (i.e., the actual hit point) passing through the node. These are the penetration model parameters (representing the theoretical probability of an observation being idle).

[0061] For example, setting the penetration model parameters .because Logarithmic terms The calculation result is negative. When the light emitted by the sensor passes through the node, the occupancy probability of the node is reduced proportionally according to the magnitude of the endpoint confidence value C.

[0062] It should be noted that for the second octree node that has been penetrated, the algorithm only updates its occupied channels, not its confidence channels. Update the node to maintain its historically accumulated true confidence attribute from being destroyed by idle rays.

[0063] In this example, the dual logical condition for extracting a validly occupied node from each updated first octree node and each updated second octree node is as follows: and In the formula, This is a preset occupancy threshold; This is the preset reliability threshold.

[0064] For example, setting an occupancy threshold Reliability threshold If a node, due to continuous reception of edge flying points or multipath interference noise from a TOF camera, manages to exceed the probability of accumulating in the occupied channel, then... However, because these noise points are assigned extremely low confidence values ​​during the underlying solution, the node appears in the confidence channel. The time-weighted cumulative value in the middle is always in The low position of the node cannot break through the confidence threshold of 0.4. Therefore, the node cannot simultaneously meet both conditions and will be directly rejected from the list of valid occupied nodes. Through this physical dual-channel independent verification of occupancy and confidence, the interference of TOF pseudo-obstacles on subsequent distance field calculations and path planning can be fundamentally suppressed.

[0065] Multiple valid occupied nodes can be obtained through the above method. The obstacle avoidance system organizes these valid occupied nodes into a dual-channel octree structure according to spatial hierarchy. Each node in the dual-channel octree retains both an occupied channel and a confidence channel. The occupied channel describes the obstacle occupancy status of the corresponding spatial region, while the confidence channel describes the observation reliability of the corresponding spatial region.

[0066] In one implementation, the occupancy regions of various target obstacles in future spacetime are predicted to obtain various spacetime occupancy channels. This includes: for each type of target obstacle, performing state estimation processing on the observed centroid corresponding to the target obstacle to obtain velocity estimation results, acceleration estimation results, and an initial state covariance matrix; for each preset prediction time, performing state deduction on the velocity estimation results, acceleration estimation results, initial state covariance matrix, and a preset state transition matrix to obtain the predicted centroid position and predicted covariance matrix of the target obstacle at the prediction time; calculating the geometric expansion coefficient of the target obstacle based on the maximum side length in the target obstacle's 3D bounding box; performing region expansion processing on the target obstacle based on the predicted centroid position, the submatrix corresponding to the position in the predicted covariance matrix, the geometric expansion coefficient, and a preset confidence level threshold to obtain the occupancy region of the target obstacle at the future prediction time; and generating various spacetime occupancy channels based on the occupancy regions of each target obstacle at each future prediction time.

[0067] For example, for each tracked target obstacle, a corresponding uniformly accelerated motion model is established, and an extended Kalman filter (EKF) is used to perform state estimation processing on the observed centroid of the target obstacle.

[0068] The state vector corresponding to the target obstacle is defined as a nine-dimensional state vector. In the formula, For entities 9-dimensional state vector; The coordinate components of the centroid of the entity in three-dimensional space; Three-dimensional velocity components; These are three-dimensional acceleration components.

[0069] Based on a uniformly accelerated motion model, corresponding state prediction equations and observation update equations are constructed, using the observed centroid data acquired at the current time as the observation input. An extended Kalman filter is used to recursively update the state vector corresponding to the target obstacle, outputting the target obstacle's state at time [time value missing]. Corresponding velocity estimation results Acceleration estimation results and the initial state covariance matrix (Right now The state covariance matrix is ​​used to reflect the uncertainty of the initial motion state estimation.

[0070] Accordingly, at each time step, the extended Kalman filter first predicts the target motion state at the current time step based on the state estimation result of the previous time step and the uniform acceleration motion model; then, it corrects the prediction result by combining the observed centroid data acquired at the current time step, thereby obtaining the updated state estimation result.

[0071] In complex dynamic scenes, the centroid of a single-frame observation from a TOF camera may fluctuate due to occlusion or flying points, leading to significant errors when directly calculating speed using centroid differences. By introducing EKF and a 9-dimensional state vector, the algorithm not only obtains smooth and reliable real-time speed and acceleration but also transforms observation noise into a covariance matrix. Quantitative expressions were made, providing complete initial conditions for subsequent uncertainty prediction.

[0072] For example, for a given future prediction time In the formula, The time interval is , such as 2.0s. The calculation expression for state deduction is as follows: ; ; In the formula, For entities In the future The predicted centroid position; Initial time Location of the center of mass; For entities In the future The predicted covariance matrix; The state transition matrix is ​​a preset value. Let T be the initial state covariance matrix at the initial time; T is the transpose. This is the preset process noise covariance matrix.

[0073] With the prediction time window The increase not only predicts the center of mass It will move forward along the directions of velocity and acceleration, predicting the covariance matrix. It can also be due to process noise It expands continuously over time.

[0074] For example, the coefficient of geometric expansion The calculation expression is: In the formula, This is the maximum side length of the 3D axis-aligned bounding box (AABB) corresponding to the target obstacle entity.

[0075] For example, for a target obstacle identified as a pedestrian, the maximum side length of its 3D bounding box. If the value is 0.8 meters, then the calculated morphological geometric expansion coefficient is... The value is 0.16. In this way, the inherent physical dimensions of the target obstacle are transformed into the static basic compensation amount in the subsequent variance inflation.

[0076] For example, define an entity At any moment occupied area For an inflated ellipsoid that incorporates both geometric dimensions and prediction uncertainties, its mathematical discriminant expression is as follows: ; In the formula, For any coordinate point in three-dimensional space; for Predicting covariance matrix Extracted only from the positional components Submatrix; It is the identity matrix; Confidence level threshold (e.g.) (This corresponds to the chi-square threshold at a 95% confidence level in statistics).

[0077] For example, in a very short time at close range, It is very small; the size of the ellipsoid is mainly determined by the physical dimensions. The decision is made to prevent the mobile robot from directly colliding with the obstacle itself; while predicting the covariance at a much farther time interval. As the ellipsoid grows rapidly and becomes dominant, it will be drastically stretched along the direction of pedestrian movement. With a statistical probability of 95%, it is ensured that the pedestrian will never overflow the boundary of this expanding ellipsoid in the future.

[0078] For example, all tracked entities are at the same prediction time. The system performs a union operation on the occupied regions to generate a spatiotemporal occupation pipeline. For example, if a pedestrian moving left and a forklift moving right coexist in the scene, the system will perform a union operation on the occupied regions in the future. The two ever-growing, elongated ellipsoids are seamlessly merged into a joint restricted area (i.e., a pipe slice) through continuous prediction time windows. By sequentially stacking these slices, a three-dimensional occupancy channel that blocks collision risks in the spatiotemporal dimension can be generated, providing an absolute mathematical obstacle avoidance boundary for the mobile robot's subsequent active safety pruning of B-spline trajectories.

[0079] In one implementation, a spatiotemporal distance field is generated based on a dual-channel octree and various spatiotemporal occupancy pipelines to determine the original distance corresponding to each spatial point. This includes: constructing spatial distance field slices corresponding to each prediction time of the current frame based on the current occupancy state of each node in the dual-channel octree and various spatiotemporal occupancy pipelines; integrating the spatial distance field slices to obtain the spatiotemporal distance field; initializing the distance values ​​corresponding to each grid in the spatiotemporal distance field to obtain the initial distance corresponding to each grid; performing global propagation processing on the initial distance corresponding to each grid according to the barrel wavefront propagation method to obtain the first distance corresponding to each spatial point; comparing the current grid state corresponding to the current frame with the historical grid state corresponding to the previous frame to extract each target change grid whose grid state has changed and the influence domain corresponding to each target change grid; and performing local propagation processing on the first distance corresponding to each spatial point based on each target change grid and each influence domain to obtain the original distance corresponding to each spatial point.

[0080] For example, a spatial distance field slice refers to a three-dimensional rasterized distance field constructed for the spatial environment around the robot at a certain prediction time, used to characterize the distance distribution from each spatial location to the nearest obstacle at that prediction time.

[0081] Specifically, each spatial distance field slice corresponds to a prediction time. For example, the prediction time could be the first future time, the second future time, etc.; different prediction times correspond to different spatial distance field slices. Each grid cell in the spatial distance field slice records a distance value, which is used to represent the Euclidean distance between the current grid cell and the nearest occupied grid cell.

[0082] Understandably, at the current moment, there is a static wall obstacle in front of the robot in the corresponding spatial distance field slice; at the predicted future moment, as the pedestrian moves in the direction of the robot's movement, the occupied area of ​​the pedestrian in the corresponding spatial distance field slice also moves forward synchronously; therefore, the occupied grid distribution in the spatial distance field slice corresponding to different predicted moments is different.

[0083] For example, a four-dimensional spatiotemporal distance field can be formed by stacking the spatial distance field slices corresponding to multiple prediction times according to the time dimension.

[0084] In this example, we obtain the various future prediction times. The time step example value is... Example value for predicted total duration When constructing slices, for the current moment... The slices are generated directly from the current occupied state of the static bichannel octree; however, for future prediction moments... The slice will then predict the dynamic spatiotemporal occupancy pipeline. A spatial union operation is performed with the occupancy state of the static octree. Then, all time slices are stored uniformly and integrated into a four-dimensional spatiotemporal distance field. .

[0085] For example, a barrel wavefront propagation algorithm is used to update the distance values ​​in the spatiotemporal distance field. First, a distance initialization operation is performed on each grid in the spatiotemporal distance field. Specifically, the distance values ​​corresponding to the grids determined to be occupied are set to a first preset value (e.g., 0) to serve as the starting source point for distance propagation; and the distance values ​​corresponding to the grids not in an occupied state are set to a second preset value (e.g., 0). This indicates that the effective distance update has not yet been completed, thus obtaining the initial distance corresponding to each grid cell.

[0086] Subsequently, based on the barrel wavefront propagation method, global propagation processing is performed on the initial distances corresponding to each grid. Specifically, according to the ascending order of the currently recorded distance values ​​of each grid, the corresponding grid is extracted as the current propagation center, and the neighboring grids around the current propagation center are traversed. The distance value corresponding to the current propagation center and the step size of the neighboring space are summed to obtain the candidate distances corresponding to each neighboring grid. If the candidate distance is less than the currently recorded distance value of the neighboring grid, the original distance value is updated using the candidate distance, and propagation continues outward until the distance diffusion in the entire spatial distance field slice is completed, obtaining the first distance corresponding to each spatial point. Among them, the smaller the distance value corresponding to the occupied state grid, the closer the corresponding spatial location is to the obstacle.

[0087] This priority queue-based propagation mechanism is physically equivalent to allowing the "distance field" to spread strictly, uniformly, and at a constant speed from all obstacle surfaces to the surrounding open areas, just like water waves. This ensures that the calculated distance value is equal to the true shortest Euclidean distance from the spatial point to the obstacle.

[0088] For example, in a warehouse mobile robot scenario, there is a shelf obstacle in front of the robot. At this point, the grid corresponding to the shelf location is determined to be occupied, and its distance value is initialized to 0; while the distance values ​​of the other empty grids are initialized to infinity. Then, using the grid corresponding to the shelf as the starting point, the distance of grids directly adjacent to the shelf is updated to one grid step; the distance of grids farther away increases layer by layer, ultimately forming a distance distribution spreading outward from the shelf. Therefore, the closer a location is to the shelf, the smaller its distance value; the farther away a location is, the larger its distance value. This allows us to obtain the distance distribution from the robot's surrounding space to the nearest obstacle.

[0089] For example, a target change grid refers to a grid whose state has changed compared to the previous frame. This change in grid state includes, but is not limited to: changing from a non-occupied state to an occupied state; changing from an occupied state to a non-occupied state; a change in occupancy confidence; and movement of the occupied area corresponding to a dynamic obstacle.

[0090] For example, in the previous frame, a certain grid position is an empty area; in the current frame, because a pedestrian is detected moving to that position, the grid changes from an unoccupied state to an occupied state; at this time, the grid is the target change grid.

[0091] For example, in the previous frame, a certain grid corresponds to a dynamic obstacle; in the current frame, the dynamic obstacle leaves that position; then the grid changes from an occupied state to an idle state, which is also a target change grid.

[0092] In this example, to avoid the extremely time-consuming global recalculation of the vast 3D space in every frame during the robot's movement, the algorithm introduces an incremental calculation mechanism. By rigorously comparing the states of consecutive frames, the algorithm accurately extracts the target grids whose occupancy states have changed.

[0093] For example, the radius of the influence domain is set. The algorithm only considers the target change grids and their radii that have changed relative to the previous frame. The local propagation operation of distance comparison and update is performed on the affected domain within the range; while for the vast safe space outside the affected domain, the distance result of the previous frame is directly reused.

[0094] For example, during the operation of a mobile robot, a pedestrian moves to the left. In the previous frame: the pedestrian corresponds to multiple grids at position A; in the current frame: the pedestrian moves to multiple grids at position B. Therefore, some grids near position A change from occupied to unoccupied; some grids near position B change from unoccupied to occupied; these changed grids are the target change grids. Subsequently, a preset range is expanded outward from the grids corresponding to positions A and B (e.g., ...). This forms a corresponding influence domain. Afterwards, distance propagation updates are only performed within this influence domain. After updating in the manner described in the previous example, the distance values ​​near location B will decrease, while the initially smaller distance values ​​near location A will gradually increase. Grids far from the pedestrian movement area, since the environment has not changed, directly reuse the distance results from the previous frame, thus eliminating the need for recalculation.

[0095] This approach is mathematically equivalent to global wavefront propagation. Its advantage lies in compressing the computation time limit of large-scale ESDF to the millisecond level, which can improve the real-time performance of the system and ultimately output accurate raw distances.

[0096] In one implementation, uncertainty correction is applied to each original distance based on the confidence channel of a dual-channel octree to obtain a conservative distance corresponding to each spatial point. This includes: extracting the confidence of the nearest obstacle corresponding to each spatial point from the confidence channel of the dual-channel octree; taking the negative of the product of a preset confidence attenuation coefficient and the confidence of each nearest obstacle to obtain each negative exponent term; multiplying the result of the exponentiation operation with the natural constant as the base and each negative exponent term as the exponent by a preset nominal depth noise to obtain the depth standard deviation corresponding to each spatial point; multiplying the preset conservative coefficient by the depth standard deviation corresponding to each spatial point to obtain the uncertainty correction term corresponding to each spatial point; and determining the conservative distance corresponding to each spatial point based on the difference between the original distance and the uncertainty correction term corresponding to each spatial point.

[0097] For example, by applying uncertainty correction to the original distance based on the confidence level channel, the calculation expression for the conservative distance is obtained as follows: ; In the formula, Let be the conservative distance of a point in space at time t; The original distance, calculated through wavefront propagation without considering uncertainties; Conservative coefficient (e.g.) ); The nominal depth noise of the TOF camera (e.g.) m); The confidence decay coefficient (e.g.) ); It is a negative exponent term; The confidence level is extracted from the octree node containing the nearest obstacle; This is an uncertainty correction term.

[0098] For example, suppose that in a region of black, light-absorbing material, observations by a TOF camera are extremely unreliable, leading to... If it is extremely low (approaching 0), then the exponential decay term will be... Approaching 1, the penalty term is subtracted. The maximum limit has been reached. If the calculated original distance... The conservative distance for this point is 0.6m. After deducting a penalty (e.g., 0.2m), the distance is... The distance is forcibly reduced to 0.4m. This allows the system to proactively underestimate the safe distance in areas where observations are unreliable, perfectly converting the uncertainty of the sensor hardware into a safety margin at the planning level.

[0099] According to the above implementation method, in areas where the underlying TOF observation is unreliable, the system will subjectively underestimate the safe distance between the robot and obstacles. The ambiguity and uncertainty exhibited by the sensor hardware at the underlying perception stage are equivalently converted into a "physical safety margin" that must be reserved in the upper-level motion planning stage. This can fundamentally prevent collisions caused by the robot blindly trusting noisy point clouds.

[0100] In one implementation, the initial spline trajectory of the mobile robot is optimized based on each spatiotemporal occupancy channel and each conservative distance to generate the target obstacle avoidance trajectory of the mobile robot. This includes: performing curve parameterization on the initial spline trajectory to obtain each initial control point and each trajectory sampling point; for each trajectory sampling point, if the difference between the adaptive safety radius and the conservative distance corresponding to the trajectory sampling point is greater than a preset value, the result of the cube power operation of the difference is determined as the collision penalty term corresponding to the trajectory sampling point; summing the collision penalty terms corresponding to each trajectory sampling point to obtain the target collision cost, and determining the smoothing cost and dynamic cost based on the multi-order derivative integral result and kinematic components corresponding to each trajectory sampling point; weighted summing of the target collision cost, smoothing cost, and dynamic cost based on each cost weight to obtain a comprehensive cost function; using each initial control point as input to a preset optimizer, iteratively updating each initial control point based on the gradient of the comprehensive cost function until a preset convergence condition is met, and outputting each target control point; and solving the preset B-spline basis function based on each target control point to generate the target obstacle avoidance trajectory of the mobile robot.

[0101] For example, the initial spline trajectory p(t) of the mobile robot can be represented as a k-th order uniform B-spline curve, and its calculation expression is as follows: In the formula, Let be the three-dimensional spatial position of the trajectory at time t (i.e., the trajectory sampling point), since The initial control point (which can be understood as the control point to be determined) Composed of linear combinations, before optimization solution For unknown quantities, in each iteration, based on the current... Its value can be calculated for cost assessment; Let i be the i-th control point; The basis functions are k-th order uniform B-spline (e.g., k=4, i.e., cubic uniform B-spline); n is the total number of control points minus one (e.g., ...). That is, the number of control points is ).

[0102] For example, calculate trajectory sampling points The formula for calculating the TOF confidence adaptive safety radius is as follows: In the formula, For adaptive safety radius; The nominal safety radius (e.g., 0.5m); This represents the local average confidence value returned from the spatiotemporal distance field synchronous query near the sampling point location; This is the adaptive gain coefficient (e.g., 0.6).

[0103] For example, a core geometric property of B-spline curves is the "convex hull property," which means that any point on the curve... It must be strictly located within the three-dimensional convex hull formed by its k adjacent control points. Furthermore, the less reliable the TOF observations (…), the more important it is to consider… The smaller the dark field or edge region, the more accurate the system's calculation of the required safety radius. The larger the uncertainty perceived at the bottom layer, the more conservative the obstacle avoidance in the upper layer planning becomes.

[0104] In this example, the calculation expressions for target collision cost, smoothing cost, and dynamic cost are as follows: ; ; ; In the formula, The cost of a collision with the target; The conservative distance between points in space; The smoothing cost is composed of the squared modulo the third derivative of the trajectory; As a cost of kinetics; , These are velocity and acceleration, respectively. , The maximum linear velocity and maximum acceleration corresponding to the physical limits of the robot (e.g.) m / s, ); The duration can be set as needed.

[0105] When the difference between the adaptive safety radius and the conservative distance exceeds a preset value (the preset value is 0, indicating spatial interference), the system does not employ linear penalty. Instead, it constructs a penalty term using a cubic power of the difference. This cubic form releases an exponentially increasing repulsive gradient when the control point approaches the obstacle's danger zone, making the L-BFGS optimizer highly sensitive to potential collisions.

[0106] In this example, the expression for calculating the comprehensive cost function is: ; Among them, all three cost weights are variables that are dynamically adjusted online: ; ; ; In the formula, J is the comprehensive cost function; These are the dynamic weights for collision, dynamics, and smoothing, respectively. For the corresponding basic weight constant (e.g.) , , TTC is the predicted collision time from the robot to the nearest dynamic obstacle (e.g., s); To prevent small values ​​from being divided by zero (such as...) s); These are the corresponding gain or attenuation coefficients, where, For urgency gain (e.g.) ), For speed gain (e.g.) ), For risk decay gain (e.g.) ); For the speed model of the mobile robot; This is a local collision risk index.

[0107] It should be noted that the local collision risk index In the formula, For the mobile robot at trajectory sampling point p(t) corresponding to time t, the conservative distance to the nearest obstacle is obtained by querying the spatiotemporal distance field.

[0108] Under extreme conditions where the robot is traveling at high speed and a pedestrian suddenly darts out in front of it, the predicted collision time (TTC) decreases dramatically and approaches a minimum. (like s). At this point, the collision cost weight The value can be increased tens of times instantly. This dynamic weighting mechanism gives mobile robots a "stress response instinct" similar to humans, prioritizing obstacle avoidance constraints in critical moments (even allowing for a temporary sacrifice of some trajectory smoothness), thus reducing the probability of collisions in real-world operation.

[0109] In one implementation, each initial control point is used as input to a preset optimizer. The initial control points are iteratively updated based on the gradient of the comprehensive cost function until a preset convergence condition is met, and each target control point is output. This includes: using each initial control point as the current control point for the current iteration cycle; updating the position of each current control point based on the gradient of the comprehensive cost function to obtain updated control points; combining the updated control points and constructing corresponding 3D convex hulls for each group of combined control points; if the 3D convex hull intersects with each spatiotemporally occupied pipeline, extracting the updated control point closest to the obstacle from the control point group as the control point to be pruned; projecting the control point to be pruned to a position satisfying a preset safety margin along the opposite direction of the distance gradient corresponding to the control point to be pruned, obtaining pruned control points; using each pruned control point and the unextracted updated control points in the control point group as the current control point for the next iteration cycle, returning to execute position update, combination, construction, extraction, and projection until the preset convergence condition is met, at which point the iteration stops, and each updated control point at the time of stopping iteration is output as the target control point.

[0110] For example, the initial control point is used as the current control point of the current iteration cycle and input into a preset optimizer (such as the L-BFGS quasi-Newton optimizer). The position is updated based on the gradient of the comprehensive cost function, and the active safety pruning operation is performed in this process. The specific implementation process is as follows: First, each initial control point is used as the current control point for the current iteration. Then, based on the gradient of the comprehensive cost function, the position of each current control point is updated to obtain updated control points. Subsequently, the updated control points are combined, and the corresponding 3D convex hulls are constructed for each group of combined control points. .

[0111] At this point, collision checking and pruning are performed: if the 3D convex hull... With each spacetime occupying a conduit If intersecting regions exist, the updated control point closest to the obstacle is extracted from the control point group as the control point to be pruned. and along Project the control point to the nearest location that satisfies the following conditions: (i.e., the opposite direction of the distance gradient to the control point to be pruned) in the direction of the pruning control point. In the formula, These are the control points for pruning. Preset a safety margin (e.g., 0.05m); Control points for pruning At any moment The conservative distance; Control points for pruning At any moment The adaptive safety radius.

[0112] Finally, each pruning control point is used as the current control point for the next iteration cycle. The execution position is updated, combined, constructed, extracted, and projected until the preset convergence condition is met, at which point the iteration stops, and each updated control point at the time of stopping the iteration is output as the target control point.

[0113] For example, the optimizer can determine that the preset convergence condition is met and stop iterating as long as any one of the following sub-conditions is met: First, the gradient magnitude of the comprehensive cost function with respect to the control point is less than the preset gradient threshold ( Second, between two consecutive iterations, the maximum spatial position update of all current control points is less than a preset displacement threshold (e.g., Third, the collision penalty term of the trajectory is completely reduced to zero (i.e., the convex hull and the occupied pipeline no longer intersect, and the pruning operation is no longer triggered), and the relative decrease rate of smoothing and dynamic cost is less than the preset decrease rate (e.g., Fourth, the current iteration count has reached the preset maximum iteration count (e.g., 100 times).

[0114] It should be noted that, for Spatial gradient Calculated by numerical difference, where, To maintain a safe distance Gradient vector in three-dimensional space; This represents the conservative distance corresponding to the spatial point; To maintain a safe distance The partial derivative components along the x-axis of a three-dimensional Cartesian coordinate system; To maintain a safe distance The partial derivative components along the y-axis in a three-dimensional Cartesian coordinate system; To maintain a safe distance The partial derivative components along the z-axis of the Cartesian coordinate system in three-dimensional space; T is the transpose operator of a matrix or vector, used to transpose the row vector formed by the three directional components into a standard column vector to meet the dimensionality requirements of subsequent matrix operations. This is a common technique in this field and will not be repeated here.

[0115] Understandably, traditional planning algorithms typically generate the entire trajectory and then check for collisions afterward, resampling if a collision occurs. Therefore, traditional planning algorithms are inefficient. This invention cleverly utilizes the convex hull property of B-splines, directly performing hard constraint intersection verification between the convex hull and the occupied pipeline at the end of each L-BFGS ladder descent iteration. Once a potential hazard is detected, physical projection "pruning" is immediately performed. This avoids the optimizer getting trapped in local optima (e.g., the trajectory getting stuck inside obstacles). Thus, compared to traditional planning algorithms, this invention improves the reliability and efficiency of collision avoidance.

[0116] For example, the final output calculation expression after optimization and convergence is as follows: ; In the formula, These are the target control points output.

[0117] Using the above implementation method, after the algorithm converges, based on each target control point... By solving the pre-defined B-spline basis functions, the target obstacle avoidance trajectory at any future time t can be obtained. ,speed and acceleration Finally, based on the physical control cycle of the mobile robot chassis (e.g., 50Hz), the continuous target obstacle avoidance trajectory is discretized and sampled to output a precise sequence of motion control commands, thereby driving the chassis to complete a smooth and safe dynamic obstacle avoidance operation.

[0118] Figure 2 This is a structural block diagram of an obstacle avoidance system for a mobile robot according to an embodiment of the present invention.

[0119] like Figure 2 As shown, the obstacle avoidance system of this mobile robot may include: The construction and clustering module 510 is used to construct a dual-channel octree based on the acquired obstacle point cloud and to cluster the obstacle point cloud to obtain various target obstacles; The prediction module 520 is used to predict the occupancy area of ​​each type of target obstacle in future time and space, and obtain each time and space occupancy channel. The conservative distance calculation module 530 is used to generate a spatiotemporal distance field based on the dual-channel octree and each of the spatiotemporal occupancy channels to determine the original distance corresponding to each spatial point, and to perform uncertainty correction on each of the original distances based on the confidence channel of the dual-channel octree to obtain the conservative distance corresponding to each spatial point. The target obstacle avoidance trajectory generation module 540 is used to optimize and solve the initial spline trajectory of the mobile robot based on each of the spatiotemporal occupancy channels and each of the conservative distances, and generate the target obstacle avoidance trajectory of the mobile robot.

[0120] In one implementation, the construction of a dual-channel octree based on the acquired obstacle point cloud is specifically used for: Construct an initial octree centered on the current pose of the mobile robot; For each first octree node in the initial octree that is hit by the obstacle point cloud, the occupancy probability and confidence value corresponding to the first octree node are updated respectively to obtain each updated first octree node. For each second octree node in the initial octree that is penetrated by light, the occupancy probability corresponding to the second octree node is updated to obtain each updated second octree node. From each of the updated first octree nodes and each of the updated second octree nodes, extract the effective occupied nodes whose occupied probability is greater than a preset occupied threshold and whose confidence value is greater than a preset confidence threshold. Based on each of the effectively occupied nodes, the dual-channel octree is constructed.

[0121] In one implementation, the prediction module includes: The state estimation processing unit is used to perform state estimation processing on the observed centroid corresponding to the target obstacle for each type of target obstacle, and to obtain the velocity estimation result, acceleration estimation result and initial state covariance matrix corresponding to the target obstacle; The state deduction unit is used to perform state deduction on the velocity estimation result, the acceleration estimation result, the initial state covariance matrix and the preset state transition matrix for each preset prediction time, so as to obtain the predicted centroid position and the predicted covariance matrix of the target obstacle at the prediction time. A geometric expansion coefficient calculation unit is used to calculate the geometric expansion coefficient of the target obstacle based on the maximum side length in the three-dimensional bounding box of the target obstacle; The region expansion processing unit is used to perform region expansion processing on the target obstacle based on the predicted centroid position, the submatrix corresponding to the position in the predicted covariance matrix, the geometric expansion coefficient, and a preset confidence level threshold, so as to obtain the area occupied by the target obstacle at the predicted future time. The spatiotemporal occupancy pipeline unit is used to generate each spatiotemporal occupancy pipeline based on the occupancy area of ​​each target obstacle at each of the future predicted times.

[0122] In one implementation, the step of generating a spatiotemporal distance field based on the dual-channel octree and each of the spatiotemporal occupancy pipes to determine the original distance corresponding to each spatial point is specifically used for: Based on the current occupancy state of each node in the dual-channel octree and each of the spatiotemporal occupancy channels, a spatial distance field slice corresponding to each prediction time of the current frame is constructed. The spatiotemporal distance field is obtained by integrating the various spatial distance field slices. The distance values ​​corresponding to each grid in the spatiotemporal distance field are initialized to obtain the initial distance corresponding to each grid. The initial distances corresponding to each grid are globally propagated according to the barrel wavefront propagation method to obtain the first distances corresponding to each spatial point. The current grid state corresponding to the current frame is compared with the historical grid state corresponding to the previous frame, and the target change grids whose grid states have changed and the influence domains corresponding to the target change grids are extracted. Based on each of the target change grids and each of the influence domains, local propagation processing is performed on the first distance corresponding to each of the spatial points to obtain the original distance corresponding to each of the spatial points.

[0123] In one implementation, the confidence channel based on the dual-channel octree performs uncertainty correction on each of the original distances to obtain a conservative distance corresponding to each spatial point, specifically used for: Extract the confidence of the nearest obstacle corresponding to each spatial point from the confidence channel of the dual-channel octree; The negative product of the preset confidence decay coefficient and the confidence of each of the nearest obstacles is taken to obtain each negative exponent term; The result of the exponentiation operation with the natural constant as the base and each of the negative exponent terms as the exponent is multiplied by the preset nominal depth noise to obtain the depth standard deviation corresponding to each of the spatial points. The uncertainty correction term for each spatial point is obtained by multiplying the preset conservative coefficient by the depth standard deviation corresponding to each spatial point. Based on the difference between the original distance and the uncertainty correction term corresponding to each spatial point, the conservative distance corresponding to each spatial point is determined.

[0124] In one embodiment, the target obstacle avoidance trajectory generation module includes: The curve parameterization processing unit is used to perform curve parameterization processing on the initial spline trajectory to obtain each initial control point and each trajectory sampling point; The collision penalty term determination unit is used to determine the collision penalty term corresponding to the trajectory sampling point if the difference between the adaptive safety radius and the conservative distance corresponding to the trajectory sampling point is greater than a preset value. The cost determination unit is used to sum the collision penalty terms corresponding to each of the trajectory sampling points to obtain the target collision cost, and to determine the smoothing cost and dynamic cost based on the multi-order derivative integral results and kinematic components corresponding to each of the trajectory sampling points. The comprehensive cost function calculation unit is used to perform a weighted summation of the target collision cost, the smoothing cost, and the dynamic cost based on each cost weight to obtain the comprehensive cost function. The iterative update unit is used to take each of the initial control points as input to the preset optimizer, and iteratively update each of the initial control points based on the gradient of the comprehensive cost function until the preset convergence condition is met, and output each target control point. The solving unit is used to solve the preset B-spline basis function based on each of the target control points to generate the target obstacle avoidance trajectory of the mobile robot.

[0125] In one implementation, the iterative update unit includes: The current control point subunit is used to take each of the initial control points as the current control point of the current iteration cycle; The position update subunit is used to update the position of each current control point based on the gradient of the comprehensive cost function, so as to obtain each updated control point; The combination subunit is used to combine the various update control points and construct the corresponding three-dimensional convex hull for each group of control points obtained by combination. The update control point extraction subunit is used to extract the update control point closest to the obstacle from the control point group as the control point to be pruned if the three-dimensional convex hull intersects with each of the spatiotemporal occupancy channels. The projection subunit is used to project the control point to be pruned to a position that meets a preset safety margin along the opposite direction of the distance gradient corresponding to the control point to be pruned, so as to obtain the pruning control point. The output subunit is used to take each of the pruned control points and the unextracted update control points in the control point group as the current control points of the next iteration cycle, return to perform position updates, combinations, constructions, extractions, and projections until the preset convergence condition is met, then stop the iteration, and output each of the update control points at the time of stopping the iteration as each of the target control points.

[0126] In one implementation, prior to the construction and clustering module, the system further includes: The joint solution module is used to perform joint solution on each first pixel of each original phase image acquired by the time-of-flight camera on the mobile robot, so as to obtain the depth, amplitude and confidence level corresponding to each first pixel. The interference suppression and filtering module is used to perform interference suppression and filtering on each first pixel based on the depth, amplitude and confidence level corresponding to each first pixel to obtain each second pixel; The second pixel projection module is used to project each second pixel onto a preset three-dimensional coordinate system to obtain each three-dimensional point; A global 3D point cloud determination module is used to determine the global 3D point cloud based on each of the 3D points and the confidence level of each of the 3D points; The obstacle extraction module is used to extract obstacles from the global 3D point cloud to obtain an obstacle point cloud.

[0127] In one embodiment, the obstacle extraction module includes: A voxel downsampling unit is used to perform voxel downsampling on the global 3D point cloud to obtain a downsampled point cloud. The outlier removal unit is used to remove outliers from the downsampled point cloud based on the local curvature, local normal deviation and local amplitude deviation of the downsampled point cloud to obtain each candidate 3D point. The hierarchical ground plane fitting unit is used to perform hierarchical ground plane fitting on each of the candidate three-dimensional points based on preset prior constraints and the confidence values ​​corresponding to each candidate three-dimensional point, so as to obtain the target ground plane. The candidate 3D point extraction unit is used to extract candidate 3D points from each of the candidate 3D points whose vertical distance from the target ground plane is greater than a preset height threshold, thereby obtaining the obstacle point cloud.

[0128] The specific functions and examples of each module and submodule of the system in this embodiment of the invention can be found in the relevant descriptions of the corresponding steps in the above method embodiments, and will not be repeated here.

[0129] The acquisition, storage, and application of user personal information involved in the technical solution of this invention all comply with the provisions of relevant laws and regulations and do not violate public order and good morals.

[0130] This invention also provides a computer device, comprising: At least one processor; and a memory communicatively connected to said at least one processor; The memory stores instructions that can be executed by the at least one processor, which, when executed by the at least one processor, enables the at least one processor to perform the method described in any one of the embodiments of the present invention.

[0131] The beneficial effects of the computer device in this embodiment of the invention are equivalent to the beneficial effects of the obstacle avoidance method of the mobile robot described above, and will not be repeated here.

[0132] This invention also provides a non-transitory computer-readable storage medium storing computer instructions, wherein the computer instructions are used to cause a computer to perform the method described in any one of the embodiments of this invention.

[0133] The beneficial effects of the storage medium of the present invention are equivalent to the beneficial effects of the obstacle avoidance method of the mobile robot described above, and will not be repeated here.

[0134] Figure 3 A schematic block diagram of an example computer device 800 that can be used to implement embodiments of the present invention is shown. Computer device 800 is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. Computer device 800 may also represent various forms of mobile devices, such as personal digital assistants, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the invention described and / or claimed herein.

[0135] like Figure 3 As shown, the computer device 800 includes a computing unit 801, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 802 or a computer program loaded from a storage unit 808 into a random access memory (RAM) 803. The RAM 803 may also store various programs and data required for the operation of the computer device 800. The computing unit 801, ROM 802, and RAM 803 are interconnected via a bus 804. An input / output (I / O) interface 805 is also connected to the bus 804.

[0136] Multiple components in computer device 800 are connected to I / O interface 805, including: input unit 806, such as keyboard, mouse, etc.; output unit 807, such as various types of monitors, speakers, etc.; storage unit 808, such as disk, optical disk, etc.; and communication unit 809, such as network card, modem, wireless transceiver, etc. Communication unit 809 allows computer device 800 to exchange information / data with other devices through computer networks such as the Internet and / or various telecommunications networks.

[0137] The computing unit 801 can be various general-purpose and / or special-purpose processing components with processing and computing capabilities. Some examples of the computing unit 801 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various special-purpose artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, a digital signal processor (DSP), and any suitable processor, controller, microcontroller, etc. The computing unit 801 performs the various methods and processes described above, such as obstacle avoidance methods for mobile robots. For example, in some embodiments, the obstacle avoidance method for mobile robots can be implemented as a computer software program tangibly contained in a machine-readable medium, such as storage unit 808. In some embodiments, part or all of the computer program can be loaded and / or installed on the computer device 800 via ROM 802 and / or communication unit 809. When the computer program is loaded into RAM 803 and executed by the computing unit 801, one or more steps of the obstacle avoidance method for mobile robots described above can be performed. Alternatively, in other embodiments, the computing unit 801 may be configured to perform obstacle avoidance methods for a mobile robot by any other suitable means (e.g., by means of firmware).

[0138] Various embodiments of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems-on-a-chip (SoCs), payload-programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include implementations in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor, capable of receiving data and instructions from a storage system, at least one input device, and at least one output device, and transmitting data and instructions to the storage system, the at least one input device, and the at least one output device.

[0139] The program code used to implement the methods of the present invention can be written in any combination of one or more programming languages. This program code can be provided to a processor or controller of a general-purpose computer, special-purpose computer, or other programmable data processing device, such that when executed by the processor or controller, the program code causes the functions / operations specified in the flowcharts and / or block diagrams to be implemented. The program code can be executed entirely on the machine, partially on the machine, as a standalone software package partially on the machine and partially on a remote machine, or entirely on a remote machine or server.

[0140] In the context of this invention, a machine-readable medium can be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, apparatus, or device. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. Machine-readable media can include, but are not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of machine-readable storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0141] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device for displaying information to the user (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor); and a keyboard and pointing device (e.g., a mouse or trackball) through which the user provides input to the computer. Other types of devices can also be used to provide interaction with the user; for example, feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including sound input, voice input, or tactile input).

[0142] The systems and technologies described herein can be implemented in computing systems that include backend components (e.g., as a data server), or computing systems that include middleware components (e.g., an application server), or computing systems that include frontend components (e.g., a user computer with a graphical user interface or web browser through which a user can interact with implementations of the systems and technologies described herein), or any combination of such backend, middleware, or frontend components. The components of the system can be interconnected via digital data communication of any form or medium (e.g., a communication network). Examples of communication networks include local area networks (LANs), wide area networks (WANs), and the Internet.

[0143] Computer systems can include clients and servers. Clients and servers are generally located far apart and typically interact via communication networks. Client-server relationships are created by computer programs running on the respective computers and having a client-server relationship with each other. Servers can be cloud servers, servers in distributed systems, or servers incorporating blockchain technology.

[0144] It should be understood that the various forms of processes shown above can be used to reorder, add, or delete steps. For example, the steps described in this invention can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein.

[0145] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for obstacle avoidance of a mobile robot, characterized by, include: A dual-channel octree is constructed based on the acquired obstacle point cloud, and the obstacle point cloud is clustered to obtain various target obstacles; The occupancy areas of each type of target obstacle in future spacetime are predicted to obtain the spacetime occupancy channels. Based on the dual-channel octree and each of the spatiotemporal occupancy channels, a spatiotemporal distance field is generated to determine the original distance corresponding to each spatial point. Based on the confidence channel of the dual-channel octree, uncertainty correction is performed on each of the original distances to obtain the conservative distance corresponding to each spatial point. Based on the spatiotemporal occupancy channels and the conservative distances, the initial spline trajectory of the mobile robot is optimized and solved to generate the target obstacle avoidance trajectory of the mobile robot.

2. The method of claim 1, wherein, The construction of a dual-channel octree based on the acquired obstacle point cloud includes: Construct an initial octree centered on the current pose of the mobile robot; For each first octree node in the initial octree that is hit by the obstacle point cloud, the occupancy probability and confidence value corresponding to the first octree node are updated respectively to obtain each updated first octree node. For each second octree node in the initial octree that is penetrated by light, the occupancy probability corresponding to the second octree node is updated to obtain each updated second octree node. From each of the updated first octree nodes and each of the updated second octree nodes, extract the valid occupied nodes whose occupied probability is greater than a preset occupied threshold and whose confidence value is greater than a preset confidence threshold. Based on each of the effectively occupied nodes, the dual-channel octree is constructed.

3. The method of claim 1, wherein, The prediction of the occupancy areas of various target obstacles in future spacetime is used to obtain various spacetime occupancy channels, including: For each type of target obstacle, the observed centroid corresponding to the target obstacle is subjected to state estimation processing to obtain the velocity estimation result, acceleration estimation result and initial state covariance matrix corresponding to the target obstacle; For each preset prediction time, state deduction is performed on the velocity estimation result, the acceleration estimation result, the initial state covariance matrix, and the preset state transition matrix to obtain the predicted centroid position and prediction covariance matrix of the target obstacle at the prediction time. Calculate the geometric expansion coefficient of the target obstacle based on the maximum side length in the three-dimensional bounding box of the target obstacle; Based on the predicted centroid position, the submatrix corresponding to the position in the predicted covariance matrix, the geometric expansion coefficient, and the preset confidence level threshold, the target obstacle is subjected to region expansion processing to obtain the area occupied by the target obstacle at the predicted future time. Based on the occupied areas of each of the target obstacles at each of the predicted future times, each of the spatiotemporal occupancy channels is generated.

4. The method of claim 1, wherein, The process of generating a spatiotemporal distance field based on the dual-channel octree and each of the spatiotemporal occupancy pipelines to determine the original distance corresponding to each spatial point includes: Based on the current occupancy state of each node in the dual-channel octree and each of the spatiotemporal occupancy channels, a spatial distance field slice corresponding to each prediction time of the current frame is constructed. The spatiotemporal distance field is obtained by integrating the various spatial distance field slices. The distance values ​​corresponding to each grid in the spatiotemporal distance field are initialized to obtain the initial distance corresponding to each grid. The initial distances corresponding to each grid are globally propagated according to the barrel wavefront propagation method to obtain the first distances corresponding to each spatial point. The current grid state corresponding to the current frame is compared with the historical grid state corresponding to the previous frame, and the target change grids whose grid states have changed and the influence domains corresponding to the target change grids are extracted. Based on each of the target change grids and each of the influence domains, local propagation processing is performed on the first distance corresponding to each of the spatial points to obtain the original distance corresponding to each of the spatial points.

5. The method of claim 1, wherein, The confidence channel based on the dual-channel octree performs uncertainty correction on each of the original distances to obtain the conservative distance corresponding to each spatial point, including: Extract the confidence of the nearest obstacle corresponding to each spatial point from the confidence channel of the dual-channel octree; The negative product of the preset confidence decay coefficient and the confidence of each of the nearest obstacles is taken to obtain each negative exponent term; The result of the exponentiation operation with the natural constant as the base and each of the negative exponent terms as the exponent is multiplied by the preset nominal depth noise to obtain the depth standard deviation corresponding to each of the spatial points. The uncertainty correction term for each spatial point is obtained by multiplying the preset conservative coefficient by the depth standard deviation corresponding to each spatial point. Based on the difference between the original distance and the uncertainty correction term corresponding to each spatial point, the conservative distance corresponding to each spatial point is determined.

6. The method of claim 1, wherein, The process of optimizing the initial spline trajectory of the mobile robot based on each of the aforementioned spatiotemporal occupancy channels and each of the aforementioned conservative distances to generate the target obstacle avoidance trajectory of the mobile robot includes: The initial spline trajectory is subjected to curve parameterization processing to obtain each initial control point and each trajectory sampling point; For each of the trajectory sampling points, if the difference between the adaptive safety radius and the conservative distance corresponding to the trajectory sampling point is greater than a preset value, then the result of the cube power operation of the difference is determined as the collision penalty term corresponding to the trajectory sampling point. The collision penalty terms corresponding to each of the trajectory sampling points are summed to obtain the target collision cost. Based on the multi-order derivative integral results and kinematic components corresponding to each of the trajectory sampling points, the smoothing cost and dynamic cost are determined respectively. The target collision cost, the smoothing cost, and the dynamic cost are weighted and summed based on each cost weight to obtain a comprehensive cost function. Each initial control point is used as input to a preset optimizer. The initial control points are iteratively updated based on the gradient of the comprehensive cost function until the preset convergence condition is met, and each target control point is output. The target obstacle avoidance trajectory of the mobile robot is generated by solving the preset B-spline basis function based on each target control point.

7. The method of claim 6, wherein, The process of using each initial control point as input to a preset optimizer, iteratively updating each initial control point based on the gradient of the comprehensive cost function until a preset convergence condition is met, and outputting each target control point includes: Each of the initial control points is used as the current control point for the current iteration cycle; Based on the gradient of the comprehensive cost function, the position of each current control point is updated to obtain each updated control point; The updated control points are combined, and a corresponding three-dimensional convex hull is constructed for each group of control points obtained by combination. If the three-dimensional convex hull intersects with each of the spatiotemporal occupancy channels, then the update control point closest to the obstacle is extracted from the control point group as the control point to be pruned. Project the control point to be pruned to a position that meets the preset safety margin along the opposite direction of the distance gradient corresponding to the control point to be pruned; Each of the pruned control points and the unextracted update control points in the control point group are used as the current control points for the next iteration cycle. The process of updating, combining, constructing, extracting, and projecting is repeated until a preset convergence condition is met, at which point the iteration stops, and each of the update control points at the time of stopping the iteration is output as the target control points.

8. The method of claim 1, wherein, Before constructing the dual-channel octree based on the acquired obstacle point cloud, the process also includes: For each raw phase image captured by the flying camera on the mobile robot, the first pixel of each raw phase image is jointly solved to obtain the depth, amplitude and confidence level corresponding to each first pixel; Based on the depth, amplitude, and confidence level of each first pixel, interference suppression and filtering are performed on each first pixel to obtain each second pixel; Each of the second pixels is projected onto a preset three-dimensional coordinate system to obtain each three-dimensional point; Based on each of the three-dimensional points and the confidence level of each of the three-dimensional points, a global three-dimensional point cloud is determined. Obstacles are extracted from the global 3D point cloud to obtain an obstacle point cloud.

9. The method according to claim 8, characterized in that, The step of extracting obstacles from the global 3D point cloud to obtain an obstacle point cloud includes: Voxel downsampling is performed on the global 3D point cloud to obtain a downsampled point cloud; Based on the local curvature, local normal deviation and local amplitude deviation of the downsampled point cloud, outliers are removed from the downsampled point cloud to obtain each candidate 3D point. Based on preset prior constraints and the confidence values ​​corresponding to each candidate 3D point, a hierarchical ground plane fitting is performed on each candidate 3D point to obtain the target ground plane; From each of the candidate 3D points, candidate 3D points whose vertical distance from the target ground plane is greater than a preset height threshold are extracted to obtain the obstacle point cloud.

10. An obstacle avoidance system for a mobile robot, characterized in that, include: The construction and clustering module is used to construct a dual-channel octree based on the acquired obstacle point cloud, and to cluster the obstacle point cloud to obtain various target obstacles; The prediction module is used to predict the occupancy area of ​​each type of target obstacle in future time and space, and obtain each time and space occupancy channel; The conservative distance calculation module is used to generate a spatiotemporal distance field based on the dual-channel octree and each of the spatiotemporal occupancy channels to determine the original distance corresponding to each spatial point, and to perform uncertainty correction on each of the original distances based on the confidence channel of the dual-channel octree to obtain the conservative distance corresponding to each spatial point. The target obstacle avoidance trajectory generation module is used to optimize and solve the initial spline trajectory of the mobile robot based on each of the spatiotemporal occupancy channels and each of the conservative distances, and generate the target obstacle avoidance trajectory of the mobile robot.

Images