Environment sensing and positioning method, device and equipment of inspection robot and medium

Abstract

The invention discloses an environment sensing and positioning method, device and equipment of an inspection robot and a medium, and relates to the technical field of robot navigation, and the method comprises the steps: collecting environment data through a sensor on the inspection robot; predicting a system state and a covariance of the inspection robot according to the environment data, and updating the system state and the covariance; performing local mapping according to the updated system state to update a pre-constructed global map; determining the current location of the inspection robot according to the environment data and the updated global map; planning in the updated global map according to the current positioning of the inspection robot to obtain a moving track; and controlling the inspection robot to move according to the moving track, so that the inspection robot reaches each preset inspection point for inspection. The positioning and navigation capabilities of the inspection robot are improved in three aspects of sensing, positioning and navigation, so that the inspection robot can adapt to environments with complex terrains, and the inspection efficiency and accuracy are improved.

Description

Technical Field

[0001] This application relates to the field of robot navigation technology, in particular to an environmental perception and positioning method, device, equipment and medium for inspection robots. Background Art

[0002] As a special industrial facility with high safety risks, the internal environment of nuclear power plants has characteristics such as strong radiation, high temperature and pressure, complex enclosure, and potential leakage of harmful gases. Traditional manual inspection methods have inherent defects such as cumulative radiation dose to personnel, poor accessibility in high-risk areas, and strong subjectivity of detection data. Especially in accident emergency conditions, it may cause major safety hazards. In recent years, intelligent inspection robots have been gradually applied to fields such as equipment status monitoring, radiation dose mapping, and anomaly detection in nuclear power plants, becoming a key technical means to improve the safety and intelligence level of operation and maintenance.

[0003] However, the internal structure of the nuclear island building has the following typical challenges: (1) Dense pipe galleries and shielding walls cause satellite positioning signals to fail; (2) High radiation field strength leads to sensor noise drift and performance degradation of electronic devices; (3) Dynamic obstacles (such as mobile equipment, condensation water vapor) interfere with environmental modeling. Existing inspection robots mostly adopt a single SLAM (Simultaneous Localization and Mapping) or inertial navigation scheme, with problems such as cumulative positioning errors, insufficient resolution of three-dimensional environmental perception, and sensor failure under radiation interference, seriously restricting the reliable operation ability of robots in nuclear environments. Summary of the Invention

[0004] In view of this, embodiments of this application provide an environmental perception and positioning method, device, equipment and medium for inspection robots to improve the navigation and positioning ability of inspection robots.

[0005] One aspect of the embodiments of this application provides an environmental perception and positioning method for inspection robots, the method comprising the following steps:

[0006] Collect environmental data through sensors on the inspection robot;

[0007] Predict the system state and covariance of the inspection robot based on the environmental data and then update the system state and the covariance;

[0008] Perform local mapping based on the updated system state to update a pre-constructed global map;

[0009] Determine the current position of the inspection robot based on the environmental data and the updated global map;

[0010] Plan a movement trajectory in the updated global map based on the current position of the inspection robot;

[0011] Control the inspection robot to move along the moving trajectory so that the inspection robot reaches each preset inspection point for inspection.

[0012] In some embodiments, the sensor includes a lidar and an RGBD camera, the environmental data includes laser scan points and RGB depth images that reach a preset frequency, and predicting the system state and covariance of the inspection robot based on the environmental data includes the following steps:

[0013] Segment the laser scan points according to the sampling time of the RGBD camera to synchronize the frequencies of the laser scan points and the RGB depth images;

[0014] Predict the system state and covariance of the inspection robot according to a predefined state transition model and IMU measurement data in the environmental data;

[0015] The expression of the state transition model is:

[0016] ;

[0017] where ⊕ represents the Lie group exponential map, is the state transition function, including IMU measurement data and process noise data ; represents the current system state, represents the predicted system state;

[0018] The expression for predicting the covariance is:

[0019] ;

[0020] where, is the covariance matrix; is the state transition Jacobian matrix; is the Jacobian matrix of the process noise; is the process noise covariance matrix, including the noise of IMU measurement data and the bias random walk noise.

[0021] In some embodiments, updating the system state and covariance includes the following steps:

[0022] Update the error state by iteratively calculating the residual and the Jacobian matrix until the error state converges, and then the system state and covariance converge to obtain the updated system state and covariance;

[0023] The residual is the residual of the laser scan points in the environmental data to the plane, and the expression of the residual is:

[0024] ;

[0025] Wherein, is the residual; is the pose of the IMU in the sensor in the global coordinate system; is the extrinsic parameter of the lidar in the sensor relative to the IMU; is the coordinate of the laser scan point in the coordinate system of the lidar; , is the normal vector and the center point of the voxel plane;

[0026] The expression for updating the covariance is:

[0027] ;

[0028] Wherein, is the covariance; is the Kalman gain coefficient; is the measurement Jacobian matrix;

[0029] The expression for the Kalman gain coefficient is:

[0030] ;

[0031] Wherein, is the measurement noise covariance matrix.

[0032] In some embodiments, the local mapping according to the updated system state to update the pre-constructed global map includes the following steps:

[0033] Determine the laser scan points obtained by scanning the lidar in the sensor according to the updated system state;

[0034] Register the scan points into the global framework, and judge whether a plane is formed by using singular value decomposition according to the distribution of the scan points in the voxel;

[0035] Process the newly created and existing voxels respectively, update the plane parameters and uncertainties, and obtain mature planes;

