A positioning and navigation method and system for an inspection robot

By using 3D raster map filtering and scoring in patrol robot navigation, selecting feasible child nodes and predicting dynamic obstacle motion trajectories, the problems of excessive patrol routes and complex obstacle avoidance caused by special shapes and dynamic obstacles are solved, and the problem of excessive patrol routes and complex obstacle avoidance caused by different shapes and dynamic obstacles is improved. Inspection efficiency and obstacle avoidance effect.

CN115291609BActive Publication Date: 2024-11-22SEVNCE ROBOTICS CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202211024795.0
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-08-25
Publication Date
2024-11-22
Estimated Expiration
2042-08-25

AI Technical Summary

Technical Problem

If there are many irregular obstacles in the prior art, the generated inspection route is longer, resulting in too long inspection time and low inspection efficiency of robots. Especially when there are dynamic obstacles, the planning process is complicated and obstacle avoidance is avoided. Poor results and may even fall into circulating obstacle avoidance.

Method used

By obtaining the current child nodes and threshold range constraints of the robot, the feasible child nodes are filtered based on the 3D raster map, and the feasible child node with the highest score is selected as the next child node; when dynamics are detected, they are detected, they are selected. When an obstacle is used, predict its motion trajectory, generate predicted 3D raster maps at different moments, filter out child nodes that do not collide with dynamic obstacles, and control the robot to accelerate or slow down to avoid.

Benefits of technology

The feasibility and efficiency of the inspection route are improved, the generated inspection route is shorter, and the obstacle avoidance effect is improved, which avoids the risk of the robot falling into circular obstacle avoidance and reduces the calculation amount of obstacle avoidance trajectory.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN115291609B_ABST
    Figure CN115291609B_ABST
Patent Text Reader

Abstract

The present invention provides a positioning and navigation method for an inspection robot, comprising a navigation generation step: obtaining the current child node and threshold range constraints of the robot, the child node data including position information, obtaining a search space for robot speed changes based on the threshold range constraints, obtaining a pre-selected child node based on the current child node information and the search space; screening out feasible child nodes from the pre-selected child nodes based on a 3D grid map, scoring the feasible child nodes according to the navigation end position, selecting the highest-scoring feasible child node as the next child node of the robot; when the position information of the next child node coincides with the navigation end position, generating navigation data of the robot according to all child nodes. The present invention solves the problem in the prior art that if there are many special-shaped obstacles, the generated inspection route is long, resulting in a long inspection time and low inspection efficiency of the robot.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and navigation, and in particular relates to a positioning and navigation method and system for an inspection robot. Background Art

[0002] In some high-risk industries, in order to ensure the safety of facilities and equipment and the operating environment, and to prevent workers from being in high-risk environments for a long time, companies use advanced robot technology to replace workers for safety inspections. Before the inspection, it is necessary to generate a robot inspection route to help the robot avoid obstacles during the inspection process and reach the inspection point smoothly. In the prior art, in order to avoid obstacles during the inspection, a square object is usually generated according to the height, length and width of the obstacle, and the inspection route is set along the edge of the square object to avoid obstacles. Because this inspection route does not walk along the edge of the obstacle, if there are many special-shaped obstacles on the inspection route, the generated inspection route is longer, resulting in too long an inspection time, which makes the robot's inspection efficiency low.

[0003] If there are dynamic obstacles in the inspection route, it is necessary to generate multiple square objects according to the height, length and width of the dynamic obstacles at different times, which makes the inspection route planning process complicated. The inspection route planning software has high operating efficiency and slow planning speed, which also leads to poor obstacle avoidance effect of the robot and even causes the robot to fall into a loop of obstacle avoidance process. Summary of the invention

[0004] The present invention aims to at least solve the technical problems existing in the prior art and provide a positioning and navigation method and system for a patrol robot to solve the problem in the prior art that when there are many special-shaped obstacles, the generated patrol route is long, resulting in excessively long patrol time and low patrol efficiency of the robot.

