Cable tunnel autonomous obstacle avoidance inspection method, system and device based on multi-sensor fusion and medium

Abstract

The invention discloses a cable tunnel autonomous obstacle avoidance inspection method, system and equipment based on multi-sensor fusion and a medium, and relates to the technical field of data processing, and the method comprises the following specific steps: correcting motion data collected by an inspection robot, carrying out pre-integration calculation according to the corrected motion data to obtain motion change data, and carrying out data processing on the motion change data; and predicting a frame state vector of the robot at the current moment, carrying out laser vision tight coupling optimization of the inspection robot, carrying out state prediction on a dynamic obstacle, carrying out collision risk assessment in combination with the frame state vector of the inspection robot, and establishing a path planning objective function to carry out real-time path re-planning. According to the method, the problem that the convergence speed and the real-time performance of a traditional algorithm are difficult to balance in a dynamic complex scene is solved through laser and vision tight coupling optimization. Through dynamic obstacle prediction, probability risk assessment and real-time path planning, an autonomous inspection system with active obstacle avoidance and real-time path generation capabilities is established.

Description

Technical Field

[0001] This invention relates to the field of data processing technology, and in particular to a method, system, equipment and medium for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion. Background Technology

[0002] With the intelligent upgrading of urban power grids and the large-scale construction of cable tunnels, the demand for automated inspection of cable equipment and environmental conditions within tunnels is becoming increasingly urgent. As a core maintenance equipment, the inspection robot IMU needs to achieve high-precision autonomous positioning, stable obstacle avoidance, and continuous operation in the complex tunnel environment. However, existing inspection navigation solutions based on single or simple fusion sensors have the following shortcomings: The robot's localization accuracy is insufficient and prone to divergence. The similar structures of the side walls and sparse features of the top of cable tunnels cause cumulative drift in the vertical direction for Simultaneous Localization and Mapping (SLAM) algorithms based on LiDAR or vision. Traditional SLAM algorithms are prone to getting trapped in local optima in environments with repetitive features, leading to a continuous accumulation of pose estimation errors and causing the robot's path to deviate from the predetermined inspection trajectory. Existing path planning methods are not adaptable enough to dynamic environmental changes, resulting in low success rates in complex obstacle scenarios. Cable tunnels contain not only fixed equipment (such as supports and firewalls) but also temporary static obstacles such as construction materials and seepage, as well as dynamic targets such as small animals. Traditional global path planning combined with local obstacle avoidance methods struggle to generate trajectories that conform to robot kinematic constraints in real time in scenarios where dense static obstacles and sudden dynamic obstacles coexist. Especially in complex areas such as dense cable support areas and bends, due to incomplete or untimely updates to environmental perception information, the success rate of path planning is often below 93%, causing the robot to frequently get stuck or perform non-smooth actions such as sudden stops or reversals, severely impacting inspection efficiency and continuity. Summary of the Invention

[0003] In view of the aforementioned existing problems, the present invention is proposed.

[0004] Therefore, the technical problem solved by this invention is: how to achieve high-precision real-time positioning of an inspection robot in a cable tunnel environment with sparse structural features and dynamic interference by using multi-sensor tight coupling optimization and adaptive fusion technology; and how to perform real-time path replanning based on collision risk prediction to achieve continuous autonomous inspection operations in scenarios where static and dynamic obstacles coexist.

[0005] To solve the above-mentioned technical problems, the present invention provides the following technical solution: a method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion, comprising, The motion data collected by the inspection robot is corrected to obtain the corrected motion data. Pre-integration is performed based on the corrected motion data to obtain motion change data; Predict the robot's frame state vector at the current moment based on motion change data; Perform laser vision tight coupling optimization on the inspection robot and optimize the prediction process of frame state vector; State prediction of dynamic obstacles and collision risk assessment based on the frame state vector of the inspection robot; A path planning objective function is established based on the collision risk assessment results to perform real-time path replanning.

[0006] As a preferred embodiment of the multi-sensor fusion-based autonomous obstacle avoidance and inspection method for cable tunnels described in this invention, the step of performing pre-integration calculations based on corrected motion data to obtain motion change data includes: Establish the pre-integral calculation formula; Input the corrected motion data into the pre-integral calculation formula; The motion change data of the inspection robot are calculated using the pre-integration calculation formula.

