Path planning system and method for unmanned inspection equipment

Abstract

This invention provides a path planning system and method for unmanned inspection equipment, belonging to the field of power inspection technology. In existing technologies, although lidar inspection robots have significant advantages in routine tasks, their technical limitations are gradually exposed in complex scenarios. This invention employs the MPC algorithm, which has powerful optimization and control capabilities. Based on the path planned by the A* algorithm, and combined with the kinematic and dynamic characteristics of the device, it can accurately predict and optimize the future trajectory of the device.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of power inspection, in particular to a path planning system and method of unmanned inspection equipment. BACKGROUND

[0002] With the acceleration of global energy transformation, smart grid has become the core infrastructure to support large-scale access of renewable energy and improve the reliability of power grid. According to the statistics of International Energy Agency (IEA), the proportion of global smart substations is expected to exceed 40% in 2025. Under this background, the substation inspection mode is rapidly evolving from traditional manual inspection to "unmanned, less manned and intelligent".

[0003] The substation inspection robot based on laser radar navigation technology has become the mainstream solution in the industry due to its high-precision positioning, path planning and multi-sensor fusion capabilities. This kind of robot constructs a three-dimensional map of the environment by emitting laser beams, plans the optimal inspection path, and carries infrared, visible light, gas sensors and other devices to realize device temperature monitoring, appearance defect identification and gas leakage detection functions. Its application has significantly improved the operation efficiency and safety, and has performed outstandingly in routine inspection scenarios.

[0004] Although the laser radar inspection robot has obvious advantages in routine tasks, its technical limitations are gradually exposed in complex scenarios, especially in emergency response, perception ability and operation flexibility. SUMMARY

[0005] In view of the problems in the prior art, the present application provides a path planning method of unmanned inspection equipment, which comprises the following steps: Step S1: the unmanned inspection equipment uses a SLAM module for positioning and constructing a three-dimensional map; Step S2: using A* algorithm for path planning; Step S3: based on the path planned by A* algorithm, using MPC algorithm combined with the kinematics and dynamics characteristics of the unmanned inspection equipment, accurately predicting and optimizing its future motion trajectory; Step S4: using a depth camera to avoid obstacles; Step S5: when the unmanned inspection equipment needs to adjust the heading to avoid obstacles and enter the next target point, using the LOS algorithm to calculate the expected heading according to the current position and the desired waypoint, and providing real-time navigation guidance.

[0006] Preferably, the method further comprises step S6: using an EKF module to predict the state at the current time according to the state estimation value at the last time and the system model; then the EKF module corrects the predicted value using the sensor observation value at the current time; the state at the current time includes position, velocity and direction.

[0007] Preferably, the method further comprises a step S7 of using an iteration module to continuously correct the control input in the control process based on the output error information obtained in the previous operation of the system, so as to gradually improve the control effect of the next time.

[0008] Preferably, after the initial path is obtained by using the A* algorithm in the step S2, before the future motion trajectory is accurately predicted and optimized in the step S3, the method further comprises a step of performing path node optimization processing on the initial path, the path node optimization processing comprising: merging or removing redundant path nodes in the initial path, and constraining the heading change of adjacent path segments, so as to generate a reference path with better direction continuity, thereby improving the trajectory tracking stability of the subsequent model predictive control algorithm.

[0009] Preferably, in the step S2, the A* algorithm evaluates the estimated cost from the current node to the target node by introducing a heuristic function, so that the search process can more intelligently select the path.

[0010] The application also provides a path planning system of an unmanned inspection device, the system comprising: a SLAM module, an A* module, an MPC module, an obstacle avoidance module and a LOS module. The SLAM module performs positioning of the unmanned inspection device and constructs a three-dimensional map. The A* module performs path planning. Based on the path planned by the A* algorithm, the MPC module accurately predicts and optimizes the future motion trajectory of the unmanned inspection device in combination with the kinematic and dynamic characteristics of the unmanned inspection device. The obstacle avoidance module uses a depth camera for obstacle avoidance. When the unmanned inspection device needs to adjust the heading to avoid obstacles and enter the next target point, the LOS module calculates the expected heading according to the current position and the expected navigation point, and provides real-time navigation guidance.

[0011] Preferably, the system further comprises an EKF module, which predicts the state at the current time according to the state estimation value at the previous time and the system model; then the EKF module corrects the predicted value by using the sensor observation value at the current time; the state at the current time comprises position, velocity and direction.

