Robot trajectory optimization method based on timing esdf and rigid trajectory segment pushing

Abstract

This invention discloses a robot trajectory optimization method based on temporal Euclidean Signed Distance Field (ESDF) and rigid trajectory segment propagation, applicable to the field of real-world navigation for mobile robots. The method involves the robot acquiring multi-frame depth observations and corresponding poses in real time via an onboard vision sensor and pose measurement module. The robot's processor aligns the multi-frame point clouds, filters flying points, selects ground features, fuses dynamic and static BEV occupancy, and inflates the robot's body size to generate a temporal Euclidean Signed Distance Field (ESDF). For the predicted waypoints output by the navigation network, continuously indexed collision trajectory segments are identified. A unified propagation normal and safety anchor points are determined based on the penetrating chord vector and obstacle boundary projection. These are then jointly optimized by considering trajectory length, smoothing, and endpoint preservation constraints, outputting a corrected trajectory for the robot's motion controller to execute. This invention reduces local trajectory distortion caused by point-by-point ESDF gradient correction, improving trajectory continuity and executability.

Description

Technical Field

[0001] This invention belongs to the field of mobile robot navigation and trajectory planning technology, and particularly relates to a robot trajectory optimization method based on temporal ESDF and rigid trajectory segment shift. Background Technology

[0002] When wheeled robots, quadruped robots, and other mobile robots operate autonomously in real-world environments such as offices, warehouses, and inspection areas, they typically utilize onboard depth cameras, LiDAR, and pose measurement modules to continuously perceive surrounding obstacles. Local navigation trajectories are then generated and updated in real-time by an onboard processor or an edge computing device communicating with the robot. Real-world environments contain narrow passages, non-convex obstacles, dynamic personnel, and sensor occlusion and measurement noise. Therefore, the navigation trajectory requires collision verification and geometric optimization before being sent to the motion controller.

[0003] Existing end-to-end visual navigation models typically predict future waypoints or control commands based on multimodal sensor data collected by the robot in real time. Due to factors such as the long-tailed distribution of training data, complex unstructured scenes, and the limited learning ability of neural networks to explicit geometric constraints, predicted trajectories may penetrate, graze edges, or fail to meet robot size constraints near physical obstacles. To avoid collisions with the real robot caused by directly sending such trajectories, a trajectory post-processing and safety verification module is usually required at the navigation network output.

[0004] Existing trajectory optimization methods based on Euclidean signed distance fields mostly employ a point-by-point gradient rejection approach. When discrete waypoints enter the interior of physical obstacles, the optimizer updates the coordinates based on the local ESDF gradients at each waypoint. Near obstacle edges, corners, or non-convex boundaries, the correction directions of waypoints within the same penetration trajectory segment may be inconsistent, easily causing local trajectory distortion, oscillations, or getting trapped in local minima. Furthermore, the depth map acquired in real time by the robot may also contain object edge flypoints, ground clutter, and occlusion noise. If directly projected onto the BEV grid, it will affect the obstacle boundary representation of the distance field.

[0005] On the other hand, mobile robot navigation networks typically output temporal trajectories for the next few seconds, but verification based solely on a single frame of local map is insufficient to simultaneously reflect robot motion and dynamic target position changes. Therefore, an optimization method is needed for real-world mobile robots, capable of accessing real-time sensor data, and comprehensively considering the temporal environment, the overall geometric relationship of continuous collision trajectory segments, and the smoothness and executability of the trajectory.

[0006] Existing trajectory planning and optimization algorithms still have limitations in real-world mobile robot trajectory post-processing scenarios. Functional gradient descent methods such as CHOMP and T-CHOMP rely on global ESDF and generate repulsive forces through point-by-point gradients. When the trajectory penetrates deep into obstacles or is located near the edges or corners of obstacles, it is prone to local gradient direction conflicts, trajectory distortion, and local minima. Methods based on B-splines and local geometry guidance, such as EGO-Planner, are easily affected by local constraints in narrow or highly structured environments and have limited adaptability to multiple dynamic obstacles. Methods such as Hybrid A*, which combine graph search with back-end smoothing, have high computational overhead and are difficult to use directly as lightweight post-processing modules for high-frequency outputs of end-to-end navigation networks. Methods that only add anti-collision regularization terms to the end-to-end model training loss lack explicit posterior physical verification and may still cause physical overshoots and penetrations in unseen long-tailed complex scenarios. Therefore, existing methods struggle to simultaneously balance real-time performance, overall geometric constraints of continuous collision segments, and the safety margin of the real robot body. Summary of the Invention

[0007] To address the problems of local trajectory distortion caused by point-by-point ESDF gradient correction, difficulty in covering temporal dynamic changes in single-frame environment representation, and the impact of sensor noise on the distance field topology during real-world mobile robot navigation, this invention provides a robot trajectory optimization method based on temporal ESDF and rigid trajectory segment shifting. This method is applicable to real-time safe optimization of navigation trajectories for wheeled robots, quadruped robots, and other mobile robots in real-world environments.

[0008] This invention receives data from the onboard vision sensor and pose measurement module in real time during the operation of the mobile robot. It constructs a temporal point cloud through multi-frame depth back-projection and pose alignment, and improves the quality of the obstacle point cloud using depth difference filtering and ground clearance height screening. Furthermore, it fuses static obstacle points and dynamic target future occupancy into a BEV grid, and forms a safe ESDF field through distance transformation and robot size inflation. In the trajectory optimization stage, collision waypoints are grouped into continuous collision trajectory segments according to time index continuity. A unified drift normal is obtained using the penetrating chord vector and obstacle boundary projection. The trajectory is then corrected by jointly solving for safe anchor point constraints and smoothing constraints and sent to the robot motion controller.

[0009] This invention is achieved through the following technical solution:

[0010] The first aspect of this invention provides a robot trajectory optimization method based on temporal ESDF and rigid trajectory segment shifting, applied to a mobile robot equipped with a vision sensor and pose measurement module, executed in real time by a robot-on-board processor or communication computing device, comprising: S1. Acquire continuous multi-frame depth maps in real time through a vision sensor; synchronously acquire the pose transformation of each frame relative to the current reference frame through a pose measurement module, back-project the depth maps of each frame into a 3D point cloud and uniformly transform them to the current reference coordinate system to form a temporal point cloud set. S2. Perform depth-difference-based flying point noise filtering on the temporal point cloud set, and combine ground plane fitting and robot passage height range to filter effective obstacle points; S3. Project the effective obstacle points onto the BEV grid to form a static occupancy map, and fuse the dynamic occupancy map obtained from the dynamic target prediction. Generate a future spatiotemporal occupancy map by pixel-by-pixel fusion, perform Euclidean distance transformation on it and combine it with robot size expansion to obtain the expanded temporal ESDF field. S4. Map the future predicted waypoint sequence output by the end-to-end navigation network to the temporal ESDF field for distance sampling, obtain the collision waypoint set and merge it into at least one continuous collision segment; S5. Construct a penetration chord vector for the entry and exit points of the continuous collision segment, and determine a unified pushing normal based on the direction orthogonal to it; then determine the target safety anchor point and verify the pushing direction by combining the obstacle boundary projection relationship; S6. Construct an optimization function that includes rigid traverse constraints, trajectory length constraints, second-order smoothness constraints, and origin-endpoint preservation constraints. Iterate and update the predicted waypoint sequence to output an interference-free and smooth trajectory.

[0011] Specifically, the depth map in step S1 is obtained directly by a depth camera, or by inference from a depth estimation model based on a real-time image captured by an RGB camera mounted on a robot.

[0012] The pose transformation is acquired by a pose measurement module, which includes a visual odometry and / or a laser odometry. The visual odometry estimates the pose transformation of the mobile robot based on the image acquired by the visual sensor, and the laser odometry estimates the pose transformation of the mobile robot based on the point cloud data acquired by the lidar on the mobile robot.

[0013] The step of backprojecting the depth maps of each frame into 3D point clouds and uniformly transforming them to the current reference coordinate system to form a temporal point cloud set specifically includes: based on the pinhole camera model, converting the pixel coordinates, depth values, and camera intrinsic parameters in the depth maps of each frame into 3D points in the camera coordinate system to obtain the 3D point clouds of each frame; and according to the pose transformation, uniformly transforming the 3D point clouds of each frame to the current reference coordinate system to form the temporal point cloud set.

[0014] Further, step S2 specifically includes: By designing a differential difference filtering operator, the depth difference between adjacent pixels is obtained to ensure that the subsequent two-dimensional bird's-eye view BEV projection is not contaminated by noise. Therefore, let the depth difference between adjacent pixels in the image domain be: ; ; in, This represents the depth difference between pixel coordinates (u, v) and its horizontally adjacent pixels. This represents the depth difference between pixel coordinates (u,v) and its vertically adjacent pixels. This represents the depth value of the depth map at pixel coordinates (u,v) in frame t, where u represents the pixel index in the horizontal direction of the image and v represents the pixel index in the vertical direction of the image. When the depth difference between adjacent pixels exceeds a preset depth mutation threshold, the corresponding 3D point is removed; when the depth map has a confidence estimate, outlier points are further filtered based on the confidence threshold. To effectively separate passable ground from actual obstacles, the RANSAC algorithm or least squares algorithm is used for ground plane fitting in a unified reference coordinate system to remove ground clutter and overhead suspended objects that have no substantial impact on passage; the algorithm expression is as follows: ; Where a, b, and c are the parameters of the fitted ground plane equation, a represents the slope coefficient in the x-direction, b represents the slope coefficient in the z-direction, and c represents the ground height intercept; x and z represent the horizontal coordinates in the unified reference coordinate system. For any valid point in the point cloud The ground height at its corresponding location is Further calculate the relative vertical height of this point with respect to the ground: ; in, This represents the relative vertical height of the valid point with respect to the fitted ground. This indicates the vertical coordinates of the valid point in a unified reference coordinate system. This indicates the fitted ground height at the horizontal position corresponding to the valid point; Subsequently, physical constraint properties of the autonomous vehicle / robot are introduced, namely, the maximum chassis clearance height threshold that is allowed to be crossed. and the effective height of the robot's roof If the height attribute of a point satisfies: ; The point is then identified as being within the height range of obstacles of interest to the system and marked as a valid static obstacle point.

[0015] Further, in step S3, effective obstacle points are projected onto the BEV grid to form a static occupancy map, and the dynamic occupancy map obtained from dynamic target prediction is fused together, and a future spatiotemporal occupancy map is generated through pixel-by-pixel fusion; specifically including: The filtered effective static obstacle points are orthogonally projected along the gravity direction onto the two-dimensional bird's-eye view BEV grid map. Based on the comparison between the cumulative number of obstacle points mapped in each grid and the preset confidence threshold, the corresponding grid is marked as statically occupied, and a static occupancy map is obtained. The multi-target detection and tracking results of the perception front end in future frames are obtained, the three-dimensional position of each dynamic target in future frames is determined, the motion heading angle of the dynamic target is calculated according to the instantaneous displacement across frames, and the geometric size is preset according to the category of the dynamic target. The corresponding dynamic occupancy area is constructed in the BEV plane, and the dynamic occupancy area is rotated and projected according to the motion heading angle to obtain the dynamic occupancy map. The static occupancy map and the dynamic occupancy map are fused pixel by pixel maximum value to generate a future spatiotemporal occupancy map.

[0016] Further, in step S3, an Euclidean distance transformation is performed on the future spatiotemporal occupancy map and combined with robot size dilation to obtain the dilated temporal ESDF field; specifically including: The future spatiotemporal occupancy map is obtained by fusing the static occupancy map and the dynamic occupancy map pixel-by-pixel maximum value, wherein the pixel value of each pixel is used to characterize whether the pixel position is an area occupied by an obstacle; Perform Euclidean distance transformation on the future spatiotemporal occupancy map to calculate the lower bound distance from the free space region to the boundary of the nearest obstacle-occupied region, and the intrusion distance from the interior of the obstacle-occupied region to the nearest free space boundary. Based on the difference between the lower bound distance and the intrusion distance, a symbolic Euclidean distance field (ESDF) is constructed. In the symbolic ESDF, a positive distance value indicates that the location is an area occupied by an obstacle or an interference area, while a negative distance value indicates that the location is a safe passage area. Using the robot size as the expansion radius, an expansion operation is performed on the symbolic ESDF to subtract the robot size from the distance value in the symbolic ESDF, thereby obtaining the expanded temporal ESDF field.

[0017] Furthermore, step S5 specifically includes: For a selected continuous collision segment, a penetration chord vector is constructed based on the starting and ending waypoints of the continuous collision segment. The penetration chord vector is then normalized, and two candidate thrust normals with opposite directions are constructed based on the direction of the penetration chord vector. Based on the boundary information of the obstacles intersecting with the continuous collision segment, the boundary points of the obstacles are projected onto the candidate push normal direction, and the geometric centroid of the continuous collision segment is projected onto the candidate normal direction. Based on the relative positional relationship between the centroid projection and the extreme value range of the obstacle boundary projection, the target safety anchor point is selected and the direction of the candidate push normal direction is verified. The verified push normal direction is used as the unified push normal direction of the continuous collision segment to perform overall rigid push of the continuous collision segment.