[0005] In order to achieve the above-mentioned purpose of the present invention, according to the first aspect of the present invention, the present invention provides a positioning and navigation method for a patrol robot, including a navigation generation step: obtaining the current child node and threshold range constraint of the robot, the child node data including position information, obtaining the search space for the robot speed change based on the threshold range constraint, and obtaining a pre-selected child node based on the current child node information and the search space; screening out feasible child nodes from the pre-selected child nodes based on a 3D grid map, scoring the feasible child nodes according to the navigation endpoint position, and selecting the feasible child node with the highest score as the next child node of the robot; when the position information of the next child node coincides with the navigation endpoint position, generating the navigation data of the robot according to all child nodes.

[0006] Furthermore, the step of obtaining the pre-selected sub-nodes includes: the sub-node data also includes posture data, speed data and time data, the posture data includes location information, and the step of obtaining the pre-selected sub-nodes includes: selecting multiple sampling points in the three-axis directions of the search space respectively, arranging and combining the selected sampling points to generate a speed vector, and obtaining the pre-selected sub-nodes according to the speed vector and the sub-node data.

[0007] Furthermore, when a dynamic obstacle is detected in the 3D grid map, the dynamic obstacle is tracked, the motion trajectory of the dynamic obstacle changing with time is predicted, and the predicted 3D grid maps at different times are generated; the step of selecting feasible sub-nodes from pre-selected sub-nodes based on the 3D grid map in the navigation generation step is specifically: judging whether the pre-selected sub-node collides with the dynamic obstacle according to the predicted 3D grid map at the same time as the pre-selected sub-node, and selecting the pre-selected sub-node that does not collide with the dynamic obstacle as the feasible sub-node; and also includes a navigation change step: predicting the motion trajectory of the dynamic obstacle at a fixed frequency on time, if it is predicted that the navigation data intersects with the motion trajectory of the dynamic obstacle, controlling the robot to follow the navigation data to accelerate through the collision point or decelerate to avoid the dynamic obstacle.

[0008] Furthermore, the step of predicting the motion trajectory of the dynamic obstacle that changes with time includes: detecting the object frame and feature vector of the dynamic obstacle at a fixed frequency; tracking multiple dynamic obstacles on the point cloud according to the object frame and the feature vector, and predicting the motion trajectory of the dynamic obstacle that changes with time according to the posture, size and speed of the multiple dynamic obstacles.

[0009] Furthermore, the navigation generation step also includes a global planning step before the navigation generation step: marking the robot's current child node, auxiliary node and inspection end point on the point cloud map to form a road network map, and generating the shortest path from the current child node to the inspection end point based on the road network map; the navigation end point includes the inspection end point and the auxiliary node; the navigation generation step is used to generate the first navigation data from the current child node to the first auxiliary node of the shortest path; the navigation generation step is also used to generate auxiliary navigation data between two adjacent auxiliary nodes in the shortest path; the navigation generation step is also used to generate the second navigation data from the last auxiliary node of the shortest path to the inspection end point; and the inspection navigation data is generated according to the first navigation data, the auxiliary navigation data and the second navigation data.

[0010] Furthermore, the global planning step also includes setting the order of each inspection endpoint based on the point cloud map and the intelligent algorithm when there are multiple inspection endpoints, and the navigation generation step generates corresponding inspection navigation data according to the order of the inspection endpoints.

[0011] Furthermore, the navigation generation step also includes: when the time data of the next child node exceeds the set time threshold, it is determined that the navigation data generation has failed, the global planning step is restarted, and the next child node is marked on the road network map as not connected to the current child node.

[0012] Furthermore, trajectory smoothing is performed on the generated navigation data.

[0013] Furthermore, if the robot is a quadruped robot, it also includes a foothold planning step: obtaining the speed data, gait cycle and current center of mass position of the next child node of the robot to obtain the next center of mass position of the robot; obtaining four foothold ranges of the robot according to the next center of mass position, collecting multiple footholds in the four foothold ranges respectively, and obtaining a set of four-legged foothold combinations through permutations and combinations; obtaining a score of the four-legged foothold combination according to the spatial relationship between the four-legged foothold combination and the current foothold of the robot, and the projection distance between the intersection point of the diagonal line of the four-legged foothold combination and the next center of mass position on the horizontal plane, and selecting the four-legged foothold combination with the highest score as the next foothold of the robot's four legs.