[0012] Preferably, the system further comprises an iteration module, which continuously corrects the control input in the control process based on the output error information obtained in the previous operation of the system, so as to gradually improve the control effect of the next time.

[0013] Preferably, the unmanned inspection device further comprises a task management module, and the system obtains a preset task from the task management module.

[0014] Preferably, the A* module evaluates an estimated cost from a current node to a target node by introducing a heuristic function, so that the search process can more intelligently select a path.

[0015] The innovations of the embodiments of the present specification include: 1. The present application has strong optimization and control ability by adopting the MPC algorithm, can accurately predict and optimize the future motion trajectory based on the path planned by the A* algorithm combined with the kinematics and dynamics characteristics of the device; 2. The present application can calculate the expected heading according to the current position and the desired waypoint by adopting the LOS algorithm, and provide real-time navigation guidance for the device. This algorithm combined with the depth camera obstacle avoidance technology provides a more intelligent and flexible navigation strategy for the remote emergency inspection device in complex environment. BRIEF DESCRIPTION OF DRAWINGS

[0016] In order to more clearly illustrate the technical solutions in the embodiments of the present specification or the related art, the drawings needed to be used in the embodiments or the related art description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and those skilled in the art can obtain other drawings according to these drawings without creative labor.

[0017] Figure 1 The flowchart of the automatic inspection task provided by the present application is shown in the figure; Figure 2 The path planning schematic diagram of the present application using A* algorithm is shown in the figure; Figure 3 The MPC algorithm schematic diagram provided by the present application is shown in the figure; Figure 4 The LOS algorithm schematic diagram provided by the present application is shown in the figure. DETAILED DESCRIPTION

[0018] The technical solutions in the embodiments of the present specification will be described clearly and completely in the following with reference to the drawings in the embodiments of the present specification. Obviously, the described embodiments are only some embodiments of the present application, not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the present application.

[0019] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this specification are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0020] This specification discloses a path planning system and method, which will be described in detail below.

[0021] The intelligent power inspection system provided by this invention features multiple modes, including fully automatic intelligent inspection, autonomous inspection, and semi-automatic inspection. Users can quickly switch between different inspection modes according to different inspection needs and scenarios, allowing for manual takeover or autonomous / semi-autonomous operation, thus improving the adaptability and practicality of the unmanned vehicle. Compared to traditional power inspection robots, this represents a completely new inspection mode and experience, improving inspection efficiency and convenience. The new power inspection operation system includes an automatic inspection task module, an autonomous + intelligent integrated inspection operation strategy, and a remote immersive inspection operation system.

[0022] The automatic inspection module includes a task management module, a positioning and navigation module, a visual information acquisition module, and an anomaly detection module.

[0023] (1) Task Management The automated inspection module boasts powerful task management capabilities, enabling centralized management of all inspection tasks. Users can add, edit, and delete inspection tasks, and set corresponding execution plans. Furthermore, the module supports custom task templates, recording inspection routes remotely and recording equipment requiring inspection along the route via teaching, facilitating the rapid creation of similar tasks.

[0024] Create automated inspection tasks In the automated inspection module, users can use the teleoperation function to control the unmanned vehicle (RV) to the inspection point from a remote control console. The system teaches the RV to record environmental information and the equipment to be inspected at each inspection point, specifying the appropriate posture. Recording the inspection route and equipment inspection process in this way significantly improves the accuracy and efficiency of the inspection task, avoiding the tedious data entry required by other inspection methods and reducing manual intervention and missed inspections during the automated inspection process. Through teleoperation and teaching, users can pre-plan the inspection route and perform simulated operations on the control console, inspecting the equipment along the way. Simultaneously, the system automatically records inspection task information, forming a complete inspection task plan. This approach not only improves the accuracy of the inspection task but also reduces the time spent on manual settings and adjustments, thereby increasing work efficiency.

[0025] In addition to remote operation and teaching, the task management module supports various other methods for recording inspection routes and equipment inspection processes. For example, users can create and edit inspection tasks by manually drawing inspection routes, importing map data, or using preset templates. These methods can meet the needs of different users and provide more flexible and convenient task management functions.

[0026] Automatically execute inspection tasks To execute an automated inspection task, the device needs to be set to automatic control mode on the control terminal, then the automated inspection task needs to be selected and sent to the remote emergency inspection device. After receiving the automated inspection task, the device plans its route using SLAM technology and proceeds to the inspection points in the task sequentially. Upon reaching an inspection point, the device performs the required actions according to the task requirements of that point and saves the equipment and environmental data for that point before proceeding to the next inspection point.