[0007] This invention integrates the corrected discrete IMU data across adjacent keyframes to generate relative rotation, relative velocity, and displacement changes decoupled from the initial state. This process transforms high-frequency discrete inertial measurement sequences into a low-dimensional parameterized representation, describing kinematic changes over continuous time intervals. This incremental form of motion constraint can directly provide accurate kinematic relationships for state estimation during subsequent optimization processes without relying on initial state assumptions.

[0008] As a preferred embodiment of the multi-sensor fusion-based autonomous obstacle avoidance and inspection method for cable tunnels described in this invention, wherein: the step of predicting the robot's frame state vector at the current moment based on motion change data includes: Establish the frame state vector prediction formula; Input motion change data into the frame state vector prediction formula; The frame state vector prediction formula outputs the robot's frame state vector at the current moment.

[0009] As a preferred embodiment of the multi-sensor fusion-based autonomous obstacle avoidance inspection method for cable tunnels described in this invention, the laser vision tight coupling optimization of the inspection robot, and the optimization of the frame state vector prediction process, includes: Point cloud data is collected by lidar and laser point cloud matching is performed to obtain the laser observation residual; Visual observation residuals are obtained by acquiring image data through a visual sensor and performing visual feature matching. Based on the laser observation residual, the visual observation residual, and the robot's frame state vector at the current moment, a joint optimization objective function is established to optimize the prediction process of the frame state vector.

[0010] This invention constructs a joint optimization objective function to simultaneously solve for the geometric constraints generated by laser point cloud matching and the projection constraints generated by visual feature matching. The precise ranging information provided by the LiDAR and the rich texture information provided by the visual sensor mutually correct and complement each other. Simultaneously, an adaptive weight adjustment mechanism based on environmental features is introduced to balance the contributions of the two types of sensors in different scenarios. This tightly coupled optimization approach enables the robot's state estimation to simultaneously satisfy geometric and visual consistency, thereby maintaining the continuity of pose estimation in tunnel sections with sparse structural features.

[0011] As a preferred embodiment of the multi-sensor fusion-based autonomous obstacle avoidance inspection method for cable tunnels described in this invention, the step of predicting the state of dynamic obstacles and assessing collision risk by combining the frame state vector of the inspection robot includes: The time state vector is formed based on the frame state vector of the inspection robot; The expression for predicting the inspection robot's time state vector at the next moment based on the time state vector is: , in, This represents the time state vector of the inspection robot at the next moment. Represents the state transition matrix. This is state transition noise; The inspection robot obtains observations of dynamic obstacles, and establishes the relationship between the observations and the time state vector using the observation equation. The expression is as follows: , in, This represents the observation values ​​of the inspection robot for dynamic obstacles. Represents the observation matrix. To observe noise and reflect sensor measurement errors; Collision risk assessment is performed based on the relationship between observed values ​​and the time state vector, expressed as follows: , in, Let be the collision probability. The total number of time points. This indicates the position of the inspection robot at time t+1. This indicates the position of the obstacle at time t. This is the safe distance threshold.

[0012] This invention predicts the motion state of dynamic obstacles using extended Kalman filtering and, combined with the motion trend described by the robot's own frame state vector, infers the future spatial relationship between the two. Collision risk assessment is performed by calculating the probability that the minimum distance between the robot and the obstacle's trajectory is below a safety threshold through integration, enabling the system to identify potential conflict trajectories in advance.

[0013] As a preferred embodiment of the multi-sensor fusion-based autonomous obstacle avoidance and inspection method for cable tunnels described in this invention, the step of establishing a path planning objective function based on collision risk assessment results and performing real-time path replanning includes: Based on the collision risk assessment results, a path planning objective function is established, expressed as follows: , Where L is the real-time path. The weighting coefficients for obstacle avoidance terms. The number of control points on the trajectory. For trajectory control point indexing, For the first The position of each trajectory control point In order to be with the first The location of the nearest obstacle point to each trajectory point The dynamic weighting coefficient for the velocity term. For the first The speed of each trajectory control point This represents the time allocation for optimizing the trajectory; The path planning objective function outputs the real-time path of the inspection robot; The inspection robot adjusts its path based on the real-time path output by the path planning objective function.