[0018] Further, step S6 specifically includes: A rigid push loss function is constructed for continuous collision trajectory segments to measure the out-of-bounds deviation of the continuous collision segments relative to the target safety boundary in the unified push method direction, so that each waypoint in the continuous collision segment completes a cooperative correction in the unified push method direction; the push loss of each continuous collision segment is summarized to obtain the overall collision loss. Construct a trajectory length constraint function to suppress trajectory detours and local redundancy; construct a second-order trajectory smoothness constraint function to suppress curvature abrupt changes and local oscillations; construct a start and end point constraint function to constrain the drift of the optimized trajectory's start and end points relative to the original start and end points. The overall collision loss, the trajectory length constraint function, the trajectory second-order smoothing constraint function, and the origin and end point constraint functions are weighted and then aggregated to obtain the overall optimization objective function. The overall objective function is solved iteratively using Adam, L-BFGS, or other first-order optimizers. When the convergence condition is met, the optimized trajectory is output.

[0019] A second aspect of the present invention: a robot trajectory optimization device based on temporal ESDF and rigid trajectory segment shifting, the device comprising the following modules: The sensor data interface and time-series point cloud recovery module acquires continuous multi-frame depth maps in real time through a vision sensor; the pose measurement module synchronously acquires the pose transformation of each frame relative to the current reference frame, backprojects the depth maps of each frame into a 3D point cloud and uniformly transforms it to the current reference coordinate system to form a time-series point cloud set. The point cloud filtering and ground screening module performs depth difference-based flying point noise filtering on the temporal point cloud set, and combines ground plane fitting and robot passage height range to screen effective obstacle points; The static and dynamic BEV occupancy fusion module projects effective obstacle points onto the BEV grid to form a static occupancy map, and fuses the dynamic occupancy map obtained from dynamic target prediction. The resulting map is then fused pixel by pixel to generate a future spatiotemporal occupancy map. The dilated ESDF generation module performs Euclidean distance transformation on it and combines it with robot size dilation to obtain the dilated temporal ESDF field; The continuous collision segment identification module maps the future predicted waypoint sequence output by the end-to-end navigation network to the temporal ESDF field for distance sampling, obtains a set of collision waypoints and merges them into at least one continuous collision segment. The module for determining the unified pushing normal and safety anchor point constructs a penetrating chord vector for the entry and exit points of the continuous collision segment, and determines the unified pushing normal based on the direction orthogonal to it; then, it combines the obstacle boundary projection relationship to determine the target safety anchor point and verify the pushing direction. The multi-objective trajectory optimization module constructs an optimization function that includes rigid traverse constraints, trajectory length constraints, second-order smoothness constraints, and origin-endpoint preservation constraints. Iteratively updates the predicted waypoint sequence and outputs an interference-free and smooth trajectory.

[0020] A third aspect of the present invention provides an electronic device disposed on or in communication with a mobile robot, comprising one or more processors and a memory; the memory is used to store one or more programs, which, when executed by the one or more processors, enable the processors to perform a robot trajectory optimization method based on sensor data acquired in real time by the mobile robot.

[0021] A fourth aspect of the present invention provides a computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the trajectory optimization method described above.

[0022] The beneficial effects of this invention are as follows: 1) Continuous collision trajectory segments are optimized as a whole object to avoid the influence of local gradients caused by directional conflicts on adjacent waypoints; 2) Temporal ESDF is fused with future dynamic occupancy to improve the forward-looking obstacle avoidance capability for moving obstacles; 3) Flypoint filtering and ground screening reduce the damage of noise to the BEV distance field; 4) The combination of rigid trajectory segment shifting and endpoint holding constraints makes the output trajectory safe, continuous and smooth executable. Attached Figure Description

[0023] Figure 1 This is a schematic diagram of the trajectory optimization iterative process of the present invention; Figure 2 This is a comparative diagram of the present invention and the traditional ESDF point-by-point optimization method; Figure 3 This is a comparison chart of the actual trajectory optimization of the method of this invention and the traditional ESDF method on the TurtleBot wheeled robot and the Go2 quadruped robot. Detailed Implementation

[0024] The present invention will be described below with reference to the accompanying drawings and specific embodiments. These embodiments are used to explain the technical solutions of the present invention and should not be construed as limiting the scope of protection.

[0025] In one embodiment, the trajectory safety post-processing module of the present invention, serving as the output of an end-to-end navigation network for a real mobile robot, can be deployed on the robot's local processor or on an edge computing device connected to the robot via wired or wireless communication. This module receives data in real time from the robot's onboard vision sensors, pose measurement module, and dynamic target detection and tracking module. Using the future discrete waypoints output by the navigation network as the optimization target, it sends the corrected safe trajectory to the robot's motion controller.

[0026] The technical chain of this invention can be summarized into the following five steps, as detailed below:

[0027] Step 1: Temporal point cloud reconstruction and alignment based on multi-frame depth and pose To construct a collision detection space that reflects the real environment around the robot and its temporal changes, this method continuously acquires multiple frames of depth maps using the robot's onboard depth camera and simultaneously reads pose information. In the implementation using an RGB camera, a real-time depth estimation model can generate the corresponding depth map based on the camera images.

[0028] Let the system use the current time as reference time 0, and let D be the depth map of the t-th frame within the time series observation window. t The camera intrinsic parameter matrix is ​​denoted as K. t For any pixel coordinates (u, v) in the image, according to the pinhole camera model, the depth scalar at that pixel can be back-projected into a three-dimensional spatial point vector in the camera's local coordinate system c: ;

[0029] in, This represents the 3D point in frame t whose pixel coordinates (u,v) are back-projected onto the camera's local coordinate system c; c represents the camera's local coordinate system; and t represents the frame number within the time-series observation window. This represents the depth value of the depth map at pixel coordinates (u, v) in frame t; u is the horizontal pixel index of the image, and v is the vertical pixel index of the image; K t Let represent the camera intrinsic parameter matrix for frame t. In this formula, K represents t The inverse matrix; This represents the homogeneous coordinate vector of the corresponding pixel.

[0030] Therefore, the dense three-dimensional point cloud set corresponding to the first frame within the time-series observation window can be recovered.