[0027] The module has a built-in special inspection task: the automatic return task, with the inspection point set at the charging station. After the operator clicks the automatic return button on the console software interface, the device begins to execute the automatic return task, navigating to the charging station via SLAM and starting charging.

[0028] Task Data Management The inspection task management interface can be accessed through the device's console software. Within the inspection task management module, the download management button allows you to view all completed tasks on the device. Selecting a task allows you to download the task data to the console. The console can also be filtered by task time to list downloaded automatic inspection tasks. Selecting an inspection task and clicking the export report button allows you to view the device and environmental data recorded for that task.

[0029] (2) Positioning and navigation The automated inspection module employs 3D LiDAR and SLAM navigation technology to achieve high-precision positioning and autonomous navigation. The 3D LiDAR emits laser beams and receives reflected signals to obtain detailed information about the surrounding environment, such as obstacles and terrain. SLAM technology can build a map and position information of the unmanned vehicle in real time, enabling autonomous navigation and positioning. This function can also combine map data to intuitively display the device's location, planned inspection routes, and historical movement trajectories.

[0030] The key to SLAM technology lies in its algorithm's processing and analysis of sensed data. The algorithm within this device preprocesses environmental sensor data, including filtering, feature extraction, and feature matching, to eliminate noise and redundant information and extract key environmental features. The algorithm then uses these features for pose estimation, that is, to estimate the precise position and orientation of the remote emergency inspection device in the environment. This is typically achieved through algorithms such as Extended Kalman Filter (EKF) and Particle Filter (PF), ensuring that the remote emergency inspection device maintains accurate awareness of its own position as it moves.

[0031] Meanwhile, SLAM technology possesses map-building capabilities. Based on localization, the remote emergency inspection device gradually constructs a map of the environment using its own sensory data. As the remote emergency inspection device moves and sensory data accumulates, the map is continuously improved and updated. There are various map-building methods, such as raster maps, topological maps, and point cloud maps, each with its unique advantages and applicable scenarios. By building maps, the remote emergency inspection device can gain a deeper understanding of the environment, thereby better performing tasks such as path planning, obstacle avoidance, and target localization.

[0032] The advantage of SLAM technology lies in its ability to simultaneously perform localization and map building, two tasks that are inseparable in the navigation of remote emergency inspection devices. By sensing the environment in real time and building a map, this device can autonomously navigate and explore in unknown environments. This technology enables the device to maintain efficient and stable operation in complex and changing environments, greatly improving its adaptability and autonomy.