[0014] As a preferred embodiment of the multi-sensor fusion-based autonomous obstacle avoidance and inspection method for cable tunnels described in this invention, the process of establishing a joint optimization objective function to optimize the prediction of the frame state vector based on the laser observation residual, the visual observation residual, and the robot's frame state vector at the current moment includes: The prediction process for optimizing the frame state vector using a joint optimization objective function is established, and its expression is:

[0015] in, This represents the optimal state estimate obtained by fusing laser and visual observation data. This indicates the initial state of the inspection robot. For laser observation residuals, This is the robot's current frame state vector. This represents the robot's position in the current frame. Let λ be the robot's pose in the current frame. Weight, and Represents the covariance matrix. For visual observation residuals; The visual constraint weights in the joint optimization objective function are dynamically adjusted based on obstacle density, and the expression is as follows: , , Where ρ is the obstacle density, Let A be the number of obstacles in the region, and let A be the area of ​​the region. The obstacle density threshold. To adjust the speed.

[0016] This invention provides an autonomous obstacle avoidance and inspection system for cable tunnels based on multi-sensor fusion.

[0017] To solve the above-mentioned technical problems, the present invention provides the following technical solution: an autonomous obstacle avoidance and inspection system for cable tunnels based on multi-sensor fusion, comprising: a correction module, a calculation module, a prediction module, an optimization module, a collision risk assessment module, and an implementation path planning module; The correction module corrects the motion data collected by the inspection robot to obtain corrected motion data. The calculation module performs pre-integration calculations based on the corrected motion data to obtain motion change data. The prediction module predicts the robot's frame state vector at the current moment based on motion change data; The optimization module performs laser vision tight coupling optimization for the inspection robot, optimizing the prediction process of the frame state vector. The collision risk assessment module predicts the state of dynamic obstacles and assesses the collision risk by combining the frame state vector of the inspection robot. The implementation path planning module establishes a path planning objective function based on the collision risk assessment results to perform real-time path replanning.

[0018] The present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the above-described method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion.

[0019] The present invention provides a computer-readable storage medium having a computer program stored thereon, characterized in that, when the computer program is executed by a processor, it implements the steps of the above-described method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion.

[0020] The beneficial effects of this invention are as follows: This invention provides motion constraints decoupled from the initial pose through IMU pre-integration, solving the cumulative drift problem in long corridor environments. By optimizing the tight coupling of laser and vision and introducing adaptive weights, geometric and texture information is fused to enhance the continuity of state estimation and environmental adaptability. Risk quantification is achieved through trajectory prediction and collision assessment of dynamic obstacles. Finally, a safe, continuous, and motion-constrained real-time inspection path is output in complex tunnel environments. Attached Figure Description

[0021] To more clearly illustrate the technical solutions of the embodiments of the present invention, 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 the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0022] Figure 1 This is a schematic diagram of the overall process of an autonomous obstacle avoidance and inspection method for cable tunnels based on multi-sensor fusion, according to an embodiment of the present invention. Detailed Implementation

[0023] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the protection scope of the present invention.

[0024] Example 1, referring to Figure 1 This is one embodiment of the present invention, which provides an autonomous obstacle avoidance and inspection method for cable tunnels based on multi-sensor fusion, including: It should be noted that with the large-scale construction of cable tunnels and the increasing demand for automated inspection, inspection robots need to achieve continuous and accurate autonomous operation in tunnel environments with complex structures, sparse features, and dynamic interference. Existing navigation schemes based on single or simple fusion sensors are prone to cumulative drift under the corridor effect, leading to decreased positioning accuracy and path deviation. Traditional path planning methods have insufficient adaptability in scenarios where static and dynamic obstacles coexist, resulting in low success rates and affecting inspection efficiency and safety.

[0025] Therefore, to address the aforementioned problems of robot localization divergence and poor adaptability of path planning to dynamic environments, the following steps (S1-S6) are implemented: The motion data collected by the inspection robot is corrected; pre-integration is performed based on the corrected motion data; and the robot's frame state vector at the current moment is predicted based on the motion change data. This solves the problem of robot localization divergence in existing methods. Furthermore, laser vision tight coupling optimization is performed on the inspection robot, collision risk assessment is conducted, and a path planning objective function is established based on the risk assessment results for real-time path replanning. This solves the problem of poor adaptability of path planning to dynamic environments in existing methods.