[0036] Select target mature planes from each of the mature planes in the local map;

[0037] Generate visual map points according to the target mature plane;

[0038] Update the pre-constructed global map according to the visual map points.

[0039] In some embodiments, before selecting a target mature plane from each of the mature planes in the local map, the method further includes the following steps:

[0040] Determine a visual submap of the current frame using the laser scan points and the visible points of the previous frame for visible voxel query;

[0041] Supplement the visual map points when the laser scan points are missing to achieve ray casting on demand;

[0042] Remove the laser scan points with occlusion, depth discontinuity, and excessive viewing angle.

[0043] In some embodiments, planning the movement trajectory in the updated global map according to the current positioning of the inspection robot includes the following steps:

[0044] Use the Astar algorithm to generate a path between the current positioning of the inspection robot and each of the inspection points as the initial trajectory;

[0045] Construct a safety corridor composed of a series of polyhedrons or spheres;

[0046] The polyhedron is composed of multiple constraint planes:

[0047] ;

[0048] Wherein, represents a certain point on the surface of the polyhedron and is the corresponding normal vector; the movement trajectory is constrained in the intersection area of the polyhedrons;

[0049] Generate a MINCO type trajectory according to the initial trajectory and make the MINCO type trajectory within the safety corridor;

[0050] The expression of the MINCO type trajectory is:

[0051] ;

[0052] represents the set of MINCO type trajectories; p(t) is a trajectory function that maps from the time interval to space; represents a function mapping relationship; represents a waypoint; the MINCO type trajectory is divided into M sub-trajectories, and T represents the time allocation vector of each sub-trajectory;

[0053] The representation of each sub-trajectory is:

[0054] ;

[0055] Among them, is the polynomial basis function vector; is the time of the i-th segment of the trajectory, is the polynomial coefficient matrix of the i-th segment of the trajectory, because c is also and function of:

[0056] ;

[0057] Optimize the MINCO class trajectory within the safety corridor to obtain the movement trajectory.

[0058] In some embodiments, the optimizing the MINCO class trajectory within the safety corridor to obtain the movement trajectory includes the following steps:

[0059] Optimize the MINCO class trajectory within the safety corridor by using the optimization equation, trajectory continuity equation, robot dynamics equation, time limit equation, obstacle avoidance constraint equation and obstacle avoidance cost function to obtain the movement trajectory;

[0060] The optimization equation is:

[0061] ;

[0062] The trajectory continuity equation is:

[0063] ;

[0064] ;

[0065] ;

[0066] Among them, represents the s-th sub-trajectory, represents the first derivative of the position;

[0067] The robot dynamics equation is:

[0068] ;

[0069] ;

[0070] ;

[0071] Among them, respectively represent the maximum speed and maximum acceleration, which are characterized by a series of soft constraints as ;

[0072] The time limit equation is as follows:

[0073] ;

[0074] Wherein, represents the time limit parameter;

[0075] The obstacle avoidance constraint equation is as follows:

[0076] ;

[0077] The obstacle avoidance cost function is as follows:

[0078] ;

[0079] Wherein, is the penalty term weight, is the obstacle position information.

[0080] On the other hand, an environment perception and positioning device for an inspection robot provided by an embodiment of the present application includes:

[0081] A data acquisition unit, configured to acquire environmental data through sensors on the inspection robot;

[0082] A prediction and update unit, configured to predict the system state and covariance of the inspection robot according to the environmental data, and then update the system state and the covariance;

[0083] A map update unit, configured to perform local mapping according to the updated system state to update a pre-constructed global map;

[0084] A positioning unit, configured to determine the current position of the inspection robot according to the environmental data and the updated global map;

[0085] A trajectory planning unit, configured to plan a movement trajectory in the updated global map according to the current position of the inspection robot;

[0086] An inspection unit, configured to control the inspection robot to move according to the movement trajectory, so that the inspection robot reaches each preset inspection point for inspection.

[0087] On the other hand, an electronic device provided by an embodiment of the present application includes a processor and a memory;

[0088] The memory is used to store a program;

[0089] The processor executes the program to implement the method described in any one of the above.

[0090] Another aspect of the embodiments of the present application also provides a computer-readable storage medium. The storage medium stores a program, and when the program is executed by a processor, the above-mentioned method according to any one of the embodiments is implemented.

[0091] The present application at least includes the following beneficial effects:

[0092] The present application can collect environmental data through sensors on the inspection robot; predict the system state and covariance of the inspection robot based on the environmental data and then update the system state and covariance; perform local mapping based on the updated system state to update the pre-constructed global map; determine the current position of the inspection robot based on the environmental data and the updated global map; plan a movement trajectory in the updated global map according to the current position of the inspection robot; and control the inspection robot to move along the movement trajectory so that the inspection robot reaches each preset inspection point for inspection. The present application improves the positioning and navigation ability of the inspection robot in terms of perception, positioning, and navigation, enables the inspection robot to adapt to environments with complex terrains, and improves the efficiency and accuracy of inspection. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required for the description of the embodiments. Obviously, the following drawings are only some embodiments of the present application. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0094] Figure 1 It is a schematic flowchart of the environmental perception and positioning method of the inspection robot provided by the embodiments of the present application;

[0095] Figure 2 It is a schematic diagram of the overall design of the inspection robot provided by the embodiments of the present application;

[0096] Figure 3 It is an example flowchart of the environmental perception and positioning method of the inspection robot provided by the embodiments of the present application;

