Safety trajectory planning method and system for autonomous navigation of inspection robot, and robot

Abstract

This invention discloses a safe trajectory planning method, system, and robot for autonomous navigation of an inspection robot. The method includes the following steps: performing tight-coupled filtering on LiDAR point clouds and IMU data to obtain registered point clouds; constructing a 3D map based on a probabilistic occupancy grid (OctoMap); fusing robot collision boxes in the map and performing RRT-star planning to obtain an initial path P; generating a convex polygonal safe corridor sequence Q; using Minimum-Snap optimization to generate a smooth trajectory Traj; and performing numerical solutions using a sparse matrix and a convex optimization solver. This invention ensures trajectory collision safety and dynamic feasibility through safe corridor constraints, improving the robustness and real-time performance of inspection robot navigation in complex application scenarios such as intelligent inspection in petrochemical enterprises.

Description

Technical Field

[0001] This invention belongs to the technical field of machine learning, specifically relating to a safe trajectory planning method for autonomous navigation of an inspection robot. Background Technology

[0002] Perception planning is a crucial area of ​​autonomous navigation for mobile robots. It aims to enable robots to perceive their surroundings in real-time, construct environmental maps, plan feasible paths, and generate safe, executable trajectories in unknown or dynamic environments using multiple sensors. With the rapid development of unmanned systems technology, mobile robot perception planning technology has significant application value in scenarios such as warehousing and logistics, power line inspection, and emergency rescue.

[0003] Current mainstream autonomous navigation systems primarily rely on multi-sensor fusion technology, with LiDAR (Light Detection and Ranging) being the preferred sensor for environmental perception due to its high-precision 3D ranging capabilities. However, using LiDAR alone can easily lead to positioning drift in fast-moving or feature-sparse scenarios. Inertial Measurement Units (IMUs) can provide high-frequency pose estimation, but they accumulate significant errors over long periods of operation. Therefore, tightly coupled SLAM (Simultaneous Localization and Mapping) technology, combining LiDAR and IMU, has become an effective way to address these issues.

[0004] After acquiring sensor data, constructing an accurate 3D environment map is fundamental for subsequent planning decisions. While traditional point cloud maps offer high accuracy, they are voluminous and lack semantic information about occupancy status, making them unsuitable for direct collision detection. The OctoMap representation based on probabilistic occupancy grids effectively compresses map storage space by discretizing the 3D space into an octree structure and can explicitly express the probability of each voxel being occupied by an obstacle.

[0005] In 3D grid maps, the RRT-star algorithm is widely used for path search in high-dimensional spaces due to its probabilistic completeness and asymptotic optimality. This algorithm, through random sampling and tree expansion, can quickly find feasible paths in complex obstacle environments. However, RRT-star only outputs a sequence of path points, without considering the size of the mobile robot itself or dynamic constraints, and due to the lack of a clear endpoint guidance mechanism, its convergence speed is slow in complex maps.

[0006] To optimize the trajectory of the output pathpoints, the Minimum-Snap optimization method generates a C³ smooth trajectory that is continuous from the position to the acceleration derivative (Jerk) by minimizing the square integral of the fourth derivative (Snap) of the trajectory. However, Minimum-Snap only guarantees that the trajectory passes through the safe region through discrete pathpoint constraints, and cannot guarantee that the trajectory stays away from obstacles throughout the entire time domain. In dense obstacle environments, high-order polynomial oscillations may cause the trajectory to deviate from the safe passage. The convex polygon safety corridor creates a strictly convex corridor centered on the pathpoints. This method combines the initial expansion of the path segment into a rectangular region with a greedy orthogonal slicing mechanism to handle obstacles, ensuring the safety of the corridor region. Adding the convex polygon corridor constraint to the Minimum-snap trajectory optimization generates a safe trajectory that satisfies the mobile robot's dynamics and collision constraints. Summary of the Invention

[0007] The main objective of this invention is to overcome the shortcomings and deficiencies of the prior art and provide a safe trajectory planning method, system and robot for autonomous navigation of inspection robots. The method includes three-dimensional occupancy grid map construction, RRT-satr global path planning with robot collision boxes, safe corridor generation and minimum-snap trajectory optimization, thereby generating a smooth and executable trajectory that simultaneously satisfies collision safety and dynamic constraints.

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] In a first aspect, the present invention provides a safe trajectory planning method for autonomous navigation of an inspection robot, comprising the following steps:

[0010] Tightly coupled Kalman filtering is applied to the 3D lidar point cloud and the inertial measurement unit (IMU) data to obtain dense 3D point cloud data;

[0011] The OctoMap representation method based on probability occupancy grid is adopted to discretize the dense 3D point cloud data into an octree structure, explicitly expressing the probability that each voxel is occupied by an obstacle, and constructing a 3D occupancy grid map.

[0012] A sampling path search algorithm is executed in a 3D occupied grid map. The initial global path P is obtained through random sampling, node expansion, neighborhood rewire and pruning operations.

[0013] Generate a safe corridor sequence Q without obstacles for the initial global path P;

[0014] The Minimum-Snap trajectory optimization method is adopted, which constructs a trajectory optimization mathematical model by minimizing the square integral of the fourth derivative of the trajectory, Snap, and using the safety corridor sequence Q as a constraint.

[0015] The Hessian matrix and constraint matrix of the trajectory optimization mathematical model are constructed using sparse matrix technology. The model is then solved numerically using an efficient convex optimization solver to ensure real-time performance and numerical stability, ultimately outputting a safe and smooth trajectory.

[0016] As a preferred technical solution, the Fast-LIO algorithm is used for tightly coupled Kalman filtering, the specific process of which includes:

[0017] Cache IMU data queue;

[0018] Extract coordinates, intensity, timestamp, and line bundle information from each frame of point cloud;

[0019] Point cloud motion compensation is performed using IMU pre-integration; a point-to-plane residual function is constructed; and iterative extended Kalman filtering is executed to optimize the pose increment.

[0020] The optimized point cloud is inserted into the ikd-Tree to update the local map, and the topic of the distortion-free 3D point cloud is published.

[0021] As a preferred technical solution, the construction of the three-dimensional occupancy grid map specifically includes:

[0022] Octree structure initialization: Set the map resolution and the maximum depth of the octree, create a root node to represent the entire 3D space bounding box and dynamically expand according to the scene range, and each tree node contains an occupancy probability value;