[0026] S1: Correct the motion data collected by the inspection robot to obtain corrected motion data; S2: Perform pre-integration calculations based on the corrected motion data to obtain motion change data; S3: Predict the robot's frame state vector at the current moment based on motion change data; S4: Perform laser vision tight coupling optimization for the inspection robot and optimize the prediction process of frame state vector; S5: Predict the state of dynamic obstacles and assess collision risk by combining the frame state vector of the inspection robot. S6: Establish a path planning objective function based on the collision risk assessment results to perform real-time path replanning.

[0027] Example 2, an embodiment of the present invention, provides a method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion, based on the previous embodiment, including: In step S1, the motion data collected by the inspection robot is corrected to obtain corrected motion data, including the following steps: Perform preprocessing of the robot's IMU raw data and correct the motion data collected by the inspection robot. The expression is as follows: , , in, This is the raw angular velocity data. The raw acceleration data, This is the angular velocity after bias correction. This is the acceleration after bias correction. This indicates the bias error of the gyroscope. This represents the bias error of the accelerometer, and k represents the time step index.

[0028] During measurement, IMUs (Inertial Measurement Units) are affected by various noises and biases, leading to errors in the measured values. To obtain more accurate angular velocity and acceleration data, bias correction is needed on the raw measurements. By subtracting the pre-estimated or online estimated angular velocity and acceleration biases, the corrected angular velocity and acceleration data are obtained.

[0029] In step S2, pre-integration is performed based on the corrected motion data to obtain motion change data, including the following steps A1-A3: A1: Establish the pre-integration calculation formula.

[0030] A2: Input the corrected motion data into the pre-integral calculation formula.

[0031] A3: The pre-integration calculation formula is used to calculate the motion change data of the inspection robot.

[0032] In this embodiment of the application, the specific steps for calculating the motion change data of the inspection robot in step A3 are as follows: The formula for pre-integration is expressed as follows:

[0033]

[0034]

[0035] in, Let be the relative rotation angle of the robot between frames i and j. This represents the change in the robot's relative velocity between frames i and j. The time step index represents the robot's relative position change between frames i and j, where k is the time step index, taking values ​​between the dataset ij. Indicates time interval, This represents the rotation angle from frame i to frame k. This represents the speed change from frame i to frame k.

[0036] It should be noted that multiplication... This represents the cumulative effect of all instantaneous rotations from the 1st time step to the (j-1)th time step, ultimately yielding the total relative rotation ΔRij between two frames (from frame i to frame j); velocity increment. middle, (Rotation from i-frame to k-frame) is used to transform the acceleration ak of k-frame from the carrier coordinate system to the world coordinate system (or the i-frame coordinate system) to ensure that the acceleration integral direction is consistent.

[0037] In an optional implementation, the motion change data of the inspection robot calculated in step A3 can also be obtained using a direct integration method based on the world coordinate system. The corrected angular velocity is integrated over time to update the global attitude rotation matrix. This matrix is ​​then used to transform the acceleration in the body coordinate system to the world coordinate system and remove the gravity component. The global velocity and position are obtained sequentially through two time integrations, and the difference in absolute pose between the two time points is calculated as the relative motion change.

[0038] In another optional implementation, the motion change data of the inspection robot calculated in step A3 can also be obtained using a local integration method based on keyframe reset. At the arrival of each keyframe, the system sets the current optimal estimated pose as the initial value for integration within that segment. Only the IMU data within that segment is integrated independently to obtain the relative motion relative to the starting point of the segment. Long-term motion changes are obtained by concatenating the results of multiple local segments.

[0039] It should be noted that the IMU acquires data at a relatively high frequency, while other sensors (such as LiDAR and cameras) have relatively low sampling frequencies. IMU pre-integration can calculate the relative rotation, relative velocity, and position changes between two adjacent low-frequency sensor frames using the IMU's high-frequency data. Integrating the short-term high-frequency information from the IMU provides crucial motion increment information for state prediction and optimization.

[0040] In step S3, the robot's frame state vector at the current moment is predicted based on the motion change data, including the following steps B1-B3: B1: Establish the frame state vector prediction formula.

[0041] B2: Input motion change data into the frame state vector prediction formula.

[0042] B3: The frame state vector prediction formula outputs the robot's frame state vector at the current moment.

[0043] The formula for frame state vector prediction is:

[0044]