[0031] To eliminate coordinate system offset caused by robot motion, this step utilizes the rigid body pose transformation matrix of each time frame relative to the current reference frame to transform and unify the point cloud of each frame into the current reference coordinate system: ; in, This indicates a transformation to a 3D point in the current reference coordinate system. This represents the rigid body pose transformation matrix from the camera coordinate system in frame t to the current reference coordinate system. 0 indicates the current reference time. The homogeneous coordinate form is used to represent both rotation and translation transformations.

[0032] After temporal aggregation, a temporal point cloud set for trajectory optimization is obtained under a unified reference coordinate system: ;

[0033] Where P represents the temporal point cloud set in a unified reference coordinate system, P t Let N represent the set of points after the point cloud transformation in frame t, N represent the number of frames included in the time series observation window, and the union of sets represents the time series aggregation of point clouds from multiple frames.

[0034] Step 2: Flypoint noise suppression and ground travel height screening based on differential filtering

[0035] Because the depth map is prone to flying point noise (interpolation jumps) at the boundaries between foreground objects and distant backgrounds, a differential difference filtering operator is designed in this step to ensure that the subsequent BEV projection is not contaminated by noise. Let the depth difference between adjacent pixels in the image domain be: ; ; in, This represents the depth difference between pixel coordinates (u, v) and its horizontally adjacent pixels. This represents the depth difference between pixel coordinates (u,v) and its vertically adjacent pixels. This represents the depth value of the depth map at pixel coordinates (u, v) in frame t. The system will then... and Compared with the preset deep mutation threshold If any direction exceeds a threshold, the 3D point recovered from that pixel is marked as a boundary flying point and discarded. For depth maps with confidence estimates, depth confidence levels below a threshold are also removed. The points are filtered together to reduce the impact of boundary flying points on subsequent BEV projections.

[0036] Furthermore, to effectively separate passable ground from real obstacles, point clouds are analyzed under a unified reference coordinate system. Perform planar model fitting (using RANSAC algorithm with interior point thresholding or least squares method): ; Where a, b, and c are the parameters of the fitted ground plane equation, a represents the slope coefficient in the x-direction, b represents the slope coefficient in the z-direction, and c represents the ground height intercept; x and z represent the horizontal coordinates in the unified reference coordinate system.

[0037] Therefore, for any valid point in the point cloud The ground height at its corresponding location is Calculate the vertical height of this point relative to the ground: ; Among them, y rel The vertical height difference between the point and the fitted ground is represented by y, where y represents the vertical coordinate of the point in the unified reference coordinate system. plane This represents the fitted ground height at the corresponding horizontal position.

[0038] Subsequently, the robot's physical constraint property is introduced: the maximum chassis height threshold that it is allowed to traverse. and the effective height of the robot's roof If the height attribute of a point satisfies: ; This indicates that the point is located within the height range of obstacles of primary concern to the system and is marked as a valid static obstacle point. This step helps to eliminate ground clutter and high-altitude suspended objects that do not substantially affect passage, thereby improving the quality of environmental mapping.

[0039] Step 3: Fusion and Expansion of Future Spatiotemporal Dynamic and Static BEV Occupancy Maps to Generate ESDF

[0040] The filtered effective static obstacle points are orthogonally projected along the gravity direction (Y-axis) onto the 2D bird's-eye view (BEV) grid map. The effective perception coverage of the BEV plane is set as follows. The physical resolution of the raster is Then the spatial coordinates of the point The corresponding BEV raster 2D index quantization formula can be expressed as: ; If the cumulative number of obstacle points mapped within a certain grid exceeds a preset confidence threshold, then that grid is marked as statically occupied, and this is recorded as a binary state. .

[0041] Where r represents the physical resolution of the BEV raster, and u and v represent the column index and row index of the BEV raster, respectively. This indicates the static occupancy status of the grid corresponding to frame t. A value of 1 indicates that the grid has a static obstacle, and a value of 0 indicates that it is not occupied by a static obstacle.

[0042] To accurately capture dynamic threats in future moments, this method explicitly integrates temporal prediction of dynamic obstacles. Based on the multi-target detection and tracking results of the sensing front-end in future frames, the center of the two-dimensional detection box of dynamic targets such as pedestrians and vehicles in the image is obtained, and their three-dimensional position is recovered by combining depth backprojection. Suppose a tracked dynamic target is in the future... The geometric center of the BEV in the frame is located at Its instantaneous displacement across frames is Then its heading angle can be expressed as: ; Preset corresponding length and width geometric dimensions (e.g., pedestrian size envelope) according to different target categories. Standard vehicle size envelope Construct a corresponding dynamically occupied solid rectangular region in the BEV plane, and according to its heading angle Perform rotational rendering projection to obtain a dynamically occupies a mask image. .

[0043] Finally, the static and dynamic occupancy maps are fused pixel-by-pixel maximum values ​​to form a temporal occupancy map that includes future spatiotemporal situations: ; The system then performs an efficient two-dimensional Euclidean distance (DT) transformation on the time-series occupancy graph. The following logical rules are defined: Indicates high-risk obstacle areas; It represents a safe and free space.

[0044] Calculate the lower bound distance from free space to the nearest obstacle boundary. And the intrusion distance from inside the obstacle to the nearest free space. Thus, the complete symbolic ESDF field is defined: ; The aforementioned distance values ​​essentially constitute a continuously changing spatial safety distribution map. In the algorithm flow, the system first reduces the 3D temporal point cloud to a bird's-eye view grid, and then generates a continuous ESDF distance field through Euclidean distance transformation. According to the notation convention, a distance value less than 0 indicates a safe passage area, while a distance value greater than 0 indicates the interior of an obstacle or a dangerous interference area.

[0045] To avoid the time-consuming collision and intersection calculations of the robot's polygonal contour during trajectory optimization iterations, this method employs an environment dilation strategy. This considers the robot's geometric and physical dimensions (length...) ,Width ), calculate its equivalent maximum circumscribed radius. (Converted to raster scale). A scalar offset correction is applied to the original ESDF field to obtain the final, dilated safe distance field: ; in, This represents the safety expansion radius calculated from the robot's physical dimensions, where L and W represent the robot's length and width, respectively, and r represents the grid physical resolution. This represents the timing ESDF value after the robot's size expansion is complete.

[0046] This step uses environmental expansion to map the robot, which has actual physical dimensions, into a simplified point mass model, thereby implicitly incorporating the robot's geometric constraints into the distance field distribution. This is beneficial for improving the efficiency of subsequent high-frequency collision detection and table lookup calculations.