[0023] Constructing a probabilistic occupancy model: for a single grid cell The probability of occupancy is expressed in logarithmic form:

[0024]

[0025] Where P(x) represents the probability that the grid is occupied, when the laser beam is observed from the sensor origin for the kth time. Pointing to the measurement point At that time, the map is updated using a ray casting model: for each free grid cell through which the ray passes. Occupied grid where the end measurement point is located Update the log-odds value respectively:

[0026]

[0027] For any query point Traverse from the root node to the leaf node and obtain their occupancy probability. ,like If the state is determined to be occupied, the robot cannot pass; If the robot is determined to be in an idle state, it can pass safely.

[0028] As a preferred technical solution, a probability decay mechanism is adopted for dynamic obstacles in the environment: for nodes that have not been updated for a long time, an update is performed every set time interval. This gradually renders old observation information invalid, ensuring the map's responsiveness to dynamic scenes.

[0029] As a preferred technical solution, the initial global path P is obtained through the following method:

[0030] An improved RRT-satr algorithm is executed on the Octomap 3D probabilistic occupancy grid map. It adopts a biased sampling strategy, and after random tree node expansion and rewire optimization, it integrates collision box detection and sampling points along the path to construct a cuboid box query occupancy probability.

[0031] from Towards Candidate nodes are generated by direction:

[0032]

[0033] To extend the step size, the unit is meters, for line segments Perform collision detection: uniformly sample multiple points along the path, construct a cuboid collision box with each sampled point as the center, and query the occupancy probability of the corresponding grid in OctoMap; if any sampled point's collision box contains a collision... If the grid is not cleared, it is considered a collision and the grid is discarded. And resampling If there is no collision, then Add to the tree structure; new edges generated by rewiring also need to undergo the above collision detection. If a collision occurs, delete the edge; otherwise, add the new edge to edge set E.

[0034] calculate and the finish line The distance, if , If the convergence threshold is reached, it is determined that the target region has been reached. Backtracking through the parent node chain until the starting point , thus obtaining the path point sequence .

[0035] As a preferred technical solution, the step of generating a safe corridor sequence Q without obstacles for the initial global path P specifically includes:

[0036] Calculate the path vector for each segment of the initial global path P, and use this as a reference to expand outwards to construct the corner points of a rectangular corridor, thus obtaining the initial polygon. ;

[0037] Extract the initial polygon In the 3D occupying raster map, construct the corresponding sub-map, and traverse the sub-map raster to build the sub-map located in the raster map. Internal obstacle set ;

[0038] When the set of obstacle points When selecting obstacle points Based on the location of the obstacle point, construct a normal cutting line for the current polygon. Segment the polygons and retain the sub-polygons that contain the points in the corresponding path segments;

[0039] Update the set of obstacles And repeat the iterative cutting and segmentation until... Finally, the strictly convex and unobstructed safe corridor sequence Q is obtained.

[0040] As a preferred technical solution, the construction of the trajectory optimization mathematical model specifically includes:

[0041] The optimization time interval is divided into multiple segments, and a polynomial function is used to calculate the trajectory within each continuous time domain segment. Perform parameterized representation;

[0042] Define the overall objective function for trajectory optimization. The overall objective function includes a main cost term J for ensuring trajectory smoothness and a center offset penalty term for improving the safety margin. ;

[0043] Wherein, the main cost item The center offset penalty term is obtained by minimizing the square integral of the fourth derivative of the trajectory, i.e., Snap. A penalty weight is introduced, and it is based on the trajectory point to its corresponding safety corridor. The distance to the centroid is constructed to avoid the generated trajectory from getting too close to the corridor boundary.

[0044] As a preferred technical solution, the numerical solution performed by combining a high-efficiency convex optimization solver specifically includes:

[0045] Construct equality constraints to limit trajectory states, including path point constraints requiring the trajectory to pass through given path points, continuity constraints for smooth high-order states at the connection of adjacent trajectory segments, and boundary motion state constraints for the start and end points.

[0046] An inequality constraint is constructed for obstacle avoidance, and the trajectory points are strictly restricted to the boundary range of the corresponding safety corridor polygon by using the form of half-plane intersection.

[0047] Combining the aforementioned trajectory optimization mathematical model, equality constraints, and inequality constraints, the Hessian matrix and constraint matrix for the quadratic programming problem are constructed.

[0048] The quadratic programming problem is solved using a convex optimization solver, and a safe and smooth trajectory that meets the dynamic constraints is output.

[0049] Secondly, the present invention provides a safe trajectory planning system for autonomous navigation of an inspection robot, which is applied to the safe trajectory planning method for autonomous navigation of the inspection robot, including a point cloud data acquisition module, a three-dimensional map construction module, an initial path planning module, a safe corridor generation module, a safe corridor generation module, and a safe trajectory output module.

[0050] The point cloud data acquisition module is used to perform tight-coupled Kalman filtering on the 3D lidar point cloud and the inertial measurement unit (IMU) data to obtain dense 3D point cloud data.

[0051] The 3D map construction module is used to discretize the dense 3D point cloud data into an octree structure using the OctoMap representation method based on probability occupancy grids, explicitly expressing the probability that each voxel is occupied by an obstacle, and constructing a 3D occupancy grid map.

[0052] The initial path planning module is used to execute a sampling path search algorithm in a three-dimensional occupied grid map, and obtain an initial global path P through random sampling, node expansion, neighborhood rewire and pruning operations;

[0053] The safe corridor generation module is used to generate a safe corridor sequence Q without obstacles for the initial global path P;

[0054] The safety trajectory optimization module is used to construct a trajectory optimization mathematical model by using the Minimum-Snap trajectory optimization method, which minimizes the square integral of the fourth derivative of the trajectory Snap, while using the safety corridor sequence Q as a constraint.

[0055] The safe trajectory output module is used to construct the Hessian matrix and constraint matrix of the trajectory optimization mathematical model using sparse matrix technology, and then perform numerical solution by combining it with an efficient convex optimization solver to ensure real-time performance and numerical stability, and finally output a safe and smooth trajectory.

[0056] Thirdly, the present invention provides an electronic device, the electronic device comprising:

[0057] At least one processor; and,

[0058] A memory communicatively connected to the at least one processor; wherein,