[0045] in, This is the robot's current frame state vector. This represents the robot's position in the current frame. This represents the robot's pose in the current frame. The current frame gyroscope bias. For the current frame accelerometer bias, Indicates matrix transpose. To remove biased accelerometer measurements, k-1 represents the state of the position frame, with the biased gyroscope measurements removed.

[0046] In step S4, the laser vision tight coupling optimization of the inspection robot is performed, and the prediction process of the frame state vector is optimized, including the following steps C1-C3: C1: Point cloud data is collected by lidar and laser point cloud matching is performed to obtain the laser observation residual.

[0047] Laser point cloud matching is performed using the following expression: , in, For laser observation residuals, For the current frame's laser point cloud, For laser observation models, This is the robot's current frame state vector.

[0048] C2: Visual observation residuals are obtained by acquiring image data through a visual sensor and performing visual feature matching.

[0049] Visual feature matching is performed using the following expression: , in, For visual observation residuals, These are the visual features captured by the vision sensor in the current frame. It is a visual observation model.

[0050] C3: Based on the laser observation residual, the visual observation residual, and the robot's frame state vector at the current moment, establish a joint optimization objective function to optimize the prediction process of the frame state vector.

[0051] In this embodiment of the application, the specific steps for establishing the joint optimization objective function in step C3 are as follows: The joint optimization objective function expression is:

[0052] in, Let λ be the optimal state estimate obtained by fusing laser and visual observation data. The weight is set based on experience and is 0.3. and Let represent the covariance matrix.

[0053] Furthermore, the visual constraint weights in the joint optimization objective function are dynamically adjusted based on obstacle density, as expressed in the following expression: , , Where ρ is the obstacle density, Let A be the number of obstacles in the region, and let A be the area of ​​the region. The obstacle density threshold is set to 0.1 based on historical data. =10 is for adjusting the speed.

[0054] It should be noted that obstacle density affects the effectiveness of visual constraints in different environmental scenarios. In areas with dense obstacles, visual feature matching may be more reliable, so the weight of visual constraints can be appropriately increased; while in areas with sparse obstacles, visual constraints may be less accurate, requiring a reduction in their weight. This can be achieved by adjusting the obstacle density... Dynamically adjusting the weights of visual constraints can make the optimization process more adaptable to different environmental scenarios.

[0055] In an alternative implementation, the joint optimization objective function established in step C3 can also employ a loosely coupled sequential optimization method based on filtering. The system uses the IMU's prediction results as a prior estimate of the state vector. The observation updates from the LiDAR and vision sensors are designed as sequential steps. For example, an EKF update is first performed using the observation residuals from LiDAR point cloud matching to obtain an intermediate state estimate. Then, a second EKF update is performed using the observation residuals from visual feature matching to obtain the final optimized state.

[0056] In another alternative implementation, the joint optimization objective function established in step C3 can also employ a loosely coupled optimization method based on fixed weights. The system simultaneously calculates residual terms from IMU pre-integration, laser matching, and visual matching. These residual terms are directly summed to form the optimization objective. The optimizer solves for the optimal state by minimizing the overall objective function.

[0057] It should be noted that LiDAR and cameras, as two different types of sensors, each have unique advantages and limitations. LiDAR can provide high-precision distance information but is not sensitive to environmental texture information; cameras can acquire rich texture and color information, but their distance measurement accuracy is relatively low. By associating and fusing the data from the two sensors through laser point cloud matching and visual feature matching, and then performing joint laser-vision optimization, the complementary information of the two sensors can be utilized to improve the accuracy and robustness of robot pose estimation.

[0058] In step S5, the state of dynamic obstacles is predicted and a collision risk assessment is performed by combining the frame state vector of the inspection robot, including the following steps D1-D4: D1: Form a time state vector based on the frame state vector of the inspection robot.

[0059] The time state vector expression is: , in, This represents the time state vector of the inspection robot. Let be the position vector, representing the three-dimensional position coordinates of the inspection robot. This is a velocity vector, representing the instantaneous velocity of the inspection robot, including its magnitude and direction. Let be the acceleration vector, representing the instantaneous acceleration of the inspection robot.

[0060] D2: Predict the time state vector of the inspection robot at the next moment based on the time state vector.

[0061] The expression for predicting the time state vector of the inspection robot at the next moment is: , in, This represents the time state vector of the inspection robot at the next moment. Represents the state transition matrix. This is state transition noise.

