Multi-sensor fusion navigation method for switch room operation robot

Abstract

The invention relates to the field of mobile robot navigation, in particular to a multi-sensor fusion navigation method for a switch room operation robot, which comprises the following specific steps: S1, data fusion; s2, a visual SLAM algorithm based on dynamic target optimization; s3, a path planning algorithm; and S4, navigation stack. According to the multi-sensor solution based on Kalman filtering, accurate positioning of the robot can be rapidly and accurately obtained, the switch room robot can be rapidly guided to reach a specified fault cabinet body to complete a series of operations such as emergency opening and unlocking based on a dynamic target optimized visual SLAM algorithm and a Dijkstra algorithm, and the fault is cut off in time.

Description

Technical Field

[0001] The invention relates to the technical field of mobile robot navigation, and in particular to a multi-sensor fusion navigation method for a switch room operating robot. Background Art

[0002] The switch room operation robot must be able to effectively solve the problem of not being able to arrive at the substation site in time in an emergency and risking emergency tripping on site. Only by issuing instructions through the remote control center in the main control room, the switch room operation robot can run to the faulty cabinet to complete a series of operations such as emergency trip unlocking, and cut off the fault in time. In the event of an emergency fault, the switch room operation robot must also be able to autonomously run to the designated switch cabinet to complete emergency opening and closing operations, thereby further improving the emergency fault handling capabilities. The switch room operation robot can not only save the time of operation and maintenance personnel, but also ensure the safety of operation and maintenance personnel. It can replace or assist manual operation tasks, greatly reduce the workload of staff, shorten fault handling time, ensure the personal safety of operators and power grid safety, and reduce the occurrence of power accidents.

[0003] The function of the switch room robot determines that it must have the ability of autonomous navigation with high precision and accuracy. However, the traditional navigation method based on a single sensor is easily affected by data noise and errors, so it is not suitable for switch room operation robots. Multi-sensor fusion navigation technology can take advantage of the advantages of multiple sensors to improve the accuracy and precision of data. For example, in the field of vehicle autonomous driving, the fusion of multiple sensors (such as visual cameras, cameras, ultrasonic sensors, etc.) can improve the vehicle's positioning and obstacle detection capabilities. Therefore, it is urgent to study a multi-sensor fusion navigation technology for switch room operation robots. Summary of the invention

[0004] In order to solve the above problems, the present invention proposes a multi-sensor fusion navigation method for a switch room operating robot.

[0005] A multi-sensor fusion navigation method for a switch room operating robot, the specific steps of which are as follows:

[0006] S1, data fusion: fuse the data of 3D vision camera, wheel encoder and inertial navigation unit to obtain the position and posture information of the robot in the environment;

[0007] S2. Visual SLAM algorithm based on dynamic target optimization: Using the fused robot position and posture information, combined with the scanning data of the visual camera, the environment map is constructed, and the algorithm is optimized and improved to improve the map construction and positioning accuracy to adapt to different application scenarios and the actual situation of the robot system;

[0008] S3, path planning algorithm: use the constructed environment map to realize the robot's path planning;

[0009] S4, Navigation stack: Combine the path planning results with the robot’s control system to achieve autonomous navigation of the robot.

[0010] The step S1 adopts an extended Kalman filter algorithm and uses data from different sensors to perform state estimation and error covariance matrix update.

[0011] The step S1 uses the wheel encoder observation value and the inertial navigation unit observation value as motion state prediction data, and the 3D vision camera observation value as point cloud motion compensation, and fuses the three observation values ​​based on the Kalman filter. The specific method is as follows:

[0012] When the wheel encoder and the inertial navigation unit are used at the same time, the fusion scheme becomes the fusion of the converted 3D vision camera observation value N1 (μ0, σ0) and the wheel encoder observation value N2 (μ1, σ1) and the inertial navigation unit observation value data N3 (μ2, σ2);

[0013]

[0014] From this we can get:

[0015]

[0016] According to the above formula, it can be deduced that:

[0017]

[0018] According to formula (3), the μ and δ values ​​can be obtained, which correspond to the x and y values ​​of the fused positioning coordinates respectively.