[0047] Step 4: Orthogonalization of String Vectors Based on Continuity Recognition and Unified Transfer of Rigid Trajectory Segments The future predicted waypoint sequence output by the end-to-end navigation network feedforward is denoted as: ; ;

[0048] Each discrete waypoint is mapped to the BEV physical grid scale, and bilinear interpolation sampling is performed in the expanded ESDF field constructed above to obtain the continuous collision distance metric of the waypoint: ; Where T represents the sequence of future predicted waypoints output by the navigation network, M represents the total number of waypoints, and x i This represents the i-th waypoint vector, which includes the x-coordinate, z-coordinate, and orientation angle θ in the BEV plane. i ;s i This represents the sampling distance of the i-th waypoint in the expanded ESDF field. Let represent the expanded ESDF field corresponding to the i-th prediction time.

[0049] This leads to the identification of a set of collision waypoints with a high risk of physical interference. .

[0050] Extracting Consecutive Collision Segments: This method utilizes tensor discrete difference techniques (such as calling the torch.diff operator) to analyze the continuity of collision waypoints in the time series index. If several consecutive collision points have a sequence index step size of 1, they are grouped into the same physical penetration group, thus dividing them into several independent "consecutive collision segments": ; ; Among them, S k Let s represent the k-th consecutive collision segment. k and e k These represent the starting entry index and the ending exit index of the continuous collision segment, respectively, and K represents the total number of continuous collision segments. In practical implementation, when there are multiple discrete continuous collision segments, the penetration chord length of each segment can be further calculated, and segments with larger chord lengths or higher interference levels can be processed first.

[0051] Constructing a unified penetration normal: For a selected continuous collision segment, select the boundary vector at the intersection of its entry and exit from the obstacle, and construct a physical chord vector representing its overall penetration tendency: ;

[0052] To eliminate the influence of distance scale, the penetration chord vector is normalized using the L2 norm to obtain the direction vector. Based on the geometric projection relationship of the Separation Axis Theorem (SAT), to ensure that continuous collision trajectory segments move out of the obstacle region with minimal geometric deformation, the direction orthogonal to the penetration chord trend is designated as the candidate displacement normal. Therefore, through clockwise and counterclockwise rotation mapping in a two-dimensional plane, two candidate unified displacement normals with opposite directions are constructed: ; Among them, c k This represents the penetration chord vector of the k-th consecutive collision segment. Let n represent the components of the chord vector in the x and z directions of the BEV plane, respectively. k Indicates c k Orthogonal candidate unified shift normal.

[0053] This unified thrust normal provides a consistent correction direction for the selected continuous collision trajectory segment, transforming subsequent optimization from dispersed corrections at multiple waypoints into a rigid thrust of the entire trajectory segment.

[0054] Target Anchoring and Direction Verification: To determine which side of the obstacle a continuous collision segment should exit from, this method combines obstacle enclosure geometry with projection analysis. By tracing the dominant entity ID corresponding to the interference region, an approximate set of directed bounding box corner points of the intersecting obstacles can be obtained. These boundary points are projected onto a determined unified normal, and the projection value of the trajectory segment's geometric centroid on the same normal is calculated. By comparing the relative positional relationship between the centroid projection and the extreme value range of the obstacle boundary projection, the shorter and more reasonable boundary side can be selected as the target safety anchor point, and the direction of the unified normal is verified accordingly.

[0055] Step 5: Multi-objective functional energy optimization and joint kinematic constraints

[0056] After determining the correction direction, the system constructs a multi-objective joint optimization loss function.

[0057] First, construct the overall rigidity shift loss for continuous collision trajectory segments: ; in, S represents the overall rigidity displacement loss of the k-th consecutive collision segment. K Indicates the number of waypoints within this continuous collision segment, x i Let a represent the vector of the i-th waypoint within the segment. k n represents the target safety anchor point determined based on the obstacle boundary projection. k This represents the unified displacement normal after direction verification, (·). T This represents the vector transpose. This formula measures and constrains the overall out-of-bounds deviation of consecutive collision segments relative to the target safety boundary in a unified normal direction, ensuring that waypoints within the same collision segment undergo coordinated correction in a consistent direction during optimization. All segmented collision constraints can be further summarized into a total collision loss: ; Among them, L coll This represents the total collision loss across all consecutive collision segments, where K represents the total number of consecutive collision segments.

[0058] In addition to collision avoidance, the system strictly ensures the dynamic rationality of the corrected trajectory in the time domain.

[0059] Trajectory length constraints are used to suppress unnecessary detours and local redundancy, and can be represented by a first-order difference Euclidean distance accumulation form: ;

[0060] The second-order smoothing constraint on the trajectory is used to suppress abrupt curvature changes, local oscillations, and excessive dynamic variations. It can be approximated using the second-order difference Laplace operator. ;

[0061] Start and end point constraints are used to prevent significant drift of the start or end point due to excessive obstacle avoidance correction during the optimization process. ; Among them, L len L represents the trajectory length constraint. smooth L represents the second-order smoothing constraint of the trajectory. anchor Indicates origin and destination endpoint constraints; M represents the total number of waypoints, x start and x goal Let represent the starting point and the target ending point of the original trajectory, respectively, and let ||·|2 represent the L2 norm. Finally, the energy terms are aggregated according to normalized weights to form the overall optimization objective: ; Where L represents the overall optimization objective, λ c , λ s , λ l , λ a These are the non-negative weights for collision loss, smoothing loss, length loss, and endpoint preservation loss, respectively.

[0062] By employing first-order optimizers with adaptive learning rate characteristics, such as Adam, to jointly solve the above objective function, and iterating until the convergence condition is met, an output trajectory with reduced collision risk, smoothness, and executability can be obtained.

[0063] This embodiment uses a server configured with an RTX 4090 GPU as the computing device for communication with the robot in a real indoor obstacle scenario, and the Unitree Go2 quadruped robot as the execution platform to illustrate the process from real-time sensor data acquisition and collision risk identification to trajectory correction and control deployment. During operation, Go2 acquires environmental data through an onboard depth camera and Unitree L1 LiDAR, and obtains its pose through visual odometry or laser odometry. The server receives the aforementioned real-time data and the local trajectory for the next 3.0 seconds output by the end-to-end navigation model, which includes 24 two-dimensional waypoints. Figure 1 As shown, when the initial trajectory interferes with a physical obstacle, the trajectory optimization process of this invention is initiated.