[0097] Figure 4 It is a structural block diagram of the environmental perception and positioning device of the inspection robot provided by the embodiments of the present application. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0098] In order to make the purpose, technical solutions, and advantages of the present application clearer, the following further details the present application with reference to the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0099] Before detailing the embodiments of the present application, the related technologies of the present application are introduced first:

[0100] Traditional robot perception and positioning solutions may have defects in the nuclear power plant environment. Using visible light for visual perception has relatively high requirements for light. The CMOS of traditional RGBD cameras will produce a large signal-to-noise ratio in dark environments, which will greatly increase the false matching rate of feature points. When using lidar, usually a 16 / 32-line mechanical LiDAR is used to construct a three-dimensional point cloud map. However, for environments with few features, it is often difficult to match and align, resulting in inaccurate positioning. At the same time, electromagnetic interference in the nuclear power plant will also have a certain impact on data collection, thus affecting the accuracy of positioning.

[0101] Some existing path planning and obstacle avoidance algorithms also have deficiencies in the dynamic environment of nuclear power plants. Some related global planning algorithms rely on pre-constructed static maps and cannot respond in real time to dynamic obstacles that may be formed due to the movement of staff. Monte Carlo simulations show that in scenarios where environmental changes occur in more than 20% of the area, the replanning rate of traditional algorithms is less than 65%. Although some dynamic obstacle avoidance algorithms based on deep reinforcement learning (DRL) can adapt to environmental changes, the calculation latency tested on NVIDIA Jetson AGX is generally above 500 ms, making it difficult to meet the real-time control requirements of 10 Hz under emergency conditions.

[0102] Based on the above analysis, the core contradictions restricting the performance of nuclear power plant inspection robots can be summarized as: the contradiction between the single perception dimension and the strong environmental interference, the contradiction between the attenuation of positioning accuracy and the reliability of long-term tasks, and the contradiction between the static path planning and the dynamic environment.

[0103] Therefore, some embodiments of this application propose an integrated solution of multi-modal fusion perception - tightly coupled positioning - real-time dynamic perception navigation. It adopts the perception and positioning solution of the FAST-LIVO2 scheme and integrates the trajectory planning scheme of MINCO. The system is designed from three levels: perception, positioning, and navigation. This application can autonomously realize real-time monitoring of various equipment and environments in the pump room, which not only improves the efficiency and accuracy of inspection but also significantly reduces the safety risks of personnel.

[0104] Referring to Figure 1 , this application embodiment provides an environmental perception and positioning method for an inspection robot, which specifically includes the following steps S100 to S150:

[0105] S100: Collect environmental data through sensors on the inspection robot;

[0106] S110: Predict the system state and covariance of the inspection robot based on the environmental data, and then update the system state and the covariance;

[0107] S120: Update the pre-constructed global map through local mapping according to the updated system state;

[0108] S130: Determine the current location of the inspection robot according to the environmental data and the updated global map;

[0109] S140: Plan a movement trajectory in the updated global map according to the current location of the inspection robot;

[0110] S150: Control the inspection robot to move according to the movement trajectory, so that the inspection robot reaches each preset inspection point for inspection.

[0111] Optionally, the sensor includes a lidar and an RGBD camera, the environmental data includes laser scan points and RGB depth images that reach a preset frequency, and predicting the system state and covariance of the inspection robot according to the environmental data includes the following steps:

[0112] Segment the laser scan points according to the sampling time of the RGBD camera to synchronize the frequencies of the laser scan points and the RGB depth images;

[0113] Predict the system state and covariance of the inspection robot according to a predefined state transition model and IMU measurement data in the environmental data;

[0114] The expression of the state transition model is:

[0115] ;

[0116] where, ⊕ represents the Lie group exponential map, is the state transition function, including IMU measurement data and process noise data ; represents the current system state, represents the predicted system state;

[0117] The expression for predicting the covariance is:

[0118] ;

[0119] where, is the covariance matrix; is the state transition Jacobian matrix; is the Jacobian matrix of the process noise; is the process noise covariance matrix, including the noise of IMU measurement data and the bias random walk noise.

[0120] Updating the system state and the covariance includes the following steps:

[0121] Updating the error state by iteratively calculating the residual and the Jacobian matrix until the error state converges, thereby causing the system state and the covariance to converge, and obtaining the updated system state and covariance;

[0122] The residual is the residual of the laser scan points in the environmental data to the plane, and the expression of the residual is:

[0123] ;

[0124] where, is the residual; is the pose of the IMU in the sensor in the global coordinate system; is the extrinsic parameter of the lidar in the sensor relative to the IMU; is the coordinate of the laser scan point in the coordinate system of the lidar; nj, is the normal vector and the center point of the voxel plane;

[0125] The expression for updating the covariance is:

[0126] ;

[0127] where, is the covariance; is the Kalman gain coefficient; is the measurement Jacobian matrix;

[0128] The expression for the Kalman gain coefficient is:

[0129] ;

[0130] where, is the measurement noise covariance matrix.

[0131] Optionally, locally mapping according to the updated system state to update a pre-constructed global map includes the following steps:

[0132] Determining the laser scan points scanned by the lidar in the sensor according to the updated system state;

[0133] Registering the scan points into the global framework, and judging whether a plane is formed by using singular value decomposition according to the distribution of the scan points in the voxel;

[0134] Processing newly created and existing voxels respectively, updating the plane parameters and uncertainties, and obtaining mature planes;

[0135] Select a target mature plane from each of the mature planes in the local map;