[0019] The specific steps for optimizing and improving the algorithm in step S2 are as follows: appropriately improve the feature factor graph model. The feature factor graph as a whole can be divided into two parts: a static feature factor graph and a dynamic feature factor graph. The static feature factor graph part is to establish the reprojection error from the map point to the camera pixel plane. The dynamic factor graph adds the assumptions of alignment and motion smoothness of the transformed pose for camera pose estimation, and further optimizes the camera pose.

[0020] The specific steps of camera pose optimization are as follows: for the kth camera pose X k and the jth map point P j , assuming that the pixel coordinates of the map point in the pixel plane are Then the error function can be established:

[0021]

[0022] The expected value of the error function is a two-dimensional zero vector. The position of the camera at different times is calculated step by step in an iterative manner. Let X k and X k+1 The transformation pose between them is T k , and Tk is relative to Xk, the pose is X k , X k+1 and T k There should be an algebraic relationship:

[0023] X k T k =X k+1 (5)

[0024] During the optimization process, the camera pose will be continuously adjusted, thus destroying this algebraic relationship. Therefore, a constraint is set to maintain the consistency between the three variables, so the error function is set:

[0025]

[0026] The expected value of the error function is a unit matrix. For the entire operation process of the algorithm, a smooth motion assumption is proposed, that is, it is assumed that the camera moves at a uniform speed in a short time. Under this assumption, two adjacent transformation poses are the same, that is:

[0027] T k =T k+1 (7)

[0028] Combining formula (5), it can be concluded that the expression of smooth motion error is:

[0029]

[0030] The expected value of the error function is also a unit matrix. The above three error functions constitute a factor graph model based on static features. With the three factors in equations (4), (6) and (8), the variable optimization of the visual SLAM system in dynamic scenes is completed to perform camera positioning and map construction.

[0031] The step S3 uses the Dijkstra algorithm based on graph search to search the map and obtain the shortest path from the starting point to the end point.

[0032] The step S4 adopts the open source navigation stack ROS Navigation Stack, uses the robot position and target point information to perform path tracking and control, and realizes autonomous navigation of the robot.

[0033] The beneficial effects of the present invention are as follows: the multi-sensor solution based on Kalman filtering can quickly and accurately obtain the accurate positioning of the robot, and the visual SLAM algorithm and Dijkstra algorithm based on dynamic target optimization can quickly guide the switch room robot to the designated fault cabinet to complete a series of emergency tripping and unlocking operations, thereby cutting off the fault in time. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0035] Figure 1 It is a schematic diagram of the process structure of the present invention;

[0036] Figure 2 It is a structural diagram of the improved characteristic factors of the present invention;

[0037] Figure 3 It is a schematic diagram of the multi-sensor fusion navigation technology of the present invention. DETAILED DESCRIPTION

[0038] In order to make the technical means, creative features, objectives and effects achieved by the present invention easy to understand, the present invention is further described below.

[0039] like Figures 1 to 3 As shown, a multi-sensor fusion navigation method for a switch room operating robot is described, and the specific steps are as follows:

[0040] S1, data fusion: fuse the data of 3D vision camera, wheel encoder and inertial navigation unit to obtain the position and posture information of the robot in the environment;

[0041] S2. Visual SLAM algorithm based on dynamic target optimization: Use the fused robot position and posture information, combined with the scanning data of the visual camera, to build the environment map, use the visual camera scanning data to build the map and estimate the robot position, and optimize and improve the algorithm to improve the accuracy of map construction and positioning to adapt to different application scenarios and the actual situation of the robot system;

[0042] S3, path planning algorithm: use the constructed environment map to realize the robot's path planning;

[0043] S4, Navigation stack: Combine the path planning results with the robot’s control system to achieve autonomous navigation of the robot.

[0044] The step S1 adopts the extended Kalman filter, i.e., the EKF algorithm, and uses the data from different sensors to perform state estimation and error covariance matrix update. The multi-sensor solution based on Kalman filtering can quickly and accurately obtain the accurate positioning of the robot. The visual SLAM algorithm based on dynamic target optimization and the Dijkstra algorithm can quickly guide the switch room robot to the designated fault cabinet to complete a series of emergency tripping and unlocking operations, thereby cutting off the fault in time.

[0045] The step S1 uses the wheel encoder observation value and the inertial navigation unit observation value as the motion state prediction data, and the 3D vision camera observation value as the point cloud motion compensation. The homogeneous multi-sensor fusion scheme based on the Kalman filter fuses the three observation values ​​to obtain a more accurate and robust robot posture prediction value; the specific method is as follows:

[0046] When the number of sensors is two, the fusion scheme becomes the fusion of the converted 3D vision camera observation value N1 (μ0, σ0) and the wheel encoder observation value N2 (μ1, σ1) and the inertial navigation unit observation value data N3 (μ2, σ2);

[0047]

[0048] From this we can get:

[0049]

[0050] According to the above formula, it can be deduced that:

[0051]

[0052] According to formula (3), the μ and δ values ​​can be obtained, which correspond to the x and y values ​​of the fused positioning coordinates respectively.

[0053] The position, direction and environmental information are obtained through multi-sensor technology, and multiple sensors are fused based on the information fusion algorithm of the isomorphic Kalman filter. The algorithm has excellent anti-interference and robustness, and establishes an environmental model to realize the autonomous planning path of the switch room robot. It can overcome the problem that a single navigation solution cannot meet the user's requirements for accuracy, flexibility and low cost. It combines 3D vision cameras, wheel encoders and inertial navigation units in multiple ways to overcome the shortcomings of visual navigation AGVs that are prone to congestion and accidents on transportation trunk roads, or the fixed paths and small number of stations in magnetic navigation systems.

[0054] The specific steps for optimizing and improving the algorithm in step S2 are as follows: appropriately improve the feature factor graph model. The feature factor graph as a whole can be divided into two parts: a static feature factor graph and a dynamic feature factor graph. The static feature factor graph part is to establish the reprojection error from the map point to the camera pixel plane. The dynamic factor graph adds the assumptions of alignment and motion smoothness of the transformed pose for camera pose estimation, and further optimizes the camera pose.

[0055] The specific steps of camera pose optimization are as follows: for the kth camera pose X k and the jth map point P j , assuming that the pixel coordinates of the map point in the pixel plane are Then the error function can be established:

[0056]

[0057] This error function corresponds to Figure 2 The expected value of the line between the orange box and the blue box is a two-dimensional zero vector. The camera's position at different times is calculated step by step in an iterative manner. Let X k and X k+1 The transformation pose between them is T k , and T k Relative to X k , the posture is X k , X k+1 and T k There should be an algebraic relationship:

[0058] X k T k =X k+1 (5)

[0059] Because the camera pose will be constantly adjusted during the optimization process, thus destroying this algebraic relationship, a constraint should be set to maintain the consistency between the three variables to prevent the system from deviating too much from the optimization of the odometer solution result. Therefore, the error function is set:

[0060]

[0061] This error function corresponds to Figure 3 The expected value of the line between adjacent orange boxes in is a unit matrix. For the entire operation process of the algorithm, a smooth motion assumption is proposed, that is, it is assumed that the camera moves at a uniform speed in a short time. Under this assumption, the two adjacent transformation poses are the same, that is:

[0062] T k =T k+1 (7)

[0063] Combining formula (5), it can be concluded that the expression of smooth motion error is:

[0064]

[0065] This error function corresponds to Figure 2 The expected value of the line between the three adjacent orange boxes is also a unit matrix; the above three error functions constitute a factor graph model based on static features. With the above three factors in formulas (4), (6), and (8), the variable optimization of the visual SLAM system in dynamic scenes can be completed, and camera positioning and map construction can be performed; the algorithm can make the camera positioning accuracy of the switch room robot in a dynamic environment more accurate, the target tracking accuracy is also more accurate, and it has a shorter solution time.

[0066] The step S3 uses the Dijkstra algorithm based on graph search to search the map, obtain the shortest path from the starting point to the end point, and optimize and improve the algorithm to improve the efficiency and robustness of path planning.

[0067] The step S4 adopts the open source navigation stack ROS Navigation Stack, uses the robot position and target point information to perform path tracking and control, realizes the robot's autonomous navigation, and optimizes and improves the navigation stack to adapt to different application scenarios and the actual situation of the robot system.

[0068] Through a variety of sensor fusion positioning technology, visual SLAM algorithm based on dynamic target optimization, Dijkstra path planning algorithm and ROS Navigation Stack navigation stack class and other technologies, the robot's positioning, environmental map construction and autonomous path planning are realized. At the same time, optimization and improvement are carried out according to different application scenarios and the actual situation of the robot system to improve the performance and robustness of the system.