[0064] Phase 1: Environmental Topology Map Reconstruction and Field Establishment Multimodal data access and coordinate inversion: Intrinsic parameter information from the Go2's onboard depth camera is subscribed to from the onboard ROS middleware, and combined with the Unitree L1 LiDAR as an auxiliary depth verification source. Simultaneously, the high-frequency relative pose matrix output by the visual / laser odometry (VIO / LIO) is read. GPU parallel computing is used to substitute the effective pixels of the depth map into the inversion formula to reconstruct a 3D point cloud in the robot's local coordinate system.

[0065] Point cloud filtering and ground selection: A mask difference operation is performed on the point cloud matrix to remove isolated noise points with obvious abrupt changes in depth gradient; and the RANSAC algorithm is used to fit the ground plane model. Then, the directed distance from each 3D point to the plane is calculated, and combined with the chassis clearance height and overall shape parameters of the Yushu Go2, only points within the set obstacle height range are retained to construct an effective obstacle set.

[0066] Occupation Construction and Distance Field Generation: The filtered obstacle point cloud is projected onto a 2D ground plane to establish a BEV grid matrix with a physical resolution of 0.05 m / grid. Then, a CUDA-accelerated Euclidean distance transform operator is used to generate an ESDF matrix, and a corresponding safety expansion radius is set based on the Go2 entity size to form a safe distance field with conservative collision margin.

[0067] Phase Two: Interference State Detection and Structural Slicing Trajectory distance sampling: The coordinates of the 24 waypoints output by the network are converted into row and column indices in a raster matrix, and bilinear interpolation sampling is performed in the expanded safe distance field to obtain a distance array of length 24.

[0068] Continuous collision segment segmentation: All trajectory point indices exhibiting collision interference are filtered out from GPU memory. For example, when waypoints corresponding to indices 15, 16, 17, and 18 all collide, the `torch.diff` operator in PyTorch can be used to calculate the step size between adjacent collision indices; when the step size is 1, these points are merged into the same continuous collision segment. If the trajectory exhibits multiple separate collision segments due to complex obstacle shapes, each segment can be modeled separately and processed according to priority.

[0069] Phase Three: Establishing the Target Safety Anchor Point and the Unified Moving Normal Unified normal vector analysis: For continuous collision segments with high penetration, the entry and exit endpoints are extracted, and the difference between them is used to construct a chord vector representing the overall penetration trend, which is then normalized. Subsequently, a 90-degree rotation is performed on this direction to obtain the basic normal vector orthogonal to the chord vector.

[0070] Obstacle entity association: Identify the obstacle entity labels that caused the interference within the BEV region corresponding to the collision segment, and extract the directed bounding box or other boundary representation of the entity in the environmental post-processing results.

[0071] Adaptive extreme value projection orientation: Calculate the projection value of the geometric center of the collision trajectory segment onto the base normal and compare it with the projection distribution of obstacle boundary points on the same normal. Select the boundary position with the shorter distance and located outside the obstacle as the target safety anchor point. If the initial normal points into the obstacle, the normal direction is flipped for correction.

[0072] Multi-objective optimization solution: The Adam optimizer in PyTorch is used to iteratively update the differentiable trajectory coordinate sequence. Optimization objectives include: rigid migration loss of continuous collision trajectory segments based on a unified migration normal and safety anchors, first-order difference length constraints, second-order smoothness constraints, and L2 preservation constraints on the start and end positions. By adjusting the weights of each component, a balance can be achieved between obstacle avoidance performance and trajectory smoothness.

[0073] Boundary control and iteration termination: To prevent waypoints from going out of bounds due to excessively large corrected gradients, the gradient norm can be clipped using torch.nn.utils.clip_grad_norm_ before each parameter update; then the optimizer is updated, and the final corrected trajectory is output after the maximum number of iterations or the loss convergence condition is reached.

[0074] The actual deployment and method are compared as follows:

[0075] To verify the applicability of this invention to different mobile robot bodies, the TurtleBot wheeled robot and the Go2 quadruped robot were deployed in an indoor scene with solid cylindrical obstacles and cardboard boxes. During operation, both types of robots acquired real-time observations and pose information from their respective onboard sensors and set ESDF safety expansion ranges according to their respective body dimensions; the trajectory optimization results were sent to the corresponding robot's motion control interface.

[0076] like Figure 2 As shown, the top row is the TurtleBot wheeled robot, and the bottom row is the Go2 quadruped robot; Figure 2 The left sub-figure shows the optimization results obtained by this invention based on the rigid shifting of continuous collision trajectory segments. Figure 2 The right subgraph shows the result of point-by-point optimization using traditional ESDF. Figure 3 The image simultaneously indicates the original trajectory, the optimized trajectory, and the location where collisions occurred using the traditional method.

[0077] exist Figure 3 Under the test conditions shown, the method of this invention treats the continuous trajectory segment interfering with the obstacle as a whole, moving it along a uniform pushing normal to the side of the safe anchor point. The optimized trajectory can bypass the physical obstacle while maintaining the continuous shape of the trajectory segment. The traditional ESDF method corrects each collision waypoint separately based on local gradients, and the optimized trajectory still interferes with the obstacle or the robot's expansion envelope. This real-world comparison illustrates the feasibility of this invention on wheeled and quadruped robots and its trajectory geometry effect compared to the point-by-point correction method.

[0078] The present invention also discloses a robot trajectory optimization device based on temporal ESDF and rigid trajectory segment shift, the device comprising the following modules: The sensor data interface and time-series point cloud recovery module acquires continuous multi-frame depth maps in real time through a vision sensor; the pose measurement module synchronously acquires the pose transformation of each frame relative to the current reference frame, backprojects the depth maps of each frame into a 3D point cloud and uniformly transforms it to the current reference coordinate system to form a time-series point cloud set. The point cloud filtering and ground screening module performs depth difference-based flying point noise filtering on the temporal point cloud set, and combines ground plane fitting and robot passage height range to screen effective obstacle points; The static and dynamic BEV occupancy fusion module projects effective obstacle points onto the BEV grid to form a static occupancy map, and fuses the dynamic occupancy map obtained from dynamic target prediction. The resulting map is then fused pixel by pixel to generate a future spatiotemporal occupancy map. The dilated ESDF generation module performs Euclidean distance transformation on it and combines it with robot size dilation to obtain the dilated temporal ESDF field; The continuous collision segment identification module maps the future predicted waypoint sequence output by the end-to-end navigation network to the temporal ESDF field for distance sampling, obtains a set of collision waypoints and merges them into at least one continuous collision segment. The module for determining the unified pushing normal and safety anchor point constructs a penetrating chord vector for the entry and exit points of the continuous collision segment, and determines the unified pushing normal based on the direction orthogonal to it; then, it combines the obstacle boundary projection relationship to determine the target safety anchor point and verify the pushing direction. The multi-objective trajectory optimization module constructs an optimization function that includes rigid traverse constraints, trajectory length constraints, second-order smoothness constraints, and origin-endpoint preservation constraints. Iteratively updates the predicted waypoint sequence and outputs an interference-free and smooth trajectory.