[0059] The memory stores computer program instructions that can be executed by the at least one processor, which are then executed by the at least one processor to enable the at least one processor to perform the safe trajectory planning method for autonomous navigation of the inspection robot.

[0060] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0061] 1. This invention deeply integrates LiDAR-Inertial SLAM (Fast-LIO) with probabilistic occupancy grid map OctoMap to construct a tightly coupled 3D environment representation system. Compared with traditional point cloud maps, OctoMap's octree structure reduces memory usage by 60%-80%.

[0062] 2. This invention integrates the Minimum-Snap smoothness objective with the hard constraint of a safe corridor to construct a quadratic programming framework with inequality constraints. Through endpoint state reparameterization and sparse matrix techniques, the OSQP solver, combined with warm-start, reduces the number of iterations from 80 to 25, and solves for 20 path points in only 12ms. Compared to the traditional Minimum-Snap approach, which lacks safety guarantees or whose soft constraints cannot strictly avoid obstacles, this invention ensures that the trajectory remains smooth while staying within the safe corridor throughout the entire process.

[0063] 3. This invention provides a systematic solution from perception, mapping, planning to optimization, supports online map updates and local replanning, and enables the algorithm to run in more complex scenarios, including internal and external environments. Attached Figure Description

[0064] To more clearly illustrate the technical solutions in the embodiments of this application, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0065] Figure 1 This is a schematic diagram of the safe path planning and trajectory optimization process for inspection robots based on 3D grid maps.

[0066] Figure 2 A schematic diagram of the result of constructing a 3D raster map.

[0067] Figure 3 This is a schematic diagram of the RRT-star path planning process.

[0068] Figure 4 This diagram illustrates path node expansion, neighborhood rewiring, and pruning operations.

[0069] Figure 5This is a schematic diagram illustrating the process of generating a convex polygon security corridor.

[0070] Figure 6 This is a schematic diagram of the results of safe path planning and trajectory optimization based on a 3D raster map.

[0071] Figure 7 This is a block diagram of the safe trajectory planning system for autonomous navigation of the inspection robot according to an embodiment of the present invention.

[0072] Figure 8 This is a structural diagram of an electronic device according to an embodiment of the present invention. Detailed Implementation

[0073] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of the present application, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present application without creative effort are within the scope of protection of the present application.

[0074] In this application, the reference to "embodiment" means that a specific feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of this application. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a mutually exclusive, independent, or alternative embodiment. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described in this application can be combined with other embodiments.

[0075] like Figure 1 As shown in the figure, the specific implementation steps of the inspection robot safety path planning and trajectory optimization method based on a 3D grid map in this embodiment are as follows:

[0076] S1. Acquisition of dense 3D point cloud data.

[0077] To provide high-precision map representation for mobile robot path planning, a LiDAR point cloud-inertial odometry SLAM algorithm called Fast-lio is used. FAST-LIO nodes are started to subscribe to LiDAR point cloud topics and IMU data topics, and the IMU data queue is cached. For each frame of point cloud, the 3D coordinates of each point are extracted. ,strength timestamp and wire harness number IMU data within the corresponding time interval is extracted based on the scan start time, and the relative motion between two scan frames is calculated by median integration. The motion compensation of each point in the point cloud is performed using the IMU pre-integration results, and its coordinates are transformed to the coordinate system of the scan start time to generate a distortion-free point cloud frame. Using ikd-Tree to maintain the local map, the distortion-free point cloud frames are... Projecting to the world coordinate system to construct the current scan In the ikd-Tree, the nearest neighbors of each point are searched to form the corresponding point set, and the point-to-plane residual function is constructed. ; Perform tightly coupled iterative extended Kalman filtering, propagate the state covariance based on the IMU dynamics model, use the lidar observation residual as the measurement update state vector and covariance, iterate until the pose increment magnitude is less than the threshold or the maximum number of iterations is reached; transform the current scan to the global coordinate system according to the optimized pose, insert the voxel filter downsampled and insert it into the ikd-Tree to update the local map, and publish the distortion-free 3D point cloud topic at the same time.

[0078] S2. Construction of a 3D probabilistic occupancy raster map (Octomap).

[0079] While the dense 3D point cloud map obtained in step S1 is highly accurate, it is also large in volume and lacks semantic information about occupancy status, making it difficult to use directly for collision detection. This embodiment uses the OctoMap framework to construct a 3D occupancy raster map, discretizing the dense point cloud into an octree structure with probability information to achieve efficient storage and collision lookup. The specific implementation process is as follows:

[0080] S21. Octree structure initialization

[0081] Set map resolution A depth of 0.1 meters corresponds to a maximum octree depth of 16 levels. A root node is created to represent the entire 3D bounding box; its size dynamically expands based on the scene's extent and is initialized to empty. Each tree node contains an occupancy probability value. Update count counter and child node pointers. To balance memory usage and precision, the confidence threshold for the occupancy probability is set to [value missing]. The idle probability threshold is .

[0082] S22, Mathematical Modeling of Probability Occupation Model

[0083] For a single grid cell Its probability of occupancy is expressed in log-odds form:

[0084]

[0085] Where P(x) represents the probability that the grid cell is occupied. Initially, the probability of all nodes is set to 0.5. .

[0086] When the laser beam is observed to originate from the sensor for the kth time... Pointing to the measurement point At that time, the map is updated using a ray casting model: for each free grid cell through which the ray passes. Occupied grid where the end measurement point is located Update their log odds values ​​respectively:

[0087]

[0088] S23, Dynamic Node Insertion and Probability Accumulation

[0089] For each registered point cloud frame, perform the following operations:

[0090] Coordinate transformation: Transform the point cloud from the lidar coordinate system to the world coordinate system to obtain a 3D point set. ;

[0091] Voxel mapping: for each point Calculate the index of its leaf node. ;

[0092] Probability update: along the sensor origin arrive The ray traverses all nodes and performs operations on the free space nodes. Execute on the endpoint node If the node does not exist, it will be created dynamically and initialized. Updated later;

[0093] Counter maintenance: Each time the node counter is updated, ,when The probability of the node is locked in time to avoid instability caused by excessive updates.

[0094] S24, Pruning Optimization and Memory Management