[0136] Generate visual map points based on the target mature plane;

[0137] Update the pre-constructed global map according to the visual map points.

[0138] Optionally, before the step of selecting a target mature plane from each of the mature planes in the local map, the method further includes the following steps:

[0139] Use the laser scan points and the visible points of the previous frame to determine the visual sub-map of the current frame for visible voxel query;

[0140] Supplement the visual map points when the laser scan points are missing to achieve on-demand ray casting;

[0141] Remove the laser scan points with occlusion, depth discontinuity, and excessive viewing angles.

[0142] Optionally, planning a movement trajectory in the updated global map according to the current positioning of the inspection robot includes the following steps:

[0143] Use the Astar algorithm to generate a path between the current positioning of the inspection robot and each of the inspection points as the initial trajectory;

[0144] Construct a safety corridor composed of a series of polyhedrons or spheres;

[0145] The polyhedron is composed of multiple constraint planes:

[0146] ;

[0147] Wherein, represents a certain point on the surface of the polyhedron and is the corresponding normal vector; the movement trajectory is constrained in the intersection area of the polyhedrons; Generate a MINCO class trajectory according to the initial trajectory and make the MINCO class trajectory within the safety corridor;

[0148] The expression of the MINCO class trajectory is:

[0149] The expression of the MINCO class trajectory is:

[0150] ;

[0151] represents the set of the MINCO class trajectories; p(t) is a trajectory function that maps from the time interval to Space; Represents a functional mapping relationship; Represents a waypoint; The MINCO - type trajectory is divided into M sub - trajectories, and T represents the time - allocation vector of each sub - trajectory;

[0152] The representation of each sub - trajectory is:

[0153] ;

[0154] Wherein, Is a polynomial basis - function vector; Is the time of the i - th segment of the trajectory, Is the polynomial - coefficient matrix of the i - th segment of the trajectory, because c is also And Function of:

[0155] ;

[0156] Optimize the MINCO - type trajectory within the safety corridor to obtain the moving trajectory.

[0157] Optionally, the optimizing the MINCO - type trajectory within the safety corridor to obtain the moving trajectory includes the following steps:

[0158] Optimize the MINCO - type trajectory within the safety corridor by using an optimization equation, a trajectory - continuity equation, a robot - dynamics equation, a time - limit equation, an obstacle - avoidance constraint equation, and an obstacle - avoidance cost function to obtain the moving trajectory;

[0159] The optimization equation is:

[0160] ;

[0161] The trajectory - continuity equation is:

[0162] ;

[0163] ;

[0164] ;

[0165] Wherein, Represents the s - th sub - trajectory, Represents The first - order derivative of the position;

[0166] The robot - dynamics equation is:

[0167] ;

[0168] ;

[0169] ;

[0170] in, Represent the maximum velocity and maximum acceleration respectively, and are characterized by a series of soft constraints: ;

[0171] The time limit equation is:

[0172] ;

[0173] in, Indicates the time limit parameter;

[0174] The obstacle avoidance constraint equation is:

[0175] ;

[0176] The obstacle avoidance cost function is:

[0177] ;

[0178] in, is the penalty term weight, It is the obstacle location information.

[0179] Next, the present application will be described with specific implementation methods.

[0180] 1. Purpose: The inspection robot can autonomously perform tasks according to the path and program predetermined by the user. From the beginning of the inspection to the inspection at a fixed point, the entire work does not require any operator to manually operate the robot except for the setting of the preset points, which greatly improves the degree of automation.

[0181] 2. Requirements analysis.

[0182] Working environment requirements: The robot and autonomous system of the nuclear power plant pump room need to be operated, stored and debugged in a relatively harsh environment. They should operate normally under the following environmental conditions:

[0183] 1. Temperature: 0~40℃.

[0184] 2.Relative humidity ≤ 95%. The impact of salt spray environment needs to be considered for field applications.

[0185] 3. Air pressure: standard atmospheric pressure.

[0186] Specifically, this embodiment may include the following solutions:

[0187] 1. Positioning module.

[0188] The general design drawing of the inspection robot in this embodiment is as shown Figure 2 , and the computing unit selects NVIDIA Jetson AGX Orin 32G. The perception devices use the Mid360 lidar and the RGBD camera of Intel RealSense D435, and the SIYI A8 mini pan-tilt camera is used to detect information such as instrument panels. Through the effective fusion of multiple sensors, high-precision environment perception and positioning of the wheel-legged inspection robot are realized.

[0189] Figure 3 This is the example flowchart of this embodiment. In this embodiment, through an efficient multi-sensor fusion SLAM framework, fast and robust positioning and mapping are achieved by tightly fusing LiDAR, IMU, and visual data. Combining the LiDAR raw point cloud obtained by Mid360 scanning, its built-in IMU inertial data, and the raw images obtained by the RealSense D435 camera, based on the error-state iterative Kalman filter (ESIKF), a sequential update strategy is adopted to solve the problem of dimensional mismatch between LiDAR and visual data. The positioning module can be specifically divided into four major modules: the error-state iterative Kalman filter (ESIKF), local mapping, LiDAR measurement, and visual measurement. Each module has a clear division of labor and works together to jointly achieve high-precision positioning, mapping, and stable operation in complex environments, providing strong support for the robot's autonomous navigation and scene understanding in the position environment. The specific perception and positioning process is as follows:

[0190] 1. Data acquisition and synchronization: Use LiDAR, Intel RealSense D435 RGBD camera, and IMU to collect data. Among them, LiDAR collects point cloud data at 10 - 100Hz, the camera collects image data at 10 - 50Hz, and IMU collects inertial data at 100 - 250Hz. Through scan reorganization, the LiDAR raw points sampled at high frequency and in sequence are segmented into scans at the camera sampling moments to ensure the synchronization of LiDAR and camera data at the same frequency, providing a unified time reference for subsequent processing.

[0191] 2. State prediction (ESIKF propagation): In the ESIKF framework, according to the IMU measurement data and the defined state transition model, forward propagation of the state and covariance is performed. Among them, the forward propagation of the state and covariance is achieved through the state transition model and the covariance propagation formula:

[0192] 2.1 State transition equation:

[0193] ;

[0194] where ⊕: Lie group exponential map (for handling non-linear updates of poses). is the state transition function, which includes IMU measurements and process noise wk.

[0195] 2.2 Covariance propagation formula:

[0196] ;

[0197] Covariance matrix;

[0198] State transition Jacobian matrix (the derivative of the linearized state transition function with respect to the state);

[0199] Jacobian matrix of the process noise (the derivative of the linearized state transition function with respect to the noise);

[0200] Process noise covariance matrix, which includes the measurement noise and bias random walk noise of the IMU.

[0201] Predict the system state from the time of the last LiDAR scan and image frame reception to the current time, providing a prior distribution for subsequent measurement updates. At the same time, perform backward propagation to compensate for the motion distortion of LiDAR points, ensuring that the LiDAR points are "measured" at the end time of the scan and improving data accuracy.

[0202] 3. LiDAR measurement update (ESIKF - LiDAR update): Project the undistorted points in the LiDAR scan onto the global frame and establish a measurement equation based on the plane of the voxel where the points are located. Considering the uncertainties of LiDAR points, plane normals, and centers, construct an equation that includes measurement noise. Update the error state by iteratively calculating the residuals and Jacobian matrix until convergence. The converged state and covariance are used to update the map geometry and provide a basis for subsequent visual updates.