[0014] In order to achieve the above-mentioned objectives of the present invention, according to the second aspect of the present invention, the present invention provides a positioning and navigation system for a patrol robot, a depth camera, a lidar sensor, an imu sensor, an RTK receiver, a satellite antenna and a processor; the processor positions and navigates the robot according to data from the depth camera, the lidar, the imu sensor and the RTK receiver, and the processor uses any of the positioning and navigation methods for the patrol robot during the positioning and navigation of the robot.

[0015] The technical principle of the present invention is as follows: a search space for the speed change of the robot is obtained according to a threshold constraint range, a speed value of the robot after the change is obtained according to the search space and the speed data, a pre-selected child node of the robot is obtained through the speed value after the change and the current child node data, and whether the pre-selected child node is feasible is judged according to a 3D grid map. If feasible, the next child node is marked as a feasible child node; the feasible child nodes are scored, and the feasible child node with the highest score is screened out as the next child node of the robot.

[0016] The beneficial effects of the present invention are as follows: the present scheme obtains pre-selected sub-nodes through a search space; the search space includes velocity vectors in different directions, and the number of pre-selected sub-nodes obtained through the search space is greater than the number of points obtained on three axes by the prior art, and the distribution range is wider; compared with the prior art, the present scheme increases the feasible schemes of the inspection route by screening out multiple feasible sub-nodes with different postures, and screens out the point with the highest score from multiple feasible sub-nodes as the next sub-node, selects the best inspection scheme from aspects such as time, speed and posture, generates the shortest inspection route, and improves the inspection efficiency of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 It is a logical schematic diagram of a positioning and navigation method of a patrol robot of the present invention;

[0018] Figure 2 It is a schematic diagram of the steps of generating navigation data of the present invention;

[0019] Figure 3 It is a schematic diagram of the global planning step and the navigation generation step of the present invention;

[0020] Figure 4 is a schematic diagram of obstacle avoidance logic when the present invention detects a dynamic obstacle;

[0021] Figure 5 It is a schematic diagram of the foothold planning structure of the present invention;

[0022] Figure 6 This is a planning effect diagram of the foothold of the present invention;

[0023] Figure 7 It is a schematic diagram of the installation of a positioning system of a patrol robot of the present invention;

[0024] Figure 8 It is a logical schematic diagram of a positioning system of a patrol robot of the present invention;

[0025] Fig. 9 It is a schematic diagram of the steps of generating a local dense map of the present invention;

[0026] Fig.10 This is a schematic diagram of a 3D grid map.

[0027] Figure numerals: depth camera 1, lidar sensor 2, satellite antenna 3, imu sensor 4, RTK receiver 5, current landing point 6, current center of mass position 61, landing range 7, next center of mass position 71. DETAILED DESCRIPTION

[0028] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0029] In the description of the present invention, it is necessary to understand that the terms "longitudinal", "lateral", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship, are based on the orientation or position relationship shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0030] In the description of the present invention, unless otherwise specified and limited, it should be noted that the terms "installed", "connected" and "connected" should be understood in a broad sense. For example, it can be a mechanical connection or an electrical connection, or it can be the internal connection between two components. It can be a direct connection or an indirect connection through an intermediate medium. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to the specific circumstances.

[0031] As attached Figure 1 As shown, the present invention provides a positioning and navigation method for an inspection robot, comprising a navigation generation step:

[0032] Obtain the current child node and threshold range constraint of the robot, where the child node data includes posture data, speed data and time data, wherein the posture data includes position information and posture information; obtain the search space of the robot speed change based on the threshold range constraint, and obtain the pre-selected child node based on the current child node information and the search space;

[0033] Based on the 3D grid map, feasible subnodes are selected from the pre-selected subnodes to form a feasible subnode set, each feasible subnode is scored according to the navigation end point position, and the feasible subnode with the highest score in the feasible subnode set is selected as the next subnode of the robot;

[0034] When the position information of the next child node coincides with the navigation end point, the navigation data of the robot is generated according to all child nodes.