[0062] D3: Obtain the observation values ​​of the inspection robot on dynamic obstacles, and use the observation equation to establish the relationship between the observation values ​​and the time state vector.

[0063] The relationship between the observed values ​​and the time state vector is established using the observation equation, expressed as follows: , in, This represents the observation values ​​of the inspection robot for dynamic obstacles. Represents the observation matrix. To observe noise, reflecting sensor measurement error.

[0064] D4: Conduct a collision risk assessment based on the relationship between the observed values ​​and the time state vector.

[0065] In this embodiment of the application, the specific steps for collision risk assessment in step S5 are as follows: The expression for collision risk assessment is: , in, Let be the collision probability. The total number of time points. This indicates the position of the inspection robot at time t+1. This indicates the position of the obstacle at time t. The safe distance threshold is set based on historical experience and is 0.5m.

[0066] It should be noted that during robot inspection, the presence of dynamic obstacles poses a threat to the robot's safe operation. The state of dynamic obstacles is estimated and predicted using EKF (Extended Kalman Filter) state updates, followed by a collision risk assessment to determine the possibility of a collision between the robot and the obstacle. If a collision risk exists, timely path replanning is necessary to ensure the robot's safe operation.

[0067] In an optional implementation, the collision risk assessment in step S5 can also employ a velocity vector-based collision detection method. The obstacle's position is obtained, and its instantaneous velocity vector is estimated using the position difference between adjacent frames. This is then combined with the robot's own current velocity vector. By calculating the components of the relative velocities of the two obstacles along the line connecting their positions, the change in distance between them after a fixed time step is predicted. If the predicted distance is less than a safety threshold, a risk is deemed to exist.

[0068] In another optional implementation, the collision risk assessment in step S5 can also employ a discrete collision detection method based on bounding box overlap. The system generates a conservative bounding box containing the outer contour of the robot and each dynamic obstacle. Based on the current velocity, the positions of both are linearly extrapolated to several discrete time points in the future. At each predicted time point, it is calculated whether the bounding boxes of both parties geometrically overlap in three-dimensional space. If overlap is detected at any predicted point, a collision risk is determined to exist.

[0069] It should be noted that this invention synchronously extrapolates the motion prediction of dynamic obstacles with the robot's own state trajectory in the spatiotemporal dimension. It uses probability integral quantization within a continuous time window to calculate the cumulative proportion of time during which the minimum distance between the two vector trajectories is below a safety threshold over a future period. This mechanism enables the system not only to identify the current conflict state but also to predict potential collision trends, providing continuous risk field information for path planning.

[0070] In step S6, a path planning objective function is established based on the collision risk assessment results to perform real-time path replanning, including the following steps E1-E3: E1: Establish the objective function for path planning based on the collision risk assessment results.

[0071] E2: The path planning objective function outputs the real-time path of the inspection robot; E3: The inspection robot adjusts its path based on the real-time path output by the path planning objective function.

[0072] In this embodiment of the application, the specific steps for real-time path replanning in step S6 are as follows: The objective function expression for path planning is: , Where L is the real-time path. The weighting coefficients for obstacle avoidance parameters change in real time based on the density of environmental obstacles. The number of control points on the trajectory. For trajectory control point indexing, For the first The position of each trajectory control point In order to be with the first The location of the nearest obstacle point to each trajectory point The dynamic weighting coefficient for the velocity term is set empirically. For the first The speed of each trajectory control point This represents the time allocation for optimizing the trajectory, with the goal of minimizing the total travel time.

[0073] It should be noted that the TEB (Timed Elastic Band) algorithm is a real-time path planning algorithm that can generate a smooth and feasible path while taking into account robot dynamics constraints and environmental obstacles. When a collision risk is detected or the path needs to be replanned, real-time path replanning is performed based on the TEB algorithm to provide the robot with a new safe path, enabling it to continue completing the inspection task.

[0074] In an optional implementation, real-time path replanning in step S6 can also employ a local trajectory sampling method. The system first generates a dynamically reachable velocity window based on the robot's dynamic constraints and the current linear and angular velocities. Dense sampling is performed within this window, generating a short-term predicted trajectory for each velocity pair through forward simulation. Each trajectory is scored, and the velocity command corresponding to the trajectory with the highest score is sent to the underlying controller for execution.