[0095] When all eight child nodes of a node in an octree have a probability of falling into the range... Interval (in this embodiment, it is taken as) When a node is marked as being in a unified state, its child nodes are deleted, thus achieving memory compression. Furthermore, in practical applications of path planning for inspection robots, ground point clouds and point clouds higher than 5 meters in height are ineffective for obstacle avoidance planning; these two types of point clouds can be filtered out. Experiments show that this pruning and filtering strategy can reduce map memory usage by 60%-80% while maintaining query accuracy.

[0096] S25. Occupancy Status Determination and Query Interface

[0097] For any query point Traverse from the root node to the leaf node and obtain their occupancy probability. ,like The system is considered occupied, and the robot cannot pass. If... If the state is determined to be idle, the robot can safely pass. Otherwise, the state is unknown and requires further observation. To support queries at different resolutions, this embodiment provides a multi-scale interface: at depth... When querying the parent node of node 6, the maximum probability of its eight child nodes is returned as a conservative occupancy estimate to ensure the safety of collision detection.

[0098] S26, Dynamic Environmental Adaptation Treatment

[0099] For dynamic obstacles in the environment, a probability decay mechanism is adopted: for nodes that have not been updated for a long time, an update is performed every 10 seconds. This gradually renders old observation information invalid, ensuring the map's responsiveness to dynamic scenes. Simultaneously, during RRT-satr planning, probability values ​​are set within... The grid within the interval is considered the cost region, and the path cost weight is set to... The planner is guided to prioritize vacant areas with high confidence levels.

[0100] Through the above steps, the dense point cloud information obtained in S1 is used to construct an OctoMap in a 10m × 20m × 8m indoor corridor scene. Figure 2 As shown, memory usage remains stable below 50MB, and the time taken to query the occupied status is... Microseconds meet real-time planning requirements.

[0101] S3. Use RRT-star on the map to plan the initial path P.

[0102] After completing map construction in S2, this embodiment executes the improved RRT-star algorithm on the OctoMap probabilistic occupancy grid map M to generate a collision-free initial path P connecting the start and end points. The specific implementation process is as follows: Figure 3 As shown.

[0103] S31. Initialization Parameters and Heuristic Random Sampling Strategy

[0104] Set the 3D coordinates of the starting point S and the ending point G, and input the robot collision detection box dimensions h: length, width, and height. Set the search radius. Used for neighborhood search, expanding step size Target area threshold Initialize the tree structure T=S, the node set N=S, and the edge set... Random sampling points are generated in each iteration. A 20% endpoint bias strategy is adopted: random numbers are generated. ,like Then let Otherwise, uniform sampling is performed within the map's free space. This strategy significantly accelerates target region convergence while ensuring probabilistic completeness, improving planning efficiency by over 40% compared to pure random sampling in complex environments.

[0105] S32, Random Tree Node Expansion and Rewiring

[0106] Figure 4 This illustrates a point in the random tree expansion process, with node numbers indicating their generation order. Node 0 is the initial node, and node 9 is the next node generated. (Node 4) along Directional expansion New node generated by length Node 4 is the temporary parent node of node 9. The numbers on the lines connecting nodes represent the Euclidean distance, i.e., the path cost. During the process of reselecting the parent node, with node 9 as the center and radius r, all candidate nodes that can serve as the parent node of node 9 are searched within the circle: 4, 5, 6, and 8. The following table shows the path cost of each candidate parent node:

[0107] Table 1

[0108]

[0109] Table 1 shows that the path cost is minimized when node 5 is the new parent node of node 9, so the parent node of node 9 is changed from node 4 to node 5.

[0110] After reselecting the parent node, a rewiring operation is performed on the random tree to further reduce the connection cost of the random tree nodes. This process can be described as: if the parent node of a neighboring node is changed to... If the path cost can be reduced, then the change should be implemented. For example... Figure 4 As shown, node 9 is the newly generated node. The node has neighboring nodes 4, 5, 6, and 8, and its original parent nodes are 0, 1, 4, and 5, respectively.

[0111] The original path and cost are as follows:

[0112] Table 2

[0113]

[0114] If changed to If node 9 is the parent node of its nearest neighbors 4, 5, 6, and 8, then their new paths and costs are shown in the table below:

[0115] Table 3

[0116]

[0117] According to Tables 2 and 3, changing the parent node of nodes 4, 5, and 8 to node 9 increases the path cost compared to the original path; therefore, the parent nodes of these nodes will not be changed. Changing the parent node of node 6 to node 9 decreases the path cost compared to the original path; therefore, the parent node of node 6 will be changed to node 9.

[0118] The core of the RRT-star algorithm consists of the two processes mentioned above: reselecting the parent node and rerouting. These two processes complement each other. Reselecting the parent node minimizes the path cost of the new node, while rerouting eliminates redundant paths, further reducing path cost.

[0119] S33, Collision Detection

[0120] from Towards Direction generation of candidate nodes For line segments Perform collision detection: Uniformly sample 10 points along the path, construct a cuboid collision box centered on each sampled point, and query the occupancy probability of the corresponding grid in OctoMap. If any sampled point's collision box contains a collision... If the grid is not cleared, it is considered a collision and the grid is discarded. And resampling If there is no collision, then Add to the tree structure. New edges generated by rewiring also need to undergo the above collision detection. If a collision is found, the edge is deleted; otherwise, the new edge is added to the edge set E.

[0121] S34, Initial path P output

[0122] calculate and the finish line The distance, if If so, it is determined that the target area has been reached. From Backtracking through the parent node chain until the starting point This yields the path point sequence P. To ensure path quality, a maximum number of iterations is set. When both the goal is reached and the number of iterations exceeds [a certain threshold], The planning process terminates upon reaching the target; if the target is not reached, the process returns to step S32 to continue expansion. After outputting path P, the Douglas-Peucker algorithm is used to extract key inflection points, compressing the number of path points to 20%-30% of the original size, thus reducing the computational burden for the subsequent convex polygon corridor generation and trajectory optimization in S4. Experiments show that the RRT-star planning method in this embodiment takes an average of 85ms in complex indoor environments, reduces path cost by approximately 25% compared to the basic RRT-star method, and satisfies 100% of the collision box constraints.

[0123] S4. Generate a convex polygonal safety corridor for the initial path.