[0033] A* Algorithm For path planning, the A* algorithm can be used. Based on an established SLAM 3D map, this algorithm automatically searches for and determines an optimal path from the starting point to the destination, taking into account factors such as the kinematic constraints of the vehicle, environmental obstacles, and the task requirements of the robotic arm. The A* algorithm is an efficient graph traversal and path search algorithm that combines the characteristics of best-first search (Dijkstra's algorithm) and heuristic search (such as the greedy algorithms in BFS or DFS).

[0034] The A* algorithm introduces a heuristic function to evaluate the estimated cost from the current node to the target node, enabling the search process to select paths more intelligently and effectively avoiding the tediousness and inaccuracy of manually setting waypoints. In the navigation tasks of robotic arms, the A* algorithm can autonomously plan optimal or suboptimal trajectories, allowing the robotic arm to complete navigation tasks more autonomously and efficiently.

[0035] Specifically, the A* algorithm defines the following evaluation function for any search node n: f(n) = g(n) + h(n) Where g(n) represents the actual cumulative path cost from the starting node to the current node n, and the path cost can be determined based on factors such as the geometric distance between nodes, travel time or energy consumption; h(n) represents the heuristic estimated cost from the current node n to the target node, which is used to guide the search direction and reduce the expansion of invalid nodes.

[0036] In this embodiment, the heuristic function h(n) is constructed using an estimation function that does not exceed the actual minimum path cost, ensuring its acceptability and guaranteeing that the A* algorithm obtains a globally optimal or suboptimal path upon termination of the search. Through this method, the A* algorithm can prioritize expanding nodes with lower overall costs during the search process, giving the path planning process a clear optimization objective and search direction.

[0037] Compared with path planning methods based on manually preset waypoints, this embodiment uses the A* algorithm to automatically generate continuous path nodes in the constructed map space without the need for manual setting of intermediate waypoints. This reduces the complexity of path planning and human error, improves the accuracy and stability of path planning, and provides a reliable reference path for subsequent trajectory optimization based on model predictive control algorithms.

[0038] Preferably, in practical engineering applications, paths generated based on the A* algorithm typically consist of a large number of discrete nodes, and there may be significant directional changes between adjacent path segments. Although such paths meet the optimality requirements geometrically, they do not fully comply with the kinematic continuity constraints of unmanned inspection equipment, easily leading to frequent changes in control variables during subsequent trajectory tracking, thereby increasing the computational burden on the model predictive control algorithm and even causing path tracking instability. To address these issues, this invention preferably introduces a path node optimization processing mechanism between the A* algorithm path planning results and the model predictive control algorithm. This mechanism analyzes the original path node sequence output by the A* algorithm, merges or removes redundant nodes, and constrains the heading changes of adjacent path segments, so that the processed path has better directional continuity and trackability while maintaining the overall planned direction.

[0039] Through the above path node optimization, the model predictive control algorithm can predict and optimize the trajectory based on a reference path that is more in line with the kinematic characteristics, effectively reducing the fluctuation range of the control input, improving the stability and reliability of the unmanned inspection equipment in complex environments, and thus enhancing the engineering applicability of the entire path planning and control system.

[0040] MPC Model Predictive Control Algorithm To achieve precise trajectory tracking, this solution can also incorporate the MPC (Model Predictive Control) algorithm. The MPC algorithm possesses powerful optimization and control capabilities, capable of accurately predicting and optimizing the future trajectory based on the path planned by the A* algorithm, combined with the kinematic and dynamic characteristics of the device. By continuously adjusting control parameters, the MPC algorithm ensures that the robotic arm closely tracks the planned trajectory, while achieving accurate and stable navigation in open spaces, providing an efficient and reliable solution for operations in complex environments.

[0041] Depth camera obstacle avoidance Substation environments are complex, and in certain special circumstances, such as when passageways are too narrow and the accuracy of lidar is limited, relying solely on lidar for obstacle avoidance may not meet navigation requirements. In such cases, the platform also needs to utilize depth cameras for obstacle avoidance. Depth cameras achieve real-time perception of the surrounding environment by emitting infrared light or other forms of light signals and receiving the information reflected back from objects. They can capture distance and shape data of objects, generating 3D images or point clouds, providing the system with rich environmental information, and ensuring that the inspection device can safely pass through narrow areas.

[0042] LOS line-of-sight guidance algorithm When a remote emergency patrol device needs to adjust its course to avoid obstacles and reach the next target point, the LOS algorithm can calculate the desired course based on the current position and the desired waypoint, providing real-time navigation guidance for the device. This algorithm, combined with depth camera obstacle avoidance technology, provides a more intelligent and flexible navigation strategy for remote emergency patrol devices in complex environments.

[0043] Data fusion algorithm based on extended Kalman filter Kalman filtering is a classic data fusion algorithm suitable for linear systems. However, in practical applications, many systems are nonlinear, requiring the use of extended Kalman filtering (EKF). EKF handles nonlinear systems through linearization approximation, allowing the Kalman filtering concept to be applied to nonlinear environments. The basic principle of EKF is to predict and update the state estimate at each time step by approximating the linearization of the nonlinear system. Specifically, EKF consists of two main stages: a prediction stage and an update stage. In the prediction stage, EKF first predicts the current state based on the state estimate from the previous time step and the system model. Since the system is nonlinear, EKF uses Taylor series expansion or other linearization methods to linearize the nonlinear model. Then, based on the linearized model, EKF calculates the predicted state and the prediction error covariance matrix for the current time step.

[0044] During the update phase, EKF corrects the predicted values ​​using the current sensor observations. Similarly, since the observation equations may be nonlinear, EKF linearizes them. Then, based on the linearized observation equations and the observed values, EKF calculates the Kalman gain (also called the weighting factor). Next, EKF uses the Kalman gain to weight and fuse the predicted and observed values ​​to obtain the state estimate for the current time step. Finally, EKF updates the error covariance matrix for use in the next time step. Implementing EKF requires defining the system's state vector, observation vector, system model, and observation model. The system state vector typically contains the variables to be estimated, such as position and velocity; the observation vector contains the sensor's observation data. The system model and observation model describe how the system state changes over time and how the sensors observe the system state. These models can be determined using experimental data or physical principles. The filtered pose noise is relatively low and can be used as a reference state for the device.

[0045] Iterative learning control algorithm The principle of iterative learning control algorithm for remote emergency patrol devices is mainly based on the output error information obtained from the previous system operation. During the control process, the control input is continuously corrected to gradually improve the control effect in the next operation. This control strategy is particularly suitable for systems with unknown models and repetitive motion characteristics. The characteristic of iterative learning control is "learning through repetition," achieving improved control performance through repeated iterations. The principle of iterative learning control is to continuously correct the control input based on the output error information obtained from the previous system operation. As iterations proceed, the control input is gradually improved for the next operation, resulting in increasingly better control performance. Iterative learning control requires relatively little prior knowledge and is an effective control strategy for systems with unknown models and repetitive motion characteristics.

[0046] In conjunction with the task management module, this device can automatically plan patrol routes and autonomously patrol according to preset task requirements. The task management module is responsible for task scheduling and execution, and can set different task plans based on patrol needs, such as scheduled patrols and area patrols. When performing patrol tasks, this device autonomously plans its route based on the task plan and SLAM navigation technology to ensure accurate arrival at the target location and successful patrol.

[0047] Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing the present invention.

[0048] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0049] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method for an unmanned inspection device, characterized in that, The method includes the following steps: Step S1: The unmanned inspection equipment uses a SLAM module for localization and to build a 3D map; Step S2: Use the A* algorithm for path planning; Step S3: Based on the path planned by the A* algorithm, the MPC algorithm is used in conjunction with the kinematic and dynamic characteristics of the unmanned inspection equipment to accurately predict and optimize its future trajectory. Step S4: Use a depth camera for obstacle avoidance; Step S5: When the unmanned inspection equipment needs to adjust its course to avoid obstacles and enter the next target point, the LOS algorithm is used to calculate the desired course based on the current position and the desired waypoint, providing real-time navigation guidance.