[0075] In another optional implementation, real-time path replanning in step S6 can also employ a local replanning method based on incremental search. When a collision risk or local map update is detected, the system uses the robot's current position as the starting point and a fixed distance ahead of the global path as the target point, running a search algorithm in a local cost map containing information about new obstacles. The search quickly generates a collision-free grid path based on movement costs (such as distance and terrain type) and a heuristic function. This path is then smoothed and output.

[0076] It should be noted that this invention introduces a dynamic weight adjustment strategy driven by real-time obstacle density into the classic Timed Elastic Band (TEB) framework, enabling the optimization objective function to adapt to environmental complexity. The path planning objective function not only generates a kinematically feasible trajectory in the tunnel but also makes an optimal trade-off between safety, efficiency, and smoothness based on gradient information from the risk field. This overcomes the response lag, trajectory jitter, or local deadlock problems caused by fixed parameters or limited planning perspective in traditional replanning methods.

[0077] In summary, this invention achieves real-time localization in feature-sparse tunnel environments through IMU pre-integration and adaptive tightly coupled optimization techniques. It addresses the problem of fixed weights and difficulty in adapting to complex and variable scenarios in traditional fusion algorithms by employing a dynamic weight adjustment mechanism based on real-time obstacle density. Furthermore, it achieves active obstacle avoidance for both static and dynamic obstacles through collision risk assessment and path planning, overcoming the shortcomings of existing methods such as low planning success rate and uneven trajectory due to response lag or parameter rigidity.

[0078] Example 3 is an embodiment of the present invention. This embodiment provides an autonomous obstacle avoidance and inspection system for cable tunnels based on multi-sensor fusion, including: a correction module, a calculation module, a prediction module, an optimization module, a collision risk assessment module, and an implementation path planning module. The correction module corrects the motion data collected by the inspection robot to obtain corrected motion data. The calculation module performs pre-integration calculations based on the corrected motion data to obtain motion change data. The prediction module predicts the robot's frame state vector at the current moment based on motion change data; The optimization module performs laser vision tight coupling optimization for the inspection robot, optimizing the prediction process of the frame state vector. The collision risk assessment module predicts the state of dynamic obstacles and assesses the collision risk by combining the frame state vector of the inspection robot. The implementation path planning module establishes a path planning objective function based on the collision risk assessment results to perform real-time path replanning.

[0079] This embodiment also provides an electronic device applicable to a method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion, comprising: a memory and a processor; the memory is used to store computer-executable instructions, and the processor is used to execute the computer-executable instructions to realize the method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as proposed in the above embodiment.

[0080] This embodiment also provides a storage medium storing a computer program that, when executed by a processor, implements an autonomous obstacle avoidance and inspection method for cable tunnels based on multi-sensor fusion as proposed in the above embodiment.

[0081] The storage medium proposed in this embodiment belongs to the same inventive concept as the method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion proposed in the above embodiments. Technical details not described in detail in this embodiment can be found in the above embodiments, and this embodiment has the same beneficial effects as the above embodiments.

[0082] Based on the above description of the implementation methods, those skilled in the art can clearly understand that the present invention can be implemented using software and necessary general-purpose hardware, and of course, it can also be implemented using hardware, but in many cases the former is a better implementation method. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as a computer floppy disk, read-only memory (ROM), random access memory (RAM), flash memory, hard disk, or optical disk, etc., including several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods of the various embodiments of the present invention.

[0083] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion, characterized in that: include, The motion data collected by the inspection robot is corrected to obtain the corrected motion data. Pre-integration is performed based on the corrected motion data to obtain motion change data; Predict the robot's frame state vector at the current moment based on motion change data; Perform laser vision tight coupling optimization on the inspection robot and optimize the prediction process of frame state vector; State prediction of dynamic obstacles and collision risk assessment based on the frame state vector of the inspection robot; A path planning objective function is established based on the collision risk assessment results to perform real-time path replanning.

2. The method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as described in claim 1, characterized in that: The process of pre-integrating based on the corrected motion data to obtain motion change data includes: Establish the pre-integral calculation formula; Input the corrected motion data into the pre-integral calculation formula; The motion change data of the inspection robot are calculated using the pre-integration calculation formula.

3. The method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as described in claim 2, characterized in that: The step of predicting the robot's frame state vector at the current moment based on motion change data includes: Establish the frame state vector prediction formula; Input motion change data into the frame state vector prediction formula; The frame state vector prediction formula outputs the robot's frame state vector at the current moment.