[0035] As attached Figure 2 As shown, judging whether the pre-selected sub-node is feasible includes judging whether there is an obstacle at the pre-selected sub-node, or detecting whether the robot is in a special state at the pre-selected sub-node, such as vacating or flipping, etc. If so, judging that the pre-selected sub-node is not feasible.

[0036] The sub-node data is obtained through the current posture and point cloud map of the robot, among which the velocity data includes the plane velocity direction, plane velocity magnitude and vertical velocity magnitude, and the posture data includes the plane position, height position and robot posture; the threshold range constraint includes the speed change range constraint and the speed threshold constraint, among which the speed change range constraint is the value range of the robot acceleration, and the speed threshold constraint is the value range of the robot speed; the search space must comply with the threshold range constraint.

[0037] The score of feasible child nodes is based on the equation: score' = k t ×score t +k v ×score v +k h ×score h +k d ×score d Get, where score' is the score value of the child node, k t is the time coefficient, k v is the speed coefficient, k h is the height coefficient, k d is the distance coefficient, specifically, k t +k v +k h +k d =1;

[0038] score t is the score value of the time data, score t By the linear function score t =k1t+b1, where k1 is the first constant, b1 is the second constant, and t is the time data; the larger the time data, the smaller the score value of the time data;

[0039] score v is the score of the plane velocity. v Obtained by piecewise function, assuming that the current plane speed of the robot is v0; assuming that the plane speed of the feasible child node of the robot is v1, the power is W, and the time data is t; assuming that the robot's limit forward speed is v2, and the limit backward speed is v3; assuming that the output power of the robot at the limit forward is W1, and the output power of the robot at the limit backward is W2; based on the law of conservation of energy: Where m is the mass of the robot; W is in the range of (0, W1). When W = 0, the solved v1 value is defined as v4, and when W = W1, the solved v1 value is defined as v5. If the plane velocity of the feasible child node exceeds the range of max(v4, v2) to min(v5, v3), the score vis 0; if the plane velocity of the feasible child node is within the range of max(v4,v2)~v0, then score v It increases with the increase of the plane velocity of the feasible child node; if the plane velocity of the feasible child node is within the range of v0~min(v5,v3), then score v The score value of decreases as the plane velocity size of the feasible child node increases.

[0040] score h is the score of the height position, score h Obtained by piecewise function, let the current height position of the robot be h0, the vertical speed be v'0, let the height position of the feasible child node of the robot be h1, the vertical speed be v'1, the power be W, and the time data be t; the maximum normal support height of the robot is h2, considering the jumping situation of the robot, the robot's limit minimum height is h3, the limit maximum height is h4, and the maximum support power is W0; based on the law of conservation of energy and the law of kinematics: Where m is the mass of the robot, g is the gravitational acceleration; W is in the range of (0, W0), when W = 0, the solved h1 value is defined as h5, when W = W0, the solved g1 value is defined as h6; if the height position of the feasible child node exceeds the range of max(h3, g4) to min(h5, h6), then score h The score is 0; if the height position of the feasible child node is within the range of max(h3,h4)~h0, then the score h The score value increases with the height position of the feasible child node; if the height position of the feasible child node is within the range of h0~min(h5,h6), the score h The score value of decreases as the height position of the feasible child node increases;

[0041] score d is the score of the plane position, score d By the linear function score d =k2d+b2, where k2 is the third constant, b2 is the fourth constant, and d is the distance between the plane position and the inspection end point; the closer the plane position is to the inspection end point, the greater the score value of the plane position.

[0042] Preferably, the step of obtaining the pre-selected sub-node includes: selecting a plurality of sampling points in the three-axis directions of the search space, arranging and combining the selected sampling points to generate a velocity vector, and obtaining the pre-selected sub-node according to the velocity vector, position data and velocity data.