2. The method according to claim 1, characterized in that: The method further includes step S6: using an EKF module to predict the current state based on the state estimate of the previous moment and the system model; then the EKF module uses the sensor observations of the current moment to correct the predicted value; the current state includes position, velocity and direction.

3. The method according to claim 2, characterized in that: The method further includes step S7: using an iterative module to continuously correct the control input during the control process based on the output error information obtained from the previous system run, so as to gradually improve the control effect in the next run.

4. The path planning method for unmanned inspection equipment according to claim 1, characterized in that: After obtaining the initial path using the A* algorithm in step S2 and before accurately predicting and optimizing its future trajectory in step S3, the method further includes a path node optimization process for the initial path. The path node optimization process includes merging or removing redundant path nodes in the initial path and constraining the heading changes of adjacent path segments to generate a reference path with better directional continuity, thereby improving the trajectory tracking stability of the subsequent model predictive control algorithm.

5. The method according to claim 1, characterized in that: In step S2, the A* algorithm introduces a heuristic function to evaluate the estimated cost from the current node to the target node, enabling the search process to select paths more intelligently.

6. A path planning system for unmanned inspection equipment, characterized in that: The system includes: a SLAM module, an A* module, an MPC module, an obstacle avoidance module, and a LOS module; The SLAM module is used to locate the unmanned inspection equipment and build a 3D map. The A* module performs path planning; Based on the path planned by the A* algorithm, the MPC module combines the kinematic and dynamic characteristics of the unmanned inspection equipment to accurately predict and optimize its future trajectory. The obstacle avoidance module uses a depth camera for obstacle avoidance. When the unmanned inspection equipment needs to adjust its course to avoid obstacles and enter the next target point, the LOS module calculates the desired course based on the current position and the desired waypoint, and provides real-time navigation guidance.

7. The system according to claim 6, characterized in that: The system also includes an EKF module, which predicts the current state based on the state estimate from the previous moment and the system model; then the EKF module uses the sensor observations from the current moment to correct the predicted value; the current state includes position, velocity, and direction.

8. The system according to claim 7, characterized in that: The system also includes an iteration module, which continuously corrects the control input during the control process based on the output error information obtained from the previous operation of the system, so as to gradually improve the control effect in the next operation.

9. The system according to claim 6, characterized in that: The unmanned inspection equipment also includes a task management module, from which the system obtains preset tasks.

10. The system according to claim 6, characterized in that: The A* module introduces a heuristic function to evaluate the estimated cost from the current node to the target node, enabling the search process to select paths more intelligently.

Images