[0069] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited to the above embodiments. The above embodiments and descriptions only describe the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A multi-sensor fusion navigation method for a switch room operating robot, characterized in that: The specific steps are as follows: S1, data fusion: fuse the data of 3D vision camera, wheel encoder and inertial navigation unit to obtain the position and posture information of the robot in the environment; S2. Visual SLAM algorithm based on dynamic target optimization: Using the fused robot position and posture information, combined with the scanning data of the visual camera, the environment map is constructed, and the algorithm is optimized and improved to improve the map construction and positioning accuracy to adapt to different application scenarios and the actual situation of the robot system; S3, path planning algorithm: use the constructed environment map to realize the robot's path planning; S4, Navigation stack: Combine the path planning results with the robot’s control system to achieve autonomous navigation of the robot.

2. A multi-sensor fusion navigation method for a switch room operating robot according to claim 1, characterized in that: The step S1 adopts an extended Kalman filter algorithm and uses data from different sensors to perform state estimation and error covariance matrix update.

3. The multi-sensor fusion navigation method for a switch room operating robot according to claim 2, characterized in that: The step S1 uses the wheel encoder observation value and the inertial navigation unit observation value as motion state prediction data, and the 3D vision camera observation value as point cloud motion compensation, and fuses the three observation values ​​based on the Kalman filter. The specific method is as follows: When the wheel encoder and the inertial navigation unit are used at the same time, the fusion scheme becomes the fusion of the converted 3D vision camera observation value N1 (μ0, σ0) and the wheel encoder observation value N2 (μ1, σ1) and the inertial navigation unit observation value data N3 (μ2, σ2); From this we can get: According to the above formula, it can be deduced that:

4. The multi-sensor fusion navigation method for a switch room operating robot according to claim 3, characterized in that: According to formula (3), the μ and δ values ​​can be obtained, which correspond to the x and y values ​​of the fused positioning coordinates respectively.

5. The multi-sensor fusion navigation method for a switch room operating robot according to claim 1, characterized in that: The specific steps for optimizing and improving the algorithm in step S2 are as follows: appropriately improve the feature factor graph model. The feature factor graph as a whole can be divided into two parts: a static feature factor graph and a dynamic feature factor graph. The static feature factor graph part is to establish the reprojection error from the map point to the camera pixel plane. The dynamic factor graph adds the assumptions of alignment and motion smoothness of the transformed pose for camera pose estimation, and further optimizes the camera pose.

6. A multi-sensor fusion navigation method for a switch room operating robot according to claim 5, characterized in that: The specific steps of camera pose optimization are as follows: for the kth camera pose X k and the jth map point P j , assuming that the pixel coordinates of the map point in the pixel plane are Then the error function can be established: The expected value of the error function is a two-dimensional zero vector. The position of the camera at different times is calculated step by step in an iterative manner. Let X k and X k+1 The transformed pose between is Tk, and T k Relative to X k , the posture is X k , X k+1 and T k There should be an algebraic relationship: X k T k =X k+1 (5) During the optimization process, the camera pose will be continuously adjusted, thus destroying this algebraic relationship. Therefore, a constraint is set to maintain the consistency between the three variables, so the error function is set: The expected value of the error function is a unit matrix. For the entire operation process of the algorithm, a smooth motion assumption is proposed, that is, it is assumed that the camera moves at a uniform speed in a short time. Under this assumption, two adjacent transformation poses are the same, that is: T k =T k+1 (7) Combining formula (5), it can be concluded that the expression of smooth motion error is: The expected value of the error function is also a unit matrix. The above three error functions constitute a factor graph model based on static features. With the three factors in equations (4), (6) and (8), the variable optimization of the visual SLAM system in dynamic scenes is completed to perform camera positioning and map construction.

7. The multi-sensor fusion navigation method for a switch room operating robot according to claim 1, characterized in that: The step S3 uses the Dijkstra algorithm based on graph search to search the map and obtain the shortest path from the starting point to the end point.

8. The multi-sensor fusion navigation method for a switch room operating robot according to claim 1, characterized in that: The step S4 adopts the open source navigation stack ROS Navigation Stack, uses the robot position and target point information to perform path tracking and control, and realizes autonomous navigation of the robot.

Images