4. The method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as described in claim 3, characterized in that: The laser vision tight coupling optimization of the inspection robot, and the optimization of the frame state vector prediction process, includes: Point cloud data is collected by lidar and laser point cloud matching is performed to obtain the laser observation residual; Visual observation residuals are obtained by acquiring image data through a visual sensor and performing visual feature matching. Based on the laser observation residual, the visual observation residual, and the robot's frame state vector at the current moment, a joint optimization objective function is established to optimize the prediction process of the frame state vector.

5. The method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as described in claim 4, characterized in that: The process of predicting the state of dynamic obstacles and assessing collision risk by combining the frame state vector of the inspection robot includes: The time state vector is formed based on the frame state vector of the inspection robot; The expression for predicting the inspection robot's time state vector at the next moment based on the time state vector is: , in, This represents the time state vector of the inspection robot at the next moment. Represents the state transition matrix. This is state transition noise; The inspection robot obtains observations of dynamic obstacles, and establishes the relationship between the observations and the time state vector using the observation equation. The expression is as follows: , in, This represents the observation values ​​of the inspection robot for dynamic obstacles. Represents the observation matrix. To observe noise and reflect sensor measurement errors; Collision risk assessment is performed based on the relationship between observed values ​​and the time state vector, expressed as follows: , in, Let be the collision probability. The total number of time points. This indicates the position of the inspection robot at time t+1. This indicates the position of the obstacle at time t. This is the safe distance threshold.

6. The method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as described in claim 5, characterized in that: The step of establishing a path planning objective function based on collision risk assessment results and performing real-time path replanning includes: Based on the collision risk assessment results, a path planning objective function is established, expressed as follows: ， Where L is the real-time path. The weighting coefficients for the obstacle avoidance term. The number of control points on the trajectory. For trajectory control point indexing, For the first The position of each trajectory control point In order to be with the first The location of the nearest obstacle point to the trajectory point The dynamic weighting coefficient for the velocity term. For the first The speed of each trajectory control point This represents the time allocation for optimizing the trajectory; The path planning objective function outputs the real-time path of the inspection robot; The inspection robot adjusts its path based on the real-time path output by the path planning objective function.

7. The method for autonomous obstacle avoidance and inspection of cable tunnels based on multi-sensor fusion as described in claim 6, characterized in that: The process of establishing a joint optimization objective function to optimize the prediction of the frame state vector based on the laser observation residual, the visual observation residual, and the robot's frame state vector at the current moment includes: The prediction process for optimizing the frame state vector using a joint optimization objective function is established, and its expression is: ， in, This represents the optimal state estimate obtained by fusing laser and visual observation data. This indicates the initial state of the inspection robot. For laser observation residuals, This is the robot's current frame state vector. This represents the robot's position in the current frame. Let λ be the robot's pose in the current frame. Weight, and Represents the covariance matrix. For visual observation residuals; The visual constraint weights in the joint optimization objective function are dynamically adjusted based on obstacle density, and the expression is as follows: , , Where ρ is the obstacle density, Let A be the number of obstacles in the region, and let A be the area of ​​the region. The obstacle density threshold. To adjust the speed.

8. A cable tunnel autonomous obstacle avoidance inspection system based on multi-sensor fusion, employing the cable tunnel autonomous obstacle avoidance inspection method based on multi-sensor fusion as described in any one of claims 1-7, characterized in that, include: The system includes a correction module, a calculation module, a prediction module, an optimization module, a collision risk assessment module, and an implementation path planning module. The correction module corrects the motion data collected by the inspection robot to obtain corrected motion data. The calculation module performs pre-integration calculations based on the corrected motion data to obtain motion change data. The prediction module predicts the robot's frame state vector at the current moment based on motion change data; The optimization module performs laser vision tight coupling optimization for the inspection robot, optimizing the prediction process of the frame state vector. The collision risk assessment module predicts the state of dynamic obstacles and assesses the collision risk by combining the frame state vector of the inspection robot. The implementation path planning module establishes a path planning objective function based on the collision risk assessment results to perform real-time path replanning.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the autonomous obstacle avoidance and inspection method for cable tunnels based on multi-sensor fusion as described in any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the autonomous obstacle avoidance and inspection method for cable tunnels based on multi-sensor fusion as described in any one of claims 1 to 7.

Images