[0043] Specifically, assuming that the plane velocity of the robot's current child node is V'(v'x ,v' y ,v' z ), according to the threshold range constraint, the maximum speed change in the X direction is Bundle Divide into n parts for sampling and get v' x (v' x1 ,v' x2 ……v' xn ), then the velocity value of the i-th portion in the X direction is: Similarly, in the y direction, Divide into m parts for sampling and get v' y (v' y1 ,v' y2 ……v' ym ); in the z direction, Divide into j parts for sampling and get v' z (v' z1 ,v' z2 ……v' zi ); v' x 、v' y 、v' z By permutation and combination, we get n×m×j V′ i (v′ xi ,v′ yi ,v′ zi ).

[0044] As attached Figure 4 As shown, when a dynamic obstacle is detected in the 3D grid map, the dynamic obstacle is tracked, the movement trajectory of the dynamic obstacle over time is predicted, and the predicted 3D grid maps at different times are generated;

[0045] The step of selecting feasible sub-nodes from pre-selected sub-nodes based on the 3D grid map in the navigation generation step is specifically as follows: judging whether the pre-selected sub-node collides with a dynamic obstacle based on the predicted 3D grid map at the same time as the pre-selected sub-node, and selecting pre-selected sub-nodes that do not collide with the dynamic obstacle as feasible sub-nodes; specifically, obtaining the time information of the pre-selected sub-node, selecting the predicted 3D grid map at the same time as the time information to judge whether the pre-selected sub-node collides with the dynamic obstacle.

[0046] It also includes a navigation change step: predicting the motion trajectory of the dynamic obstacle at a fixed frequency. If it is predicted that the navigation data and the motion trajectory of the dynamic obstacle intersect, the robot is controlled to follow the navigation data to accelerate through the collision point or decelerate to avoid the dynamic obstacle; the fixed frequency can be set according to the robot's travel speed, which can be once per second or twice per second. The center of mass trajectory of the robot tracking the navigation data to accelerate through the collision point or decelerate to avoid the dynamic obstacle is set as the obstacle avoidance trajectory;

[0047] The calculation process of the obstacle avoidance trajectory is as follows: based on the threshold range constraint, the search space of the robot's speed change is obtained; based on the speed data, posture data and search space of the tracking trajectory point and the short-term target point, the obstacle avoidance trajectory point is obtained; the short-term target point is a feasible sub-node on the navigation data that is a fixed distance away from the feasible obstacle avoidance point, where the fixed distance can be 10 meters, 20 meters or determined according to the robot's travel speed; the tracking trajectory point is a feasible sub-node on the navigation data that is closest to the feasible obstacle avoidance point;

[0048] Based on the 3D grid map at different times, feasible obstacle avoidance points are screened out from the obstacle avoidance trajectory points, and the feasible obstacle avoidance points are scored according to the navigation data. The feasible obstacle avoidance point with the highest score is selected as the next obstacle avoidance point of the robot; when the posture data of the next obstacle avoidance point coincides with the posture data and speed data of the feasible sub-node in the navigation data, the robot's obstacle avoidance trajectory is generated according to all the next obstacle avoidance points.

[0049] The score of feasible obstacle avoidance points is as follows: b =score m +score w ; where score b is the score of the feasible obstacle avoidance point, score m Score the distance between the plane position of the feasible obstacle avoidance point and the short-term target point. The closer the distance between the plane position of the feasible obstacle avoidance point and the short-term target point, the higher the distance score. m =k3d'+b3, where k3 is the fifth constant, b3 is the sixth constant, and d' is the distance from the feasible obstacle avoidance point to the short-term target point.

[0050] score w Score the similarity between the pose data of the feasible obstacle avoidance point and the pose data of the tracking trajectory point. The higher the similarity between the pose data of the feasible obstacle avoidance point and the pose data of the tracking trajectory point, the higher the similarity score; score w =k4w+b4, where k4 is the seventh constant, b4 is the eighth constant, and w is the similarity between the pose data of the feasible obstacle avoidance point and the pose data of the tracking trajectory point.

[0051] The next obstacle avoidance point obtained according to the scoring method of the feasible obstacle avoidance points enables the robot to quickly return to the original navigation data and avoid sharp turns. Compared with the existing technology, this obstacle avoidance method predicts the motion trajectory of dynamic obstacles to avoid the robot from falling into a loop of obstacle avoidance. In addition, the obstacle avoidance trajectory is shorter and has high obstacle avoidance efficiency, which reduces the calculation amount of the obstacle avoidance trajectory and makes the generation process of the obstacle avoidance trajectory faster.

[0052] The steps of predicting the motion trajectory of dynamic obstacles over time include: detecting the object frame and feature vector of the dynamic obstacle at a fixed frequency; tracking multiple dynamic obstacles on the point cloud according to the object frame and feature vector, and predicting the motion trajectory of the dynamic obstacle over time according to the position, size and speed of the multiple dynamic obstacles. The position, size and speed of the dynamic obstacle are calculated by the Kalman filter algorithm.

[0053] If a collision still occurs during the robot's movement, the navigation generation step is restarted.

[0054] As attached Figure 3 As shown, the navigation generation step also includes a global planning step before:

[0055] The current child node, auxiliary node and inspection end point of the robot are marked on the point cloud map to form a road network map. The auxiliary nodes include intersections, turning points, stairs, and other points that change the current posture of the robot. The auxiliary nodes can be automatically identified or manually marked. The road network map also includes the robot's recommended heading; based on the A* algorithm, the distance between the current child node and the adjacent auxiliary node is obtained, based on the A* algorithm, the distance between the inspection end point and the adjacent auxiliary node is obtained, and based on the Dijkstra algorithm, the shortest path from the current child node to the inspection end point is generated on the road network map.

[0056] The navigation endpoint includes an inspection endpoint and an auxiliary node; the navigation generation step is used to generate first navigation data from the current child node to the first auxiliary node of the shortest path; the navigation generation step is also used to generate auxiliary navigation data between two adjacent auxiliary nodes in the shortest path; the navigation generation step is also used to generate second navigation data from the last auxiliary node of the shortest path to the inspection endpoint; and the inspection navigation data is generated based on the first navigation data, the auxiliary navigation data and the second navigation data.

[0057] The global planning step also includes setting the order of each inspection endpoint based on the point cloud map and the intelligent algorithm when there are multiple inspection endpoints; the navigation generation step generates corresponding inspection navigation data according to the order of the inspection endpoints. Specifically, the intelligent algorithm is a genetic algorithm.

[0058] As attached Figure 2As shown, the navigation generation step also includes: when the time data of the next child node exceeds the set time threshold, it is judged that the navigation data generation fails, restart the global planning step, and mark the next child node as not connected to the current child node on the road network map. The feasible child node is placed in the open table, and the pre-selected child node judged as infeasible is placed in the close table. The child node placed in the close table can no longer be selected as a pre-selected child node.

[0059] Preferably, in order to make the robot's movement process smooth and without lag, a Kalman filter is used to perform trajectory smoothing processing on the generated navigation data.

[0060] As attached Figure 5 and attached Figure 6 As shown, if the robot is a quadruped robot, it also includes the foothold planning step:

[0061] The speed data, gait cycle and current center of mass position 61 of the next child node of the robot are obtained to obtain the next center of mass position 71 of the robot; specifically, the gait cycle of the robot is preferably 0.6 seconds, but can also be other values, which can be set according to the travel speed of the robot.

[0062] The four foothold ranges 7 of the robot are obtained based on structural mechanics and the next center of mass position, and multiple footholds are collected in the four foothold ranges 7 respectively, and a set of four-legged foothold combination combinations is obtained through permutations and combinations; the permutation and combination method includes combining and arranging the footholds in the four foothold ranges 7 to obtain a collection of four-legged foothold combination combinations.

[0063] The score of the four-legged foothold combination is obtained according to the spatial relationship between the four-legged foothold combination and the current foothold 6 of the robot, and the projection distance between the intersection of the diagonal connection line of the four-legged foothold combination and the next center of mass position on the horizontal plane, and the four-legged foothold combination with the highest score is selected as the next foothold of the robot's four legs. The scoring equation of the four-legged foothold combination is a linear equation. The smaller the projection distance, the higher the score; the more similar the spatial relationship between the four-legged foothold combination and the current foothold 6 of the robot is, the higher the score.

[0064] As attached Figure 7 As shown, the present invention also provides a positioning and navigation system for an inspection robot, comprising a depth camera 1, a lidar sensor 2, an imu sensor 4, an RTK receiver 5, a satellite antenna 3 and a processor; the processor positions and navigates the robot according to data from the depth camera 1, the lidar, the imu sensor 4 and the RTK receiver 5, and any of the above-mentioned positioning and navigation methods for the inspection robot is used in the process of positioning and navigating the robot.

[0065] Specifically, the processor receives data from the depth camera 1, the lidar sensor 2, the imu sensor 4 and the RTK receiver 5, and the processor stores a positioning and mapping algorithm.

[0066] As attached Figure 7 As shown, the depth camera 1 is installed at five positions of the robot, namely, front, back, left, right and bottom; the lidar sensor 2 is installed in front of the robot; the imu sensor 4 and the RTK receiver 5 are installed inside the robot; and the satellite antenna 3 is installed at the head and tail of the robot.

[0067] After the depth camera 1, lidar sensor 2, imu sensor 4, RTK receiver 5 and antenna are installed, the coordinate system of the rigid body constraint is calibrated to obtain the rigid body transformation matrix before the coordinate system.

[0068] As attached Figure 8 As shown, the processor synchronizes the continuous data obtained by the depth camera 1, the lidar sensor 2, the imu sensor 4 and the RTK receiver 5 in time and space, and then fuses the time and space synchronized data and performs front-end tracking to obtain odometer information; the bias of the imu sensor 4 is obtained through pre-integration optimization to provide a relatively accurate bias estimate for the front-end tracking; the front-end tracking algorithm includes one or more of the fast_livo algorithm and the R3live algorithm.

[0069] The odometer information is judged as a key frame, and features are extracted for the odometer information judged as a key frame to obtain the scene features of the key frame, the optimized pose, the newly added key frame odometer pose and point cloud, and save them as historical key frames; if it is judged not to be a key frame, the current pose is obtained by multiplying the change matrix of the odometer and the optimized pose with the odometer pose.

[0070] Through closed-loop and repositioning detection, historical key frames with similar scene features to the current key frame are searched in the historical key frames, and the pose transformation matrix between the current key frame and the historical key frame is calculated, and the pose transformation matrix is ​​given to the back-end as a factor for optimization.

[0071] The back-end optimization recalculates the key frames with unstable posture. The specific optimization process is: optimize the odometer factor, closed-loop and repositioning factor, imu pre-integration factor, RTK position and direction factor graph to obtain the posture of the key frame, point cloud map and the last key frame tracking and optimized posture transformation matrix.

[0072] The robot's sub-node data can be obtained based on the current posture and point cloud map.

[0073] As attached Fig. 9As shown, a local sparse map is filtered out in the point cloud map according to the current posture; the residual point cloud of dynamic obstacles in the local sparse map is cleared based on the point cloud data obtained by the lidar sensor 2 and the depth map obtained by the depth camera 1, and a local dense map is generated according to the depth data of the depth camera 1, the stiffness transformation matrix and the local sparse map.

[0074] Based on deep learning point cloud segmentation or grid map segmentation, a local 3D grid map is finally obtained, as shown in the attached figure. Fig.10 As shown, the 3D grid map includes obstacles such as the ground, walls, stairs, trees, grass, snow, water, and people and vehicles.

[0075] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0076] Although the embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the claims and their equivalents.

Claims

1. A positioning and navigation method for an inspection robot, characterized in that: Including navigation generation steps: The current child node and threshold range constraint of the robot are obtained, the child node data includes position information, the search space of the robot speed change is obtained based on the threshold range constraint, and the pre-selected child node is obtained based on the current child node information and the search space; the child node data also includes posture data, speed data and time data, the posture data includes position information, and the steps of obtaining the pre-selected child node include: selecting multiple sampling points in the three-axis directions of the search space, arranging and combining the selected sampling points, generating a speed vector, and obtaining the pre-selected child node according to the speed vector and the child node data; Based on the 3D grid map, feasible subnodes are screened out from the pre-selected subnodes, and the feasible subnodes are scored according to the navigation end point position, and the feasible subnode with the highest score is selected as the next subnode of the robot; when dynamic obstacles are detected in the 3D grid map, the dynamic obstacles are tracked, the motion trajectory of the dynamic obstacles changing with time is predicted, and the predicted 3D grid maps at different times are generated; The step of selecting feasible subnodes from pre-selected subnodes based on the 3D grid map in the navigation generation step is specifically as follows: judging whether the pre-selected subnode collides with a dynamic obstacle based on the predicted 3D grid map at the same time as the pre-selected subnode, and selecting the pre-selected subnode that does not collide with the dynamic obstacle as a feasible subnode; When the position information of the next child node coincides with the navigation end point, the navigation data of the robot is generated according to all child nodes; The threshold range constraint includes a speed variation range constraint and a speed threshold constraint, wherein the speed variation range constraint is a value range of the robot acceleration, and the speed threshold constraint is a value range of the robot speed.

2. A positioning and navigation method for an inspection robot as claimed in claim 1, characterized in that: The method also includes a navigation change step: predicting the motion trajectory of the dynamic obstacle at a fixed frequency. If it is predicted that the navigation data intersects the motion trajectory of the dynamic obstacle, the robot is controlled to follow the navigation data to accelerate through the collision point or decelerate to avoid the dynamic obstacle.

3. A positioning and navigation method for an inspection robot as claimed in claim 1 or 2, characterized in that: The step of predicting the motion trajectory of a dynamic obstacle that changes over time includes: detecting the object frame and feature vector of the dynamic obstacle at a fixed frequency; tracking multiple dynamic obstacles on the point cloud according to the object frame and the feature vector, and predicting the motion trajectory of the dynamic obstacle that changes over time according to the posture, size and speed of the multiple dynamic obstacles.

4. A positioning and navigation method for an inspection robot as claimed in claim 1 or 2, characterized in that: The navigation generation step also includes a global planning step before it: Mark the robot's current subnode, auxiliary node, and inspection endpoint on the point cloud map to form a road network map, and generate the shortest path from the current subnode to the inspection endpoint based on the road network map; The navigation end point includes the inspection end point and the auxiliary node; the navigation generation step is used to generate the first navigation data of the first auxiliary node of the shortest path from the current child node; The navigation generation step is also used to generate auxiliary navigation data between two adjacent auxiliary nodes in the shortest path; The navigation generation step is also used to generate second navigation data from the last auxiliary node of the shortest path to the inspection end point; and the inspection navigation data is generated according to the first navigation data, the auxiliary navigation data and the second navigation data.

5. A positioning and navigation method for an inspection robot as claimed in claim 4, characterized in that: The global planning step also includes setting the order of each inspection endpoint based on the point cloud map and intelligent algorithm when there are multiple inspection endpoints, and the navigation generation step generates corresponding inspection navigation data according to the order of the inspection endpoints.

6. A positioning and navigation method for an inspection robot as claimed in claim 1, 2 or 5, characterized in that: The navigation generation step also includes: when the time data of the next child node exceeds the set time threshold, it is determined that the navigation data generation has failed, the global planning step is restarted, and the next child node is marked on the road network map as not connected to the current child node.

7. A positioning and navigation method for an inspection robot as claimed in claim 1, 2 or 5, characterized in that: Perform trajectory smoothing on the generated navigation data.

8. A positioning and navigation method for an inspection robot as claimed in claim 1, 2 or 5, characterized in that: If the robot is a quadruped robot, it also includes the foothold planning step: Obtain the speed data, gait cycle and current center of mass position of the next child node of the robot to obtain the next center of mass position of the robot; According to the next center of mass position, four foothold ranges of the robot are obtained, multiple foothold points are collected in the four foothold ranges respectively, and a set of four-leg foothold point combinations is obtained through permutations and combinations; The score of the four-legged foothold combination is obtained according to the spatial relationship between the four-legged foothold combination and the current foothold of the robot, as well as the projection distance between the intersection of the diagonal lines of the four-legged foothold combination and the next center of mass position on the horizontal plane, and the four-legged foothold combination with the highest score is selected as the next foothold of the robot's four legs.

9. A positioning and navigation system for an inspection robot, characterized in that: A depth camera, a lidar sensor, an imu sensor, an RTK receiver, a satellite antenna and a processor; the processor positions and navigates the robot based on the data from the depth camera, the lidar, the imu sensor and the RTK receiver, and the positioning and navigation method for the inspection robot described in any one of claims 1 to 8 is used in the process of positioning and navigating the robot by the processor.