[0079] Accordingly, this application also provides an electronic device, including: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the robot trajectory optimization method based on temporal ESDF and rigid trajectory segment shift as described above. In the hardware structure of any data processing device in which the robot trajectory optimization device based on temporal ESDF and rigid trajectory segment shift provided in this embodiment of the invention is located, in addition to the processor, memory, and network interface, the data processing device in the embodiment may also include other hardware depending on the actual function of the data processing device, which will not be elaborated further.

[0080] Accordingly, this application also provides a computer-readable storage medium storing computer instructions that, when executed by a processor, implement the robot trajectory optimization method based on temporal ESDF and rigid trajectory segment propagation as described above. The computer-readable storage medium can be an internal storage unit of any data-processing device as described in any of the foregoing embodiments, such as a hard disk or memory. The computer-readable storage medium can also be an external storage device, such as a plug-in hard disk, smart media card (SMC), SD card, flash card, etc., equipped on the device. Furthermore, the computer-readable storage medium can include both internal storage units of any data-processing device and external storage devices. The computer-readable storage medium is used to store the computer program and other programs and data required by the data-processing device, and can also be used to temporarily store data that has been output or will be output.

[0081] Other embodiments of this application will readily occur to those skilled in the art upon consideration of the specification and practice of the disclosure herein. This application is intended to cover any variations, uses, or adaptations of this application that follow the general principles of this application and include common knowledge or customary techniques in the art not disclosed herein.

Claims

1. A robot trajectory optimization method based on temporal ESDF and rigid trajectory segment shifting, applied to a mobile robot equipped with a vision sensor and pose measurement module, executed in real time by the robot's onboard processor or communication computing device, characterized in that... include: S1. Acquire continuous multi-frame depth maps in real time using a visual sensor; The pose measurement module synchronously acquires the pose transformation of each frame relative to the current reference frame, backprojects the depth map of each frame into a 3D point cloud and uniformly transforms it to the current reference coordinate system to form a temporal point cloud set. S2. Perform depth-difference-based flying point noise filtering on the temporal point cloud set, and combine ground plane fitting and robot passage height range to filter effective obstacle points; S3. Project the effective obstacle points onto the BEV grid to form a static occupancy map, and fuse the dynamic occupancy map obtained from the dynamic target prediction. Generate a future spatiotemporal occupancy map by pixel-by-pixel fusion, perform Euclidean distance transformation on it and combine it with robot size expansion to obtain the expanded temporal ESDF field. S4. Map the future predicted waypoint sequence output by the end-to-end navigation network to the temporal ESDF field for distance sampling, obtain the collision waypoint set and merge it into at least one continuous collision segment; S5. Construct a penetration chord vector for the entry and exit points of the continuous collision segment, and determine a unified pushing normal based on the direction orthogonal to it; then determine the target safety anchor point and verify the pushing direction by combining the obstacle boundary projection relationship; S6. Construct an optimization function that includes rigid traverse constraints, trajectory length constraints, second-order smoothness constraints, and origin-endpoint preservation constraints. Iterate and update the predicted waypoint sequence to output an interference-free and smooth trajectory.

2. The robot trajectory optimization method according to claim 1, characterized in that, The depth map in step S1 is obtained directly by a depth camera, or by inference from a depth estimation model based on real-time images captured by an RGB camera mounted on a robot. The pose transformation is acquired by a pose measurement module, which includes a visual odometry and / or a laser odometry. The visual odometry estimates the pose transformation of the mobile robot based on the image acquired by the visual sensor, and the laser odometry estimates the pose transformation of the mobile robot based on the point cloud data acquired by the lidar on the mobile robot. The step of backprojecting the depth maps of each frame into 3D point clouds and uniformly transforming them to the current reference coordinate system to form a temporal point cloud set specifically includes: based on the pinhole camera model, converting the pixel coordinates, depth values, and camera intrinsic parameters in the depth maps of each frame into 3D points in the camera coordinate system to obtain the 3D point clouds of each frame; and according to the pose transformation, uniformly transforming the 3D point clouds of each frame to the current reference coordinate system to form the temporal point cloud set.

3. The robot trajectory optimization method according to claim 1, characterized in that, Step S2 specifically includes: By designing a differential difference filtering operator, the depth difference between adjacent pixels is obtained to ensure that the subsequent two-dimensional bird's-eye view BEV projection is not contaminated by noise. Therefore, let the depth difference between adjacent pixels in the image domain be: ； ； in, This represents the depth difference between pixel coordinates (u, v) and its horizontally adjacent pixels. This represents the depth difference between pixel coordinates (u,v) and its vertically adjacent pixels. This represents the depth value of the depth map at pixel coordinates (u,v) in frame t, where u represents the pixel index in the horizontal direction of the image and v represents the pixel index in the vertical direction of the image. When the depth difference between adjacent pixels exceeds a preset depth mutation threshold, the corresponding 3D point is removed; when the depth map has a confidence estimate, outlier points are further filtered based on the confidence threshold. To effectively separate passable ground from actual obstacles, the RANSAC algorithm or least squares algorithm is used for ground plane fitting in a unified reference coordinate system to remove ground clutter and overhead suspended objects that have no substantial impact on passage; the algorithm expression is as follows: ； Where a, b, and c are the parameters of the fitted ground plane equation, a represents the slope coefficient in the x-direction, b represents the slope coefficient in the z-direction, and c represents the ground height intercept; x and z represent the horizontal coordinates in the unified reference coordinate system. For any valid point in the point cloud The ground height at its corresponding location is Further calculate the relative vertical height of this point with respect to the ground: ； in, This represents the relative vertical height of the valid point with respect to the fitted ground. This indicates the vertical coordinates of the valid point in a unified reference coordinate system. This indicates the fitted ground height at the horizontal position corresponding to the valid point; Subsequently, physical constraint properties of the autonomous vehicle / robot are introduced, namely, the maximum chassis clearance height threshold that is allowed to be crossed. and the effective height of the robot's roof If the height attribute of a point satisfies: ； The point is then identified as being within the height range of obstacles of interest to the system and marked as a valid static obstacle point.