[0124] In autonomous navigation path planning systems, ensuring collision-free trajectories is crucial. This embodiment combines the initial expansion of path segment P into a rectangular region with a greedy orthogonal slicing mechanism to handle obstacles, ensuring the convexity and safety of the trajectory optimization region in subsequent steps S5. The specific process for generating the convex polygon safe corridor is as follows: Figure 5 As shown.

[0125] S41. Initial Rectangular Corridor Construction

[0126] Simplified path point sequence Calculate the path vector for each segment. .like If the path segment is too short, it will be degraded to the center point. The side length is A square; otherwise, calculate the unit direction. and orthogonal unit vectors The axial extension is determined based on the path segment length. Take half the width Construct the four corner points of the rectangular corridor:

[0127]

[0128] Clip the coordinates of each corner point to the map boundary. Within the range, connect them in a counter-clockwise order to form the initial convex polygon. This ensures geometric validity and the accuracy of subsequent point testing within the polygon.

[0129] S42, Obstacle Perception and Mask Generation

[0130] For each Calculate the axial bounding box: , Similarly, and expand safety margins Extract submap To improve efficiency, Transform to submap coordinate system: The center of each grid cell in the sub-map. Perform vectorized point testing within the polygon and generate a Boolean mask. If and only if the point is located at Internal. Traversal Construct a set of obstacle points from all grid cells:

[0131]

[0132] This set records all items located in The global coordinates of the internal obstacle grid are used for subsequent iterative cutting.

[0133] S43, Normal Iterative Cutting Optimization

[0134] when Execute loop: Select the nearest obstacle point Calculate the midpoint of the path segment arrive vector and its unit normal vector .exist The length of the structure is Cutting line Using polygon segmentation algorithms to Divided into two sub-polygons Select to include The sub-polygon as a new and update Repeat cutting until... Finally, a strictly convex safe corridor sequence without obstacles is obtained. Each corridor guarantees: ① strict convexity; ② complete coverage of the corresponding path segment; ③ no grid cells with an occupancy probability exceeding a threshold.

[0135] S5. Optimize the initial path with Minimum-snap safety trajectory within the safety corridor.

[0136] S3 and S4 are given the two-dimensional pathpoint sequences output by the RRT-STAR algorithm. ,in and the corresponding strictly convex safe corridor sequence Each A convex polygon represents a path segment. arrive The surrounding unobstructed free space.

[0137] S51. Optimize the objective function;

[0138] The optimization objective is to generate smooth and feasible trajectories in the continuous time domain. This satisfies the following: minimizing the square integral of the trajectory snap (fourth derivative) ensures that the rate of change of acceleration (jerk) is minimized, thereby improving the motion robustness of the inspection robot.

[0139]

[0140] in Total duration It represents the fourth derivative.

[0141] Constraints:

[0142] Path point constraint: The trajectory must pass through all path points provided by RRT-star, i.e. For all ,in .

[0143] Continuity constraint: The trajectory is high-order continuous at the connection points (position-to-jerk continuity), i.e. for and all intermediate points .

[0144] Boundary constraints: The velocity and acceleration at the start and end points are zero (vehicle starts / stops from a standstill), i.e. , .

[0145] Safety corridor constraint: The trajectory point must be strictly located within the convex polygon corridor, i.e. And extend to all trajectory segments to avoid collisions: for each segment trajectory exist The internal geometry is satisfied.

[0146] S52, Trajectory Parameterization

[0147] Divide the time interval according to the waypoints. part: , No. Segment correspondence arrive The duration is To ensure jerk continuity (C³ smoothness), each dimension of the trajectory uses a 7th-order polynomial:

[0148]

[0149] in Segment index, coefficient vector The global decision variable is all coefficients across all segments:

[0150]

[0151] To simplify the introduction of security constraints, endpoint state reparameterization will be implemented subsequently.

[0152] S53, Derivation of the Cost Function

[0153] Single-segment trajectory cost part The dimension of the snap is:

[0154]

[0155] Square and integrate:

[0156]

[0157] Where the Hessian matrix It is an 8x8 symmetric positive definite matrix, whose elements are determined by polynomial integration:

[0158]

[0159] The lower-order terms are zero. This can be generalized to the general case. polynomials of degree and The first derivative.

[0160] Total Cost: The total cost of a two-dimensional trajectory is the sum of the costs of each dimension and each segment.

[0161]

[0162] Global Hessian matrix For block diagonal matrices: Sparsity facilitates the solution.

[0163] To enhance numerical stability and prevent the trajectory from approaching the corridor boundary, a center offset penalty is introduced:

[0164]

[0165] in For the first The centroid (geometric center) of each corridor is calculated using the vertices of the polygon:

[0166]

[0167] For area, The vertex is the endpoint. Overall goal: ,in (Location component only) .

[0168] S6. Construct the constraint matrix and perform numerical solutions using an efficient convex optimization solver.

[0169] S5 establishes a mathematical framework for trajectory optimization using the Minimum-Sanp method. The following are the steps for numerical solution:

[0170] S61. Matrixing of Equality Constraints (Path Points, Continuity, and Boundaries)

[0171] Differential constraints (endpoint states) define the Vandermonde matrix to constrain position, velocity, acceleration, and jerk:

[0172]

[0173] Single-segment constraint (with) (e.g., dimension) Location ,speed Endpoint vector ,in ,wait.

[0174] Continuity constraints, in The following conditions must be met:

[0175]

[0176] Matrix form: Global equation ,in Assemble the connection equations for all segments. Boundaries and path points are fixed; fixed variables include the starting point. Similar destinations; waypoints Given. Free variable: intermediate velocity. acceleration jerk .

[0177] S62, Inequality Constraints: Integration of Safe Corridors

[0178] Safety corridor It is a strictly convex polygon, by Edge definition. Represented by a half-plane: ,in each line It is the outer normal vector. For trajectory points ,constraint:

[0179]

[0180] Expand to the global level: ,in Linked to location components via coefficient mapping (only affects...) (Location item), thus ensuring that the entire trajectory is within the safe corridor.

[0181] S63. Final QP Standard Form and Solution

[0182] Variable elimination and reparameterization, converting coefficients Reparameterization to endpoint states Using mapping ,in Calculated using the Vandermonde inverse. The cost function is transformed into:

[0183]

[0184] in Separate fixed amount (Path points, boundaries) and free variables (Intermediate velocity and acceleration):