[0203] The system state always has the IMU as the core and includes 19-dimensional parameters (pose, position, velocity, IMU bias, gravity vector, exposure time, e.g.: , because the system state is 19-dimensional, so the covariance matrix P is also a symmetric matrix of, then the converged state and covariance are used to update the map geometry through the Kalman gain and the measurement residuals for update, as follows:

[0204] 3.1 The residual from the LiDAR point to the plane is:

[0205]

[0206] Wherein:

[0207] : The pose of the IMU in the global coordinate system (determined by the system state).

[0208] : The extrinsic parameters of the LiDAR relative to the IMU (calibrated).

[0209] : The coordinates of the LiDAR points in the LiDAR coordinate system.

[0210] nj, : The normal vector and the center point of the voxel plane (from the local map).

[0211] 3.2 Measurement noise covariance matrix .

[0212] 3.3 Kalman gain calculation:

[0213] ;

[0214] is the measurement Jacobian matrix (the derivative of the residual with respect to the system state).

[0215] 3.4 Covariance update formula:

[0216] .

[0217] 4. Local map construction and update: After the LiDAR is updated, the scanned points are registered into the global framework. According to the distribution of the points within the voxels, the singular value decomposition is used to determine whether a plane is formed. The newly created and existing voxels are processed separately to update the plane parameters and uncertainties. The mature planes are used to generate visual map points. The LiDAR points that are visible and have a significant gray-scale gradient in the current image are selected as candidates. After being projected onto the current image, the visual map points are determined according to the depth and gray-scale gradient. The image patch pyramid and the estimated state information are attached to the visual map points, and the patches are updated regularly. The scores are calculated according to the photometric similarity and the viewing angle of the patches, and the patch with the highest score is selected as the reference patch. The affine transformation is performed using the plane normal calculated from the LiDAR points, and the plane normal is further optimized by minimizing the photometric error between the reference patch and other patches.

[0218] 5. Visual Measurement Update (ESIKF - Visual Update): Appropriate visual map points are selected from the map through three steps: visible voxel query, ray casting on demand, and outlier rejection. The visible voxel query uses LiDAR points and visible points from the previous frame to determine the visual submap of the current frame; ray casting on demand supplements visual map points when LiDAR data is missing; outlier rejection removes points with occlusion, depth discontinuity, and excessive viewing angles. A visual measurement model is constructed based on the selected visual map points, and the inverse composition formula is used to improve the calculation efficiency. The current image is aligned by minimizing the photometric error between the reference patch and the current patch, and iterative updates are performed at different levels of the image pyramid from coarse to fine until convergence. The converged state is used to generate visual map points and update the reference patch.

[0219] 6. System Loop and Continuous Optimization: The above process is continuously looped. With the input of new sensor data, the system state and map are continuously updated. In the loop, each module cooperates with each other to continuously optimize the positioning and mapping results, enabling the system to adapt to a dynamically changing environment and maintain high-precision performance.

[0220] II. Navigation Module.

[0221] To better meet the requirements of this project, in this embodiment, after building the global map, the operator manually controls the robot to set detection points at places of interest, which is called the "pilot" model in this embodiment. After completing all the dotting plans, the robot will return to the initial position or the charging station position, and the robot will store all information including the global map and preset trajectory points to assist in the formal inspection. When the formal inspection starts, the robot starts from the initial position and will find a reasonable and safe trajectory to pass through each detection point in turn according to the algorithm designed in this embodiment, and will also avoid possible obstacles on the road.

[0222] The navigation scheme of this embodiment adopts a hierarchical trajectory optimization scheme, which can be specifically divided into path search at the front end and trajectory optimization at the back end.

[0223] The path search at the front end uses the classic A-star algorithm, which is a heuristic search algorithm for finding the shortest path from the initial node to the target node in a graph. By appropriately selecting and adjusting the heuristic function, the A-star algorithm can effectively find the shortest path in a complex environment while maintaining computational feasibility and efficiency. However, the disadvantage is that it does not consider the actual model of the robot, and the generated trajectory is not smooth enough, which is difficult to apply in the real nuclear power plant environment.

[0224] In response to the above problems, this embodiment performs trajectory optimization at the backend. First, after using Astar to find a path, a safety corridor (SFC) composed of a series of polyhedra or spheres is constructed. What this embodiment ensures is that the trajectory in the trajectory optimization stage must remain within the safety corridor, which can not only improve the flexibility of optimization, reduce the computational amount, but also ensure safety constraints.

[0225] Polyhedron Composed of multiple constraint planes:

[0226] ;

[0227] Where Represents a certain point on the surface of the polyhedron, Is the corresponding normal vector. During trajectory optimization, the trajectory must be constrained within the intersection region of these polyhedra.

[0228] Subsequently, a type of trajectory called MINCO (Minimum Control Effort Trajectory Optimization) is adopted:

[0229] ;

[0230] Represents the set of MINCO - type trajectories, the p(t) trajectory function, mapping from the time interval To Space, Represents a function mapping relationship, Represents the waypoint, and T represents the time allocation vector for each segment of the trajectory.

[0231] This trajectory is divided into M segments, and each segment is:

[0232] ;

[0233] Where , is the polynomial basis function vector, Is the time of the i - th segment of the trajectory, Is the polynomial coefficient matrix of the i - th segment of the trajectory. Since c is also And Function of:

[0234] .

[0235] Therefore, the MINCO - type trajectory is a type that only depends on the waypoints (waypoints) and time If it is a trajectory with parameters, then in this embodiment, the Astar path searched at the front end can be used as the initial waypoint, and after setting the initial time, further optimization can be carried out in combination with the actual situation. By optimizing the coefficient c and the time allocation T, the trajectory can simultaneously meet the time-optimal, energy-optimal, and ensure safety.

[0236] This embodiment expects to reach the target point quickly and smoothly. Then this embodiment optimizes the MINCO type trajectory. When the higher-order derivative of a trajectory is smaller, the change will be smoother. Therefore, the optimization equation of this embodiment is designed as follows:

[0237] ;

[0238] At the same time, this embodiment should also consider the continuity of the trajectory, that is:

[0239] ;

[0240] ;

[0241] ;

[0242] represents the s-th segment of the trajectory, represents the first-order derivative of the position.

[0243] Consider the dynamic feasibility of the robot:

[0244] ;

[0245] ;

[0246] ;

[0247] respectively represent the maximum speed and acceleration limits, and are characterized by a series of soft constraints as .

[0248] Consider a minimum time limit:

[0249] ;

[0250] is also a time limit parameter.

[0251] This embodiment should also set conditions so that the trajectory does not exist in the obstacles, so that even the optimized trajectory will not collide with the environment:

[0252] ;

[0253] In this embodiment, the hard constraint conditions of the robot can be adjusted to soft constraint conditions by setting an optimization function. The following soft constraints are set in this embodiment, such that getting close to an obstacle will result in a relatively large cost:

[0254] ;

[0255] is a penalty term weight, obstacle position information.

[0256] Finally, the optimized trajectory of this embodiment is transmitted to the control module.

[0257] Then, the trajectory designed as above can plan a smooth and fast trajectory that meets the dynamic and safety requirements during the traveling process. Coupled with the positioning module of this embodiment, it enables the wheeled-foot robot to achieve autonomous navigation in real time.

[0258] III. Detection module.

[0259] When the robot cruises to a preset point, it will compare the differences between its own position and attitude and the set standard values, so as to adjust its own state. Moreover, the SIYI A8mini pan-tilt mounted on the robot's head will also be adjusted by detecting the differences between the currently captured photo and the stored photo, so that the robot can adopt the best angle to identify the scale of the instrument panel and ensure the accuracy of the identification.

[0260] IV. The overall perception, positioning, and navigation process steps of this embodiment are as follows:

[0261] 4.1 Global map construction:

[0262] First, place the robot at the initial position of the inspection. Subsequently, start the perception and positioning modules, and use the remote control or keyboard to remotely control the robot to scan the global environment. For scenes with few texture details, repeated scanning should be performed. After the entire scene is scanned and the robot returns to the initial position, the map information saved during the scanning process is the global map we constructed.

[0263] 4.2 Inspection point setting:

[0264] After the global map is constructed, control the robot to perform a dotting operation at the points of interest or the positions that need to be detected. When the robot performs subsequent autonomous inspection operations, it will successively reach the set inspection points, and will stop at this position and perform a series of image matching operations, so that the robot's pan-tilt can accurately face the position of the instrument panel, facilitating the recognition algorithm to recognize the scale information.

[0265] 4.3 Relocalization:

[0266] Before the robot officially starts the inspection tour, it is necessary to move the robot to its original initial position for repositioning of the robot. This step is to ensure that the robot can be in the correct position on the global map and improve the accuracy of positioning.

[0267] 4.4 Autonomous Navigation:

[0268] Subsequently, the autonomous navigation module of the robot is activated. The robot then updates in real time based on the original map information, searches for a more reasonable trajectory to reach the predicted inspection points in sequence, starts the detection, moves forward to the next target point after successful detection, and avoids obstacles on the way. Finally, it returns to the starting point or charging point of the inspection tour to complete the inspection.

[0269] Refer to Figure 4 , the embodiments of the present application provide an environmental perception and positioning device for an inspection robot, including:

[0270] A data acquisition unit for acquiring environmental data through sensors on the inspection robot;

[0271] A prediction and update unit for predicting the system state and covariance of the inspection robot based on the environmental data and then updating the system state and the covariance;

[0272] A map update unit for performing local mapping based on the updated system state to update a pre-constructed global map;

[0273] A positioning unit for determining the current position of the inspection robot based on the environmental data and the updated global map;

[0274] A trajectory planning unit for planning a movement trajectory in the updated global map based on the current position of the inspection robot;

[0275] An inspection unit for controlling the inspection robot to move along the movement trajectory so that the inspection robot reaches each preset inspection point for inspection.

[0276] It can be understood that the content in the above method embodiments is applicable to the device embodiments of the present application. The functions specifically implemented by the device embodiments of the present application are the same as those of the above method embodiments, and the beneficial effects achieved are also the same as those of the above method embodiments.

[0277] In some alternative embodiments, the functions / operations recited in the block diagrams may not occur in the order presented in the operational illustrations. For example, depending on the functions / operations involved, two blocks shown in succession may actually be executed substantially concurrently or the blocks may sometimes be executed in reverse order. Additionally, the embodiments presented and described in the flowcharts of the present application are provided by way of example for the purpose of providing a more comprehensive understanding of the technology. The disclosed methods are not limited to the operations and logical flows presented herein. Alternative embodiments are contemplated in which the order of various operations is altered and in which sub-operations described as part of a larger operation are performed independently.

[0278] Furthermore, although the present application has been described in the context of functional modules, it should be understood that, unless otherwise stated to the contrary, one or more of the described functions and / or features may be integrated in a single physical device and / or software module, or one or more functions and / or features may be implemented in separate physical devices or software modules. It should also be understood that a detailed discussion of the actual implementation of each module is not necessary for an understanding of the present application. Rather, given the attributes, functions, and internal relationships of the various functional modules in the devices disclosed herein, the actual implementation of the modules will be understood within the ordinary skill of an engineer. Accordingly, those of ordinary skill in the art will be able to implement the present application as set forth in the claims without undue experimentation. It should also be understood that the specific concepts disclosed are merely illustrative and are not intended to limit the scope of the present application, which is determined by the full scope of the appended claims and their equivalents.

[0279] If the described functions are implemented in the form of software functional units and sold or used as independent products, they may be stored in a computer-readable storage medium. Based on such understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, or a part of this technical solution, may be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present application. The aforementioned storage medium includes various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs.

[0280] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a definitional sequence of executable instructions for implementing logical functions, which can be embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, a system including a processor, or other systems that can fetch and execute instructions from the instruction execution system, apparatus, or device. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device.

[0281] More specific examples (a non-exhaustive list) of the computer-readable medium include the following: an electrical connection part (electronic device) having one or more wirings, a portable computer diskette (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium can even be paper or other suitable media on which the program can be printed, as the program can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpretation, or otherwise processing as appropriate, and then stored in a computer memory.

[0282] It should be understood that various parts of the present application can be implemented by hardware, software, firmware, or a combination thereof. In the above-described embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or a combination of the following techniques well-known in the art can be used: discrete logic circuits having logic gate circuits for implementing logical functions on data signals, application specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field programmable gate arrays (FPGAs), etc.

[0283] In the description of this specification, the description with reference to terms such as "one embodiment", "some embodiments", "example", "specific example", or "some examples", etc. means that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0284] Although embodiments of the present application have been shown and described, those of ordinary skill in the art can understand that various changes, modifications, substitutions, and variations can be made to these embodiments without departing from the principles and spirit of the present application. The scope of the present application is defined by the claims and their equivalents.

[0285] The above has specifically described the preferred embodiments of the present application, but the present application is not limited to the described embodiments. Those skilled in the art can also make various equivalent deformations or substitutions without violating the spirit of the present application, and these equivalent deformations or substitutions are all included in the scope defined by the claims of the present application.

Claims

1. The environmental perception and positioning method of the inspection robot is characterized by: The method comprises the following steps: Collect environmental data through sensors on the inspection robot; Predicting the system state and covariance of the inspection robot according to the environmental data and then updating the system state and the covariance; Performing local mapping according to the updated system state to update a pre-built global map; Determining the current location of the inspection robot according to the environmental data and the updated global map; Planning a moving trajectory in the updated global map according to the current location of the inspection robot; The inspection robot is controlled to move along the moving trajectory so that the inspection robot reaches each preset inspection point for inspection.

2. The environmental perception and positioning method of the inspection robot according to claim 1 is characterized in that: The sensor includes a laser radar and an RGBD camera, the environmental data includes laser scanning points and RGB depth images reaching a preset frequency, and the system state and covariance of the inspection robot are predicted according to the environmental data, including the following steps: Splitting the laser scanning points according to the sampling time of the RGBD camera to synchronize the frequencies of the laser scanning points and the RGB depth image; Predicting the system state and the covariance of the inspection robot according to a predefined state transition model and IMU measurement data in the environmental data; The expression of the state transition model is: ； Among them, ⊕ represents the Lie group index mapping, is the state transfer function, including IMU measurement data and process noise data ; Indicates the current state of the system, representing a predicted state of said system; The expression for predicting the covariance is: ； in, is the covariance matrix; is the state transfer Jacobian matrix; is the Jacobian matrix of process noise; is the process noise covariance matrix, including the noise of IMU measurement data and the bias random walk noise.

3. The environmental perception and positioning method of the inspection robot according to claim 1 is characterized in that: The updating of the system state and the covariance comprises the following steps: The error state is updated by iteratively calculating the residual and the Jacobian matrix until the error state converges, thereby making the system state and the covariance converge, and obtaining the updated system state and the covariance; The residual is the residual from the laser scanning point in the environmental data to the plane, and the expression of the residual is: ； in, is the residual; is the position and posture of the IMU in the sensor in the global coordinate system; is the external parameter of the laser radar in the sensor relative to the IMU; is the coordinate of the laser scanning point in the coordinate system of the laser radar; , is the normal vector and center point of the voxel plane; The expression for updating the covariance is: ； in, is the covariance; is the Kalman gain coefficient; is the measurement Jacobian matrix; The expression of the Kalman gain coefficient is: ； in, is the measurement noise covariance matrix.

4. The environmental perception and positioning method of the inspection robot according to claim 1 is characterized in that: The performing of local mapping according to the updated system state to update the pre-built global map comprises the following steps: Determine a laser scanning point obtained by a laser radar scan in the sensor according to the updated system state; Registering the scan points to a global framework, and determining whether a plane is formed by using singular value decomposition according to the distribution of the scan points in the voxels; The newly created and existing voxels are processed separately, the plane parameters and uncertainties are updated, and the mature plane is obtained; Selecting a target maturation plane from each of the maturation planes in the local map; Mature plane into visual map points according to the target; The pre-built global map is updated according to the visual map points.

5. The environmental perception and positioning method of the inspection robot according to claim 4 is characterized in that: Before selecting a target mature plane from each of the mature planes in the local map, the method further comprises the following steps: Determine a visual sub-image of a current frame using the laser scanning points and visible points of a previous frame to perform visible voxel query; Supplementing the visual map points when the laser scan points are missing to achieve on-demand ray casting; The laser scanning points with occlusion, depth discontinuity and excessive viewing angle are removed.

6. The environmental perception and positioning method of the inspection robot according to claim 1, characterized in that: The step of planning a moving trajectory in the updated global map according to the current location of the inspection robot comprises the following steps: Astar algorithm is used to generate a path from the current location of the inspection robot to each inspection point as an initial trajectory; Construct a safe corridor consisting of a series of polyhedrons or spheres; The polyhedron is composed of multiple constrained planes: ； in, Represents the polyhedron A point on the surface of is the corresponding normal vector; the moving trajectory is constrained in the intersection area of ​​the polyhedron; Generate a MINCO-type trajectory according to the initial trajectory, and make the MINCO-type trajectory within the safety corridor; The expression of the MINCO class trajectory is: ； represents the set of MINCO-type trajectories; p(t) is the trajectory function, from the time interval Map to space; Represents a function mapping relationship; represents a waypoint; the MINCO type trajectory is divided into M segments of sub-trajectories, and T represents the time allocation vector of each segment of the sub-trajectory; Each sub-trajectory is represented as: ； in, is the polynomial basis function vector; is the time of the ith trajectory, is the polynomial coefficient matrix of the i-th trajectory, because c is and Function: ； The MINCO-type trajectory in the safety corridor is optimized to obtain the moving trajectory.

7. The environmental perception and positioning method of the inspection robot according to claim 6, characterized in that: The step of optimizing the MINCO-type trajectory in the safety corridor to obtain the moving trajectory comprises the following steps: The MINCO-type trajectory in the safety corridor is optimized by using an optimization equation, a trajectory continuity equation, a robot dynamics equation, a time limit equation, an obstacle avoidance constraint equation, and an obstacle avoidance cost function to obtain the moving trajectory; The optimization equation is: ； The trajectory continuity equation is: ； ； ； in, represents the sub-trajectory described in the sth segment, express The first derivative of position; The robot dynamics equation is: ； ； ； in, Represent the maximum velocity and maximum acceleration respectively, and are characterized by a series of soft constraints: ; The time limit equation is: ； in, Indicates the time limit parameter; The obstacle avoidance constraint equation is: ； The obstacle avoidance cost function is: ； in, is the penalty term weight, It is the obstacle location information.

8. The environmental perception and positioning device of the inspection robot is characterized by: The device comprises: A data acquisition unit, used to collect environmental data through sensors on the inspection robot; A prediction and update unit, used for predicting the system state and covariance of the inspection robot according to the environmental data and then updating the system state and the covariance; A map updating unit, configured to perform local mapping according to the updated system state to update a pre-built global map; A positioning unit, used to determine the current position of the inspection robot according to the environmental data and the updated global map; A trajectory planning unit, used to plan a moving trajectory in the updated global map according to the current location of the inspection robot; The inspection unit is used to control the inspection robot to move along the moving trajectory so that the inspection robot reaches each preset inspection point for inspection.

9. An electronic device, characterized in that: The electronic device comprises a processor and a memory; The memory is used to store programs; The processor executes the program to implement the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The storage medium stores a program, and the program is executed by a processor to implement the method according to any one of claims 1 to 7.

Images