4. The robot trajectory optimization method according to claim 1, characterized in that, In step S3, effective obstacle points are projected onto the BEV grid to form a static occupancy map, and the dynamic occupancy map obtained from dynamic target prediction is fused together. A future spatiotemporal occupancy map is then generated through pixel-by-pixel fusion. Specifically, this includes: The filtered effective static obstacle points are orthogonally projected along the gravity direction onto the two-dimensional bird's-eye view BEV grid map. Based on the comparison between the cumulative number of obstacle points mapped in each grid and the preset confidence threshold, the corresponding grid is marked as statically occupied, and a static occupancy map is obtained. The multi-target detection and tracking results of the perception front end in future frames are obtained, the three-dimensional position of each dynamic target in future frames is determined, the motion heading angle of the dynamic target is calculated according to the instantaneous displacement across frames, and the geometric size is preset according to the category of the dynamic target. The corresponding dynamic occupancy area is constructed in the BEV plane, and the dynamic occupancy area is rotated and projected according to the motion heading angle to obtain the dynamic occupancy map. The static occupancy map and the dynamic occupancy map are fused pixel by pixel maximum value to generate a future spatiotemporal occupancy map.

5. The robot trajectory optimization method according to claim 1, characterized in that, In step S3, Euclidean distance transformation is performed on the future spatiotemporal occupancy map and combined with robot size dilation to obtain the dilated temporal ESDF field; specifically including: The future spatiotemporal occupancy map is obtained by fusing the static occupancy map and the dynamic occupancy map pixel-by-pixel maximum value, wherein the pixel value of each pixel is used to characterize whether the pixel position is an area occupied by an obstacle; Perform Euclidean distance transformation on the future spatiotemporal occupancy map to calculate the lower bound distance from the free space region to the boundary of the nearest obstacle-occupied region, and the intrusion distance from the interior of the obstacle-occupied region to the nearest free space boundary. Based on the difference between the lower bound distance and the intrusion distance, a symbolic Euclidean distance field (ESDF) is constructed. In the symbolic ESDF, a positive distance value indicates that the location is an area occupied by an obstacle or an interference area, while a negative distance value indicates that the location is a safe passage area. Using the robot size as the expansion radius, an expansion operation is performed on the symbolic ESDF to subtract the robot size from the distance value in the symbolic ESDF, thereby obtaining the expanded temporal ESDF field.

6. The robot trajectory optimization method according to claim 1, characterized in that, Step S5 specifically includes: For a selected continuous collision segment, a penetration chord vector is constructed based on the starting and ending waypoints of the continuous collision segment. The penetration chord vector is then normalized, and two candidate thrust normals with opposite directions are constructed based on the direction of the penetration chord vector. Based on the boundary information of the obstacles intersecting with the continuous collision segment, the boundary points of the obstacles are projected onto the candidate push-up normal direction, and the geometric centroid of the continuous collision segment is projected onto the candidate push-up normal direction. Based on the relative positional relationship between the centroid projection and the extreme value range of the obstacle boundary projection, the target safety anchor point is selected and the direction of the candidate push-up normal direction is verified. The verified push-up normal direction is used as the unified push-up normal direction of the continuous collision segment to perform overall rigid push-up of the continuous collision segment.

7. The robot trajectory optimization method according to claim 1, characterized in that, Step S6 specifically includes: A rigid push loss function is constructed for continuous collision trajectory segments to measure the out-of-bounds deviation of the continuous collision segments relative to the target safety boundary in the unified push method direction, so that each waypoint in the continuous collision segment completes a cooperative correction in the unified push method direction; the push loss of each continuous collision segment is summarized to obtain the overall collision loss. Construct a trajectory length constraint function to suppress trajectory detours and local redundancy; construct a second-order trajectory smoothness constraint function to suppress curvature abrupt changes and local oscillations; construct a start and end point constraint function to constrain the drift of the optimized trajectory's start and end points relative to the original start and end points. The overall collision loss, the trajectory length constraint function, the trajectory second-order smoothing constraint function, and the origin and end point constraint functions are weighted and then aggregated to obtain the overall optimization objective function. The overall objective function is solved iteratively using Adam or L-BFGS. When the convergence condition is met, the optimized trajectory is output.

8. A robot trajectory optimization device based on temporal ESDF and rigid trajectory segment shifting, characterized in that, The device includes the following modules: The sensor data interface and time-series point cloud recovery module acquires continuous multi-frame depth maps in real time through a vision sensor; the pose measurement module synchronously acquires the pose transformation of each frame relative to the current reference frame, backprojects the depth maps of each frame into a 3D point cloud and uniformly transforms it to the current reference coordinate system to form a time-series point cloud set. The point cloud filtering and ground screening module performs depth difference-based flying point noise filtering on the temporal point cloud set, and combines ground plane fitting and robot passage height range to screen effective obstacle points; The static and dynamic BEV occupancy fusion module projects effective obstacle points onto the BEV grid to form a static occupancy map, and fuses the dynamic occupancy map obtained from dynamic target prediction. The resulting map is then fused pixel by pixel to generate a future spatiotemporal occupancy map. The dilated ESDF generation module performs Euclidean distance transformation on it and combines it with robot size dilation to obtain the dilated temporal ESDF field; The continuous collision segment identification module maps the future predicted waypoint sequence output by the end-to-end navigation network to the temporal ESDF field for distance sampling, obtains a set of collision waypoints and merges them into at least one continuous collision segment. The module for determining the unified pushing normal and safety anchor point constructs a penetrating chord vector for the entry and exit points of the continuous collision segment, and determines the unified pushing normal based on the direction orthogonal to it; then, it combines the obstacle boundary projection relationship to determine the target safety anchor point and verify the pushing direction. The multi-objective trajectory optimization module constructs an optimization function that includes rigid traverse constraints, trajectory length constraints, second-order smoothness constraints, and origin-endpoint preservation constraints. Iteratively updates the predicted waypoint sequence and outputs an interference-free and smooth trajectory.

9. An electronic device disposed on or communicating with a mobile robot, characterized in that, It includes one or more processors and a memory; the memory is used to store one or more programs, which, when executed by the one or more processors, enable the processors to implement the robot trajectory optimization method according to any one of claims 1-7 based on sensor data acquired in real time by the mobile robot.

10. A computer-readable storage medium storing computer instructions thereon, characterized in that, When the computer instructions are executed by the processor, they implement the trajectory optimization method as described in any one of claims 1-7.

Images