[0185]

[0186] Let be the permutation matrix. Final QP:

[0187]

[0188]

[0189] OSQP format:

[0190]

[0191]

[0192] in (Multiply by 2 to match criteria) The safety trajectory was numerically solved using the OSQP solver. Numerical stability: preprocessing scaling. Upon reaching the unit norm, the condition number from Down to Tolerance Ensure accuracy.

[0193] S7. Algorithm testing using the Lslidar C16 LiDAR sensor on a mobile robot platform.

[0194] This embodiment demonstrates algorithm testing on the Jackal mobile robot platform, using an Lslidar C16 lidar sensor and an integrated IMU module. Specific parameters are as follows:

[0195] LiDAR and IMU parameter table:

[0196] Table 4

[0197]

[0198] The testing process was as follows: First, the Lslidar C16 LiDAR was fixedly mounted in the center of the Jackal mobile robot platform, which has a built-in IMU module to provide attitude and acceleration data. Then, the Jackal mobile robot's 12V power interface cable was connected to the Lslidar C16 LiDAR's power port to ensure stable power supply. Simultaneously, the Lslidar C16 LiDAR's network cable interface was connected to the laptop's network port for data transmission. The Jackal mobile robot platform is independently powered by its own battery, requiring no external power supply.

[0199] The Ubuntu 20.04 operating system was launched on a laptop, and inter-device communication was implemented using the ROS (Robot Operating System) framework. First, the Jackal robot and the Lslidar C16 LiDAR node were started to ensure real-time sensor data acquisition. Then, the positioning data provided by the IMU and the point cloud data scanned by the LiDAR were used as inputs and fed into the Fast-LIO node for processing. This node uses a tightly coupled laser-inertial odometry method to output the registered 3D point cloud information and robot odometry data.

[0200] Next, the registered 3D point cloud information and odometry data are input into the OctoMap node. This node constructs a 3D occupancy grid map based on an octree structure of probabilistic occupancy grids to represent the distribution of obstacles in the environment. Subsequently, the RRT* node is started, and the start and end coordinates are manually set in the RViz visualization interface. This node performs random sampling, node expansion, neighborhood rewiring, and pruning operations to generate an initial global path P.

[0201] Finally, the Minimum-Snap node is activated to optimize the trajectory of the initial path P. First, an initial rectangular corridor is generated for each segment of path P. Then, a bounding box is calculated for each initial corridor, and a sub-map is extracted. All obstacle points are detected within the sub-map. Based on the detected obstacle points, normal cutting lines are constructed one by one to segment the current corridor, selecting the sub-polygon containing the midpoint of the path segment as the new corridor. This cutting process is repeated until no obstacle points remain, resulting in a strictly convex and obstacle-free safe corridor sequence Q. Based on the safe corridor sequence Q, the Minimum-Snap trajectory optimization method is used. By minimizing the square integral of the fourth derivative (Snap) of the trajectory, and simultaneously applying convex polygon corridor constraints, start / end point constraints, and continuity constraints, a C³ smooth trajectory Traj with continuous position, velocity, acceleration up to Jerk is generated. This trajectory can be directly executed by the Jackal robot for safe navigation.

[0202] Figure 6 The test results of this embodiment are presented, where the map is a 3D occupancy grid map constructed by OctoMap in an indoor corridor scene, the polyline segment represents the initial path planned by the RRT* algorithm, and the curve segment represents the smooth trajectory optimized by Minimum-Snap under the constraint of a convex polygon safe corridor. These results verify the effectiveness and robustness of the method of this invention on a real hardware platform.

[0203] It should be noted that, for the sake of simplicity, the aforementioned method embodiments are all described as a series of actions. However, those skilled in the art should understand that the present invention is not limited to the described order of actions, because according to the present invention, some steps can be performed in other orders or simultaneously.

[0204] Based on the same idea as the safe trajectory planning method for autonomous navigation of inspection robots in the above embodiments, the present invention also provides a safe trajectory planning system for autonomous navigation of inspection robots. This system can be used to execute the above-described safe trajectory planning method for autonomous navigation of inspection robots. For ease of explanation, the structural diagram of the embodiment of the safe trajectory planning system for autonomous navigation of inspection robots only shows the parts related to the embodiments of the present invention. Those skilled in the art will understand that the illustrated structure does not constitute a limitation on the device, and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0205] Please see Figure 7 In another embodiment of this application, a safe trajectory planning system 100 for autonomous navigation of an inspection robot is provided. The system includes a point cloud data acquisition module 101, a three-dimensional map construction module 102, an initial path planning module 103, a safe corridor generation module 104, a safe trajectory optimization module 105, and a safe trajectory output module 106.

[0206] The point cloud data acquisition module 101 is used to perform tight-coupled Kalman filtering on the three-dimensional lidar point cloud and the inertial measurement unit (IMU) data to obtain dense three-dimensional point cloud data.

[0207] The three-dimensional map construction module 102 is used to discretize the dense three-dimensional point cloud data into an octree structure using the OctoMap representation method based on probability occupancy grid, explicitly expressing the probability that each voxel is occupied by an obstacle, and constructing a three-dimensional occupancy grid map.

[0208] The initial path planning module 103 is used to execute a sampling path search algorithm in a three-dimensional occupied grid map, and obtain an initial global path P through random sampling, node expansion, neighborhood rewire and pruning operations;

[0209] The safe corridor generation module 104 is used to generate a safe corridor sequence Q without obstacles for the initial global path P;

[0210] The safety trajectory optimization module 105 is used to construct a trajectory optimization mathematical model by using the Minimum-Snap trajectory optimization method, which minimizes the square integral of the fourth derivative of the trajectory Snap, while using the safety corridor sequence Q as a constraint.

[0211] The safe trajectory output module 106 is used to construct the Hessian matrix and constraint matrix of the trajectory optimization mathematical model using sparse matrix technology, and to perform numerical solution by combining an efficient convex optimization solver to ensure real-time performance and numerical stability, and finally output a safe and smooth trajectory.

[0212] It should be noted that the safety trajectory planning system for autonomous navigation of the inspection robot of the present invention corresponds one-to-one with the safety trajectory planning method for autonomous navigation of the inspection robot of the present invention. The technical features and beneficial effects described in the embodiments of the safety trajectory planning method for autonomous navigation of the inspection robot described above are all applicable to the embodiments of safety trajectory planning for autonomous navigation of the inspection robot. For details, please refer to the description in the embodiments of the method of the present invention, which will not be repeated here.

[0213] Furthermore, in the implementation of the safety trajectory planning system for autonomous navigation of the inspection robot in the above embodiments, the logical division of each program module is only an example. In actual applications, the above functions can be assigned to different program modules as needed, for example, for the sake of corresponding hardware configuration requirements or software implementation convenience. That is, the internal structure of the safety trajectory planning system for autonomous navigation of the inspection robot is divided into different program modules to complete all or part of the functions described above.

[0214] Please see Figure 8 In one embodiment, an electronic device is provided for implementing a safe trajectory planning method for autonomous navigation of an inspection robot. The electronic device 200 may include a first processor 201, a first memory 202 and a bus, and may also include a computer program stored in the first memory 202 and executable on the first processor 201, such as a safe trajectory planning program 203 for autonomous navigation of an inspection robot.

[0215] The first memory 202 includes at least one type of readable storage medium, including flash memory, portable hard drive, multimedia card, card-type memory (e.g., SD or DX memory), magnetic memory, magnetic disk, optical disk, etc. In some embodiments, the first memory 202 can be an internal storage unit of the electronic device 200, such as the portable hard drive of the electronic device 200. In other embodiments, the first memory 202 can also be an external storage device of the electronic device 200, such as a plug-in portable hard drive, smart media card (SMC), secure digital card (SD), flash card, etc., equipped on the electronic device 200. Furthermore, the first memory 202 can include both internal and external storage units of the electronic device 200. The first memory 202 can be used not only to store application software and various types of data installed on the electronic device 200, such as the code of the safety trajectory planning program 203 for autonomous navigation of the inspection robot, but also to temporarily store data that has been output or will be output.

[0216] In some embodiments, the first processor 201 may be composed of integrated circuits, such as a single packaged integrated circuit or multiple integrated circuits with the same or different functions, including combinations of one or more central processing units (CPUs), microprocessors, digital processing chips, graphics processors, and various control chips. The first processor 201 is the control unit of the electronic device, connecting various components of the entire electronic device through various interfaces and lines. It executes programs or modules stored in the first memory 202 and calls data stored in the first memory 202 to perform various functions of the electronic device 200 and process data.

[0217] Figure 8 Only electronic devices with components are shown; it will be understood by those skilled in the art that... Figure 8 The structure shown does not constitute a limitation on the electronic device 200, and may include fewer or more components than shown, or combine certain components, or have different component arrangements.

[0218] The inspection robot's autonomous navigation safety trajectory planning program 203, stored in the first memory 202 of the electronic device 200, is a combination of multiple instructions. When run in the first processor 201, it can achieve the following:

[0219] Tightly coupled Kalman filtering is applied to the 3D lidar point cloud and the inertial measurement unit (IMU) data to obtain dense 3D point cloud data;

[0220] The OctoMap representation method based on probability occupancy grid is adopted to discretize the dense 3D point cloud data into an octree structure, explicitly expressing the probability that each voxel is occupied by an obstacle, and constructing a 3D occupancy grid map.

[0221] A sampling path search algorithm is executed in a 3D occupied grid map. The initial global path P is obtained through random sampling, node expansion, neighborhood rewire and pruning operations.

[0222] Generate a safe corridor sequence Q without obstacles for the initial global path P;

[0223] The Minimum-Snap trajectory optimization method is adopted, which constructs a trajectory optimization mathematical model by minimizing the square integral of the fourth derivative of the trajectory, Snap, and using the safety corridor sequence Q as a constraint.

[0224] The Hessian matrix and constraint matrix of the trajectory optimization mathematical model are constructed using sparse matrix technology. The model is then solved numerically using an efficient convex optimization solver to ensure real-time performance and numerical stability, ultimately outputting a safe and smooth trajectory.

[0225] Furthermore, if the modules / units integrated in the electronic device 200 are implemented as software functional units and sold or used as independent products, they can be stored in a non-volatile computer-readable storage medium. The computer-readable medium may include: any entity or device capable of carrying the computer program code, a recording medium, a USB flash drive, a portable hard drive, a magnetic disk, an optical disk, a computer memory, or a read-only memory (ROM).

[0226] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and RAMbus dynamic RAM (RDRAM), etc.

[0227] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0228] The above embodiments are preferred embodiments of the present invention, but the embodiments of the present invention are not limited to the above embodiments. Any changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be considered equivalent substitutions and shall be included within the protection scope of the present invention.

Claims

1. A method for safe trajectory planning for autonomous navigation of an inspection robot, characterized in that, Includes the following steps: Tightly coupled Kalman filtering is applied to the 3D lidar point cloud and the inertial measurement unit (IMU) data to obtain dense 3D point cloud data; The OctoMap representation method based on probability occupancy grid is adopted to discretize the dense 3D point cloud data into an octree structure, explicitly expressing the probability that each voxel is occupied by an obstacle, and constructing a 3D occupancy grid map. A sampling path search algorithm is executed in a 3D occupied grid map. The initial global path P is obtained through random sampling, node expansion, neighborhood rewire and pruning operations. Generate a safe corridor sequence Q without obstacles for the initial global path P; The Minimum-Snap trajectory optimization method is adopted, which constructs a trajectory optimization mathematical model by minimizing the square integral of the fourth derivative of the trajectory, Snap, and using the safety corridor sequence Q as a constraint. The Hessian matrix and constraint matrix of the trajectory optimization mathematical model are constructed using sparse matrix technology. The model is then solved numerically using an efficient convex optimization solver to ensure real-time performance and numerical stability, ultimately outputting a safe and smooth trajectory.

2. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 1, characterized in that, The Fast-LIO algorithm is used for tightly coupled Kalman filtering. The specific process includes: Cache IMU data queue; Extract coordinates, intensity, timestamp, and line bundle information from each frame of point cloud; Point cloud motion compensation is performed using IMU pre-integration; a point-to-plane residual function is constructed; and iterative extended Kalman filtering is executed to optimize the pose increment. The optimized point cloud is inserted into the ikd-Tree to update the local map, and the topic of the distortion-free 3D point cloud is published.

3. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 1, characterized in that, The construction of the three-dimensional occupancy grid map specifically involves: Octree structure initialization: Set the map resolution and the maximum depth of the octree, create a root node to represent the entire 3D space bounding box and dynamically expand according to the scene range, and each tree node contains an occupancy probability value; Constructing a probabilistic occupancy model: for a single grid cell The probability of occupancy is expressed in logarithmic form: Where P(x) represents the probability that the grid is occupied, when the laser beam is observed from the sensor origin for the kth time. Pointing to the measurement point At that time, the map is updated using a ray casting model: for each free grid cell through which the ray passes. Occupied grid where the end measurement point is located Update the log-odds value respectively: For any query point Traverse from the root node to the leaf node and obtain their occupancy probability. ,like If the state is determined to be occupied, the robot cannot pass; If the robot is determined to be in an idle state, it can pass safely.

4. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 3, characterized in that, For dynamic obstacles in the environment, a probability decay mechanism is adopted: for nodes that have not been updated for a long time, an update is performed at set intervals. This gradually renders old observation information invalid, ensuring the map's responsiveness to dynamic scenes.

5. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 1, characterized in that, The initial global path P is obtained through the following method: An improved RRT-satr algorithm is executed on the Octomap 3D probabilistic occupancy grid map. It adopts a biased sampling strategy, and after random tree node expansion and rewire optimization, it integrates collision box detection and sampling points along the path to construct a cuboid box query occupancy probability. from Towards Candidate nodes are generated by direction: To extend the step size, the unit is meters, for line segments Perform collision detection: uniformly sample multiple points along the path, construct a cuboid collision box with each sampled point as the center, and query the occupancy probability of the corresponding grid in OctoMap; if any sampled point's collision box contains a collision... If the grid is not cleared, it is considered a collision and the grid is discarded. And resampling If there is no collision, then Add to the tree structure; new edges generated by rewiring also need to undergo the above collision detection. If a collision occurs, delete the edge; otherwise, add the new edge to edge set E. calculate and the finish line The distance, if , If the convergence threshold is reached, it is determined that the target region has been reached. Backtracking through the parent node chain until the starting point , thus obtaining the path point sequence .

6. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 1, characterized in that, The process of generating a safe corridor sequence Q without obstacles for the initial global path P specifically includes: Calculate the path vector for each segment of the initial global path P, and use this as a reference to expand outwards to construct the corner points of a rectangular corridor, thus obtaining the initial polygon. ; Extract the initial polygon In the 3D occupying raster map, construct the corresponding sub-map, and traverse the sub-map raster to build the sub-map located in the raster map. Internal obstacle set ; When the set of obstacle points When selecting obstacle points Based on the location of the obstacle point, construct a normal cutting line for the current polygon. Segment the polygons and retain the sub-polygons that contain the points in the corresponding path segments; Update the set of obstacles And repeat the iterative cutting and segmentation until... Finally, the strictly convex and unobstructed safe corridor sequence Q is obtained.

7. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 1, characterized in that, The construction of the trajectory optimization mathematical model specifically includes: The optimization time interval is divided into multiple segments, and a polynomial function is used to calculate the trajectory within each continuous time domain segment. Perform parameterized representation; Define the overall objective function for trajectory optimization. The overall objective function includes a main cost term J for ensuring trajectory smoothness and a center offset penalty term for improving the safety margin. ; Wherein, the main cost item The center offset penalty term is obtained by minimizing the square integral of the fourth derivative of the trajectory, i.e., Snap. A penalty weight is introduced, and it is based on the trajectory point to its corresponding safety corridor. The distance to the centroid is constructed to avoid the generated trajectory from getting too close to the corridor boundary.

8. The method for safe trajectory planning for autonomous navigation of an inspection robot according to claim 1, characterized in that, The numerical solution method, which combines a high-efficiency convex optimization solver, specifically includes: Construct equality constraints to limit trajectory states, including path point constraints requiring the trajectory to pass through given path points, continuity constraints for smooth high-order states at the connection of adjacent trajectory segments, and boundary motion state constraints for the start and end points. An inequality constraint is constructed for obstacle avoidance, and the trajectory points are strictly restricted to the boundary range of the corresponding safety corridor polygon by using the form of half-plane intersection. Combining the aforementioned trajectory optimization mathematical model, equality constraints, and inequality constraints, the Hessian matrix and constraint matrix for the quadratic programming problem are constructed. The quadratic programming problem is solved using a convex optimization solver, and a safe and smooth trajectory that meets the dynamic constraints is output.

9. A safe trajectory planning system for autonomous navigation of an inspection robot, characterized in that, The safe trajectory planning method for autonomous navigation of the inspection robot applied to any one of claims 1-8 includes a point cloud data acquisition module, a 3D map construction module, an initial path planning module, a safe corridor generation module, a safe corridor generation module, and a safe trajectory output module; The point cloud data acquisition module is used to perform tight-coupled Kalman filtering on the 3D lidar point cloud and the inertial measurement unit (IMU) data to obtain dense 3D point cloud data. The 3D map construction module is used to discretize the dense 3D point cloud data into an octree structure using the OctoMap representation method based on probability occupancy grids, explicitly expressing the probability that each voxel is occupied by an obstacle, and constructing a 3D occupancy grid map. The initial path planning module is used to execute a sampling path search algorithm in a three-dimensional occupied grid map, and obtain an initial global path P through random sampling, node expansion, neighborhood rewire and pruning operations; The safe corridor generation module is used to generate a safe corridor sequence Q without obstacles for the initial global path P; The safety trajectory optimization module is used to construct a trajectory optimization mathematical model by using the Minimum-Snap trajectory optimization method, which minimizes the square integral of the fourth derivative of the trajectory Snap, while using the safety corridor sequence Q as a constraint. The safe trajectory output module is used to construct the Hessian matrix and constraint matrix of the trajectory optimization mathematical model using sparse matrix technology, and then perform numerical solution by combining it with an efficient convex optimization solver to ensure real-time performance and numerical stability, and finally output a safe and smooth trajectory.

10. An electronic device, characterized in that, The electronic device includes: At least one processor; and, A memory communicatively connected to the at least one processor; wherein, The memory stores computer program 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 safe trajectory planning method for autonomous navigation of the inspection robot as described in any one of claims 1-8.

Images