A method for calculating posture error of substation inspection robot

Abstract

The present invention discloses a method for calculating the posture error of a substation inspection robot, which relates to the field of power station operation and maintenance technology, including establishing a posture calculation model for the inspection robot; calculating the posture error of the inspection robot; repositioning the inspection robot, compensating for the posture error of the inspection robot, and navigating the inspection robot to a predetermined inspection point. The method can calculate the navigation error between the preset stop position and the actual stop position of the substation inspection robot each time, and calibrate the current position of the substation inspection robot according to the calculation result, so that the inspection robot reaches the calibrated stop point and shoots the inspection image at the calibrated pan-tilt angle, so as to avoid the inspection robot, when it stops at each inspection point along the predetermined inspection route, the navigation error will gradually accumulate, resulting in a linear increase in the pixel deviation between the pre-calibrated target detection image and the actual detection image.

Description

Technical Field

[0001] The present invention relates to the technical field of power station operation and maintenance, and in particular to a method for calculating the position and posture error of a substation inspection robot. Background Art

[0002] With the rapid development of robot technology, the application of intelligent substation inspection robots is becoming more and more widespread. Substations are also using inspection robots to replace manual inspections. Due to the particularity of the substation environment, when replacing manual inspections, robots need to use infrared thermal imagers to collect equipment temperature and use visible light cameras to identify the status of instruments, desiccants and other equipment. After completing the inspection task, the robot needs to return to the charging room, which places very high requirements on the inspection robot's repeated positioning accuracy.

[0003] At present, substation inspection robots generally use LIDAR or visual simulation localization and mapping (SLAM) navigation technology to navigate in outdoor scenes. The navigation software is embedded in the onboard computer of the inspection robot. However, due to the limitation of navigation accuracy, the substation inspection robot has an error ranging from centimeters to decimeters between the preset stop position and the actual stop position each time, that is, the navigation error, which will make it difficult for the inspection robot to reach the calibrated stop point and shoot the inspection image at the calibrated pan-tilt angle. The actual posture of the camera has an error with the preset posture, resulting in blurred and incomplete detection images taken by the camera. As the inspection robot stops at various inspection points along the predetermined inspection route, the navigation error of the inspection robot will gradually accumulate, and the pixel deviation between the pre-calibrated target detection image and the actual detection image will increase linearly, and even the actual detection image will exceed the camera's viewing angle. Summary of the invention

[0004] In order to remedy the defects of the prior art, the purpose of the present invention is to provide a method for calculating the posture error of a substation inspection robot, which can calculate the navigation error between the preset stop position and the actual stop position of the substation inspection robot each time, and calibrate the current position of the substation inspection robot according to the calculation result, so that the inspection robot reaches the calibrated stop point and takes inspection images at a calibrated pan-tilt angle, so as to avoid the inspection robot stopping at various inspection points along the predetermined inspection route. The navigation error will gradually accumulate, resulting in a linear increase in the pixel deviation between the pre-calibrated target detection image and the actual detection image.

[0005] The present invention provides the following technical solutions: establishing a posture calculation model for a patrol robot; calculating the posture error between a current patrol point where the patrol robot is located and a predetermined patrol point based on the posture calculation model for the patrol robot; repositioning the patrol robot, compensating for the posture error of the patrol robot, and navigating the patrol robot to the predetermined patrol point.

[0006] As a preferred solution of the method for calculating the posture error of the substation inspection robot described in the present invention, it comprises: obtaining a three-dimensional model map of the substation, and pre-marking the inspection route on the three-dimensional model map of the substation, and at the same time calibrating the inspection points on the inspection route, and calibrating the posture of the inspection robot's pan-tilt head and the posture of the camera at each of the inspection points.

[0007] As a preferred solution of the method for calculating the posture error of the substation inspection robot of the present invention, the posture calculation model of the inspection robot includes:

[0008] The position and posture of the inspection robot is represented by a 4×4 matrix, and its calculation expression is:

[0009]

[0010] Among them, p represents the posture of the inspection robot, SE(3) represents the special Euclidean group, R∈R3×3 represents the orthogonal rotation matrix, t∈R3×1 represents the translation vector matrix, that is, the translation vector. For convenience, we use a two-tuple (R, t) to equivalently represent the posture p of the inspection robot;

[0011] Tij = (Rij, tij) represents the transformation from Pi to Pj, that is, Pj = TijP i , that is, the transformation of the inspection robot's position p from point i to point j in a plane coordinate system.

[0012] As a preferred solution of the method for calculating the posture error of the substation inspection robot described in the present invention, in which: in the shooting state, the current chassis posture of the inspection robot and the displacement error vector t of the chassis h The expression is:

[0013]

[0014] in, Indicates the current chassis posture, that is, the current posture of the inspection robot, P h Indicates the chassis position at the last inspection point, t h It represents the displacement error vector of the chassis, that is, the error vector between the displacement of the inspection robot from the previous inspection point to the next scheduled inspection point and the next scheduled inspection point actually reached.

[0015] As a preferred solution of the method for calculating the posture error of the substation inspection robot of the present invention, in which: in the three-dimensional model map of the substation, the position of the inspection robot is used as the coordinate system, wherein the inspection robot (t hx , t hy , thz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the location, then the vector t of the inspection robot posture error h =(t hx , t hy , t hz ) T .

[0016] As a preferred solution of the method for calculating the posture error of the substation inspection robot of the present invention, the predetermined position of the camera is regarded as a three-dimensional point X in the three-dimensional model of the substation. c , X c The three-dimensional coordinates of c =(X c , Y c , Z c ), corresponding to the X in the camera 3D model c , its calculation expression is:

[0017]

[0018] Among them, X C represents the pose of the camera's intended position, represents the pose of the actual position of the camera, and the error vector of the actual position of the three-dimensional point is The inspection robot includes a calibration state and a shooting state when working. The camera posture Pc in the calibration state and the camera posture Pc in the shooting state The calculation expression is:

[0019] P C =T PC T hP P h

[0020] = <R x R y , t hp +t pc

[0021]

[0022] Then, the calculation expression of the error Tc of the camera pose Pc is:

[0023]

[0024] Then, the inspection robot chassis position translation vector t h The expression is:

[0025]

[0026] Among them, t hrepresents the displacement error vector of the chassis, P c Indicates the camera pose in the calibration state. Indicates the camera pose in the shooting state, P p represents the gimbal position, P h Indicates the chassis posture in the calibration state, T hp represents the posture transformation matrix of the inspection robot in the calibration state, and T hp =(1,t hp ) and T pc =(1,t pc ), where t hp and t pc Represents the position parameter of the inspection robot at the inspection point, R x R y Represents the orthogonal matrix under the standard state, corresponding to the camera pose P c The pitch angle θ x and the rotation angle θ y , Indicates the position of the camera before adjustment in shooting state, T c Represents the camera pose P c The error, is the orthogonal matrix under the shooting state, corresponding to the pose under the shooting state The pitch angle θ x and the rotation angle θ y , R cx R cy The orthogonal matrix representing the pitch angle and rotation angle error of the camera corresponding to the predetermined shooting picture, t hp and t pc Indicates the position parameters of the inspection robot at the inspection point, Indicates the chassis posture under shooting status, represents the posture transformation matrix of the inspection robot under shooting state, The vector representing the current camera pose, that is, the current camera pose P c The pitch angle θ x and the rotation angle θ y The vector of c =(t cx ,t cy ,t cz ) T is the camera displacement error vector, (t cx ,t cy ,t cz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the camera in the three-dimensional model of the substation.

[0027] As a preferred solution of the method for calculating the posture error of the substation inspection robot described in the present invention, the inspection robot is repositioned, the coordinates of the inspection robot in the three-dimensional model map of the substation are obtained, and the displacement error vector t of the chassis is calculated. h , the posture error of the inspection robot is compensated, and the inspection robot is navigated to a predetermined inspection point.

[0028] The beneficial effects of the present invention are as follows: it is capable of calculating the navigation error between the preset stop position and the actual stop position of the substation inspection robot each time, and calibrating the current position of the substation inspection robot according to the calculation result, so that the inspection robot reaches the calibrated stop point and takes inspection images at a calibrated pan-tilt angle, thereby avoiding the inspection robot from gradually accumulating navigation errors when stopping at various inspection points along a predetermined inspection route, resulting in a linear increase in the pixel deviation between the pre-calibrated target detection image and the actual detection image. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 A schematic diagram of a basic flow chart of a method for calculating a posture error of a substation inspection robot provided by an embodiment of the present invention;

[0030] Figure 2 An effect diagram of a simulation experiment conducted on a simulation platform of a method for calculating a posture error of a substation inspection robot provided by one embodiment of the present invention;

[0031] Figure 3 A real-world quantitative comparison result diagram of a method for calculating the posture error of a substation inspection robot provided by an embodiment of the present invention, wherein (a) is a translation error comparison diagram, (d) is an image coverage comparison diagram, (c) an ADOF comparison diagram, (d) Au and (e) a list of Au. DETAILED DESCRIPTION

[0032] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are described in detail below in conjunction with the accompanying drawings.

[0033] Example 1

[0034] Reference Figure 1 , is an embodiment of the present invention, and provides a method for calculating the posture error of a substation inspection robot, comprising:

[0035] S1: Establish the posture calculation model of the inspection robot;

[0036] Obtain the three-dimensional model map of the substation, and mark the inspection route on the three-dimensional model map of the substation in advance, calibrate the inspection points on the inspection route, and calibrate the position of the inspection robot's pan / tilt and the camera at each inspection point.

[0037] Inspection robot: including camera, router and chassis. The camera is equipped with an infrared camera and a high-definition camera with up to 32x zoom. The image resolution, principal point and focal length of the high-definition camera are 2688×1520, [1356.09, 774.79], 2596.99 respectively. The router is used to upload real-time detection images to the upper computer, receive and distribute the control commands of PT, and distribute the control commands of the camera and chassis. The chassis is controlled by an onboard computer with Ubuntu 18.04 and Robot Operating System (ROS1, kinetic distribution). The camera and router are powered by the chassis battery. The communication between the camera, chassis and router is carried out through an Ethernet network. It can be controlled by the upper computer: a laptop with a 1.9GHz Intel Core CPU and 16GB memory. The APR program is developed based on the software development kit (SDK) of the camera posture and chassis of the Microsoft Foundation Class (MFC) platform. It is responsible for automatically receiving real-time detection images and generating real-time control instructions for the camera posture and chassis through the router's WiFi network.

[0038] The posture of the inspection robot includes the chassis posture, pan-tilt posture and camera posture.

[0039] The chassis posture indicates the scheduled stop position of the inspection robot on the scheduled inspection route, that is, the stop position of the inspection point. Generally, the inspection point is an inspection stop area, which is slightly larger than the vertical projection area of ​​the inspection robot on the ground. It can refer to the size relationship between cars and parking spaces in life. As long as the inspection robot enters this area, it means that the inspection robot enters the scheduled stop position. The movement of the inspection robot depends on the movement of its chassis, so the chassis posture can be used to represent the posture of the inspection robot; the pan-tilt posture indicates the posture of the pan-tilt on the inspection robot. The pan-tilt is a supporting device for installing and fixing the camera. It is divided into fixed and electric pan-tilts. The fixed pan-tilt is suitable for situations where the monitoring range is not large. After the camera is installed on the fixed pan-tilt, the horizontal and pitch angles of the camera can be adjusted; the camera posture includes the horizontal, vertical and focal length postures of the camera. The horizontal posture indicates movement in the horizontal direction, that is, the rotation of the camera, and the vertical posture indicates movement in the vertical direction, that is, the height and pitch of the camera lens. The focal length posture indicates zoom, that is, adjusting the focal length of the camera. The camera preferably uses a high-resolution camera.

[0040] The inspection robot posture calculation model includes:

[0041] The position and posture of the inspection robot is represented by a 4×4 matrix, and its calculation expression is:

[0042]

[0043] Where p represents the posture of the inspection robot, SE(3) represents the special Euclidean group, R∈R3×3 represents the orthogonal rotation matrix, t∈R3×1 represents the translation vector matrix, that is, the translation vector. For convenience, we use a two-tuple (R, t) to equivalently represent the posture p of the inspection robot; and Tij=(Rij, tij) represents the transformation from Pi to Pj, that is, Pj=TijP i , that is, the transformation of the inspection robot's position p from point i to point j in a plane coordinate system.

[0044] S2: Based on the inspection robot posture calculation model, calculate the posture error between the inspection point where the inspection robot is currently located and the predetermined inspection point.

[0045] In the shooting state, the current chassis position of the inspection robot and the displacement error vector t of the chassis h The expression is:

[0046]

[0047] in, Indicates the current chassis posture, that is, the current posture of the inspection robot, P h Indicates the chassis position at the last inspection point, t h It represents the displacement error vector of the chassis, that is, the error vector between the displacement of the inspection robot from the previous inspection point to the next scheduled inspection point and the next scheduled inspection point actually reached.

[0048] In the 3D model map of the substation, the position of the inspection robot is used as the coordinate system, where the inspection robot (t hx , t hy , t hz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the location, then the vector t of the inspection robot posture error h =(t hx , t hy , t hz ) T Initially, all Z axes in the substation 3D model map are parallel to the road surface, and the Y axis is perpendicular to the road surface.

[0049] The predetermined position of the camera is regarded as a 3D point X in the 3D model of the substation. c , X c The three-dimensional coordinates of c =(X c , Y c , Z c ), corresponding to X in the camera 3D model c, its calculation expression is:

[0050]

[0051] Among them, X C represents the pose of the camera's intended position, represents the pose of the actual position of the camera, and the error vector of the actual position of the three-dimensional point is

[0052] In the calibrated state, the transformation between the chassis pose and the camera pose is expressed as:

[0053] P p =T hp P h =(R x R y , t hp )

[0054] Among them, P p represents the gimbal position, P h represents the chassis posture, T hp represents the posture transformation matrix of the inspection robot, and T hp =(1, t hp ) and T pc =(1, t pc ), where t hp and t pc Represents the position parameter of the inspection robot at the inspection point, R x R y are orthogonal matrices corresponding to the camera poses P c The pitch angle θx and rotation angle θy, the calculation expression of the orthogonal matrix is:

[0055]

[0056] It should be noted that θx and θy are known parameters at each preset inspection point, while the focal length of the camera is a neglected constant in this model because current cameras have the function of automatic zoom and focus.

[0057] In the shooting state, the chassis posture P h and represents the camera pose, P c The angle of deviation from calibration means that the current chassis posture The calculation expression is:

[0058]

[0059] in, Indicates the current chassis posture, that is, the current posture of the inspection robot, P h represents the chassis posture, Pp represents the gimbal posture, t h represents the displacement error vector of the chassis, then t h =(t hx , t hy , t hz ) T , that is, the error vector between the displacement of the inspection robot from the previous inspection point to the next scheduled inspection point and the actual arrival at the next scheduled inspection point, (t hx , t hy , t hz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the inspection robot in the three-dimensional model of the substation, with the inspection robot as the coordinate system. Indicates the current camera pose.

[0060] Current chassis position The displacement error vector of the chassis is t h and the current camera pose The camera pose Pc in the calibration state and the camera pose in the shooting state can be obtained The calculation expression is:

[0061] P C =T PC T hP P h

[0062] = <R x R y , t hp +t pc

[0063]

[0064] Then, the calculation expression of the error Tc of the camera pose Pc is:

[0065]

[0066]

[0067] Then, the inspection robot chassis position translation vector t h The expression is:

[0068]

[0069] Among them, t h represents the displacement error vector of the chassis, P c Indicates the camera pose in the calibration state. Indicates the camera pose in the shooting state, P p represents the gimbal position, Ph Indicates the chassis posture in the calibration state, T hp represents the posture transformation matrix of the inspection robot in the calibration state, and T hp =(1,t hp ) and T pc =(1,t pc ), where t hp and t pc Represents the position parameter of the inspection robot at the inspection point, R x R y Represents the orthogonal matrix under the standard state, corresponding to the camera pose P c The pitch angle θ x and the rotation angle θ y , Indicates the position of the camera before adjustment in shooting state, T c Represents the camera pose P c The error, is the orthogonal matrix under the shooting state, corresponding to the pose under the shooting state The pitch angle θ x and the rotation angle θ y , R cx R cy The orthogonal matrix representing the pitch angle and rotation angle error of the camera corresponding to the predetermined shooting picture, t hp and t pc Indicates the position parameters of the inspection robot at the inspection point, Indicates the chassis posture under shooting status, represents the posture transformation matrix of the inspection robot under shooting state, The vector representing the current camera pose, that is, the current camera pose P c The pitch angle θ x and the rotation angle θ y The vector of c =(t cx ,t cy ,t cz ) T is the camera displacement error vector, (t cx ,t cy ,t cz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the camera in the three-dimensional model of the substation.

[0070] Calibrate the inspection robot at the predetermined inspection point, and the pixel error between the camera's predetermined inspection image and the actual inspection image.

[0071] First, consider the camera position as a 3D point X in the camera's 3D model. c, X c The three-dimensional coordinates of c =(X c , Y c , Z c ), X c The vector of the three-dimensional coordinates is X c =(X c , Y c , Z c ) T In the three-dimensional model where the camera is located, that is, in the camera coordinate system Oc-xyz, the coordinate expression of the pixel point m of the principal point of the plane image captured by the corresponding camera is:

[0072] m=(u,v) T

[0073] Wherein, m represents the pixel point of the principal point of the plane image captured by the predetermined camera, (u, v) represents the coordinates of the pixel point m of the principal point of the plane image captured by the predetermined camera, and calibrating the plane image captured by the camera means calibrating the coordinates of the pixel point of the principal point of the plane image captured by the predetermined camera, and the calculation expression is:

[0074]

[0075] Among them, fu and fv represent the horizontal and vertical scale coefficients of the plane image taken by the camera respectively. When fu=fv, the plane image taken by the camera is a square, and (u0, v0) represents the pixel coordinates of the principal point of the plane image taken by the current camera.

[0076] The predetermined position of the camera is regarded as a 3D point X in the 3D model of the substation. c , X c The three-dimensional coordinates of c =(X c , Y c , Z c ), and the error vector of the actual position of the 3D point is The pixel point m* of the principal point of the plane image actually captured by the camera = (u*, v*) T , corresponding to X in the camera 3D model c , its calculation expression is:

[0077]

[0078] S3: Reposition the inspection robot, compensate for the position error of the inspection robot, and navigate the inspection robot to the predetermined inspection point.

[0079] Reposition the inspection robot, obtain the coordinates of the inspection robot in the three-dimensional model map of the substation, and calculate the displacement error vector t of the chassis. h , the posture error of the inspection robot is compensated, and the inspection robot is navigated to the predetermined inspection point.

[0080] It is able to calculate the navigation error between the preset stop position and the actual stop position of the substation inspection robot each time, and calibrate the current position of the substation inspection robot based on the calculation result, so that the inspection robot reaches the calibrated stop point and takes inspection images at the calibrated pan-tilt angle, thereby avoiding the inspection robot from gradually accumulating navigation errors when stopping at various inspection points along the predetermined inspection route, resulting in a linear increase in the pixel deviation between the pre-calibrated target detection image and the actual detection image.

[0081] Example 2

[0082] Reference Figure 2 and 3 , which is another embodiment of the present invention. This embodiment is different from the first embodiment in that it provides an experimental verification of a method for calculating the posture error of a substation inspection robot. In order to verify and illustrate the technical effect adopted in this method, this embodiment adopts a traditional technical solution and the method of the present invention for comparative testing, and compares the test results by means of scientific demonstration to verify the real effect of this method.

[0083] like Figure 2 As shown in the figure, a simulation experiment was conducted on a simulation platform on Unreal Engine 4.26. A virtual environment model of a real-world substation and an inspection robot were used on the practical simulation platform. It is fully controllable and can be ideally repeated. The linear velocity and angular velocity of the virtual robot and the resolution and intrinsic parameters of the virtual camera pose camera are the same as those of our real-world inspection robot. Gaussian random white noise is added to the motion control commands of the chassis and camera pose to simulate mechanical wear and tear, so as to be closer to the real situation.

[0084] According to the principle of pre-setting calibration points in substation inspection, we set 10 calibration points for the inspection robot in the virtual and real environments. In the virtual environment, the order of magnitude of the initial posture error is consistent with that in the real world.

[0085] The algorithm parameters were the same in both virtual and real world experiments. Specifically, the parameters of the PI controller were set to kp = 0.4 and ki = 0.04. The angle threshold eθ was chosen to be 0.1o. The pixel thresholds eu and ev were both chosen to be 1.0 pixel.

[0086] In addition, the depth of common feature points in the calibration, first frame and current image is calculated respectively by using 3D reconstruction technology, and the translation vector between the current posture and the calibration posture is obtained indirectly to avoid degradation problems. All methods start from the same position, then perform posture calculation and adjustment within 1.5 meters, posture calculation and adjustment of 1x focal length, and finally capture the inspection image with calibration zoom to facilitate the comparison of the inspection image with the predetermined inspection image.

[0087] To quantitatively evaluate the consistency between calibration and inspection images captured by different methods, we propose two metrics, image coverage and average dense optical flow (ADOF). Image coverage is defined as the ratio of the number of overlapping pixels between the inspection and calibration images to the image resolution. The higher the image coverage, the smaller the ADOF, and the more consistent the calibration and inspection images.

[0088] Please refer to Figure 3 ,Similar to the simulation experiments, we start from the same initial pose and execute different ,strategies once for each calibration point. Figure 3 (a) shows the comparison of the calculated translation errors. The proposed method achieves a median error of 26.50 mm, while the existing median errors are 102.83 mm and 167.91 mm, respectively. The advantage of the proposed method is that the proposed translation calculation algorithm does not require the application of the classical 2D-2D pose calculation method. APR represents the proposed method, and ACR and CRP represent the existing methods.

[0089] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in the scope of the claims of the present invention.

Claims

1. A method for calculating the posture error of a substation inspection robot, characterized in that: include: Establish a posture calculation model for the inspection robot; Based on the inspection robot posture calculation model, calculating the posture error between the inspection point where the inspection robot is currently located and the predetermined inspection point; Repositioning the inspection robot, compensating for the position and posture error of the inspection robot, and navigating the inspection robot to a predetermined inspection point; The inspection robot posture calculation model includes: The position and posture of the inspection robot is represented by a 4×4 matrix, and its calculation expression is: Among them, p represents the posture of the inspection robot, SE(3) represents the special Euclidean group, R∈R3×3 represents the orthogonal rotation matrix, t∈R3×1 represents the translation vector matrix, that is, the translation vector. For convenience, we use a two-tuple (R, t) to equivalently represent the posture p of the inspection robot; Tij = (Rij, tij) represents the transformation from Pi to Pj, that is, Pj = TijP i , that is, the transformation of the inspection robot's position p from point i to point j in a plane coordinate system; In the shooting state, the current chassis position of the inspection robot and the displacement error vector t of the chassis h The expression is: in, Indicates the current chassis posture, that is, the current posture of the inspection robot, P h Indicates the chassis position at the last inspection point, t h Represents the displacement error vector of the chassis, that is, the error vector between the displacement of the inspection robot from the previous inspection point to the next scheduled inspection point and the next scheduled inspection point actually reached; In the 3D model map of the substation, the position of the inspection robot is used as the coordinate system, where the inspection robot (t hx , t hy , t hz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the location, then the vector t of the inspection robot posture error h =(t hx , t hy , t hz ) T ; The predetermined position of the camera is regarded as a 3D point X in the 3D model of the substation. c , X c The three-dimensional coordinates of c =(X c , Y c , Z c ), corresponding to X in the camera 3D model c , its calculation expression is: Among them, X C represents the pose of the camera's intended position, represents the pose of the actual position of the camera, and the error vector of the actual position of the three-dimensional point is The inspection robot includes a calibration state and a shooting state when working. The camera posture Pc in the calibration state and the camera posture Pc in the shooting state The calculation expression is: P C =T PC T hP P h =<R x R y ,t hp +t pc Then, the calculation expression of the error Tc of the camera pose Pc is: Then, the inspection robot chassis position translation vector t h The expression is: Among them, t h represents the displacement error vector of the chassis, P c Indicates the camera pose in the calibration state. Indicates the camera pose in the shooting state, P p represents the gimbal position, P h Indicates the chassis posture in the calibration state, T hp represents the posture transformation matrix of the inspection robot in the calibration state, and T hp =(1,t hp ) and T pc =(1,t pc ), where t hp and t pc Represents the position parameter of the inspection robot at the inspection point, R x R y Represents the orthogonal matrix under the standard state, corresponding to the camera pose P c The pitch angle θ x and the rotation angle θ y , Indicates the position of the camera before adjustment in shooting state, T c Represents the camera pose P c The error, is the orthogonal matrix under the shooting state, corresponding to the pose under the shooting state The pitch angle θ x and the rotation angle θ y , R cx R cy The orthogonal matrix representing the pitch angle and rotation angle error of the camera corresponding to the predetermined shooting picture, t hp and t pc Indicates the position parameters of the inspection robot at the inspection point, Indicates the chassis posture under shooting status, represents the posture transformation matrix of the inspection robot under shooting state, The vector representing the current camera pose, that is, the current camera pose P c The pitch angle θ x and the rotation angle θ y The vector of c =(t cx ,t cy ,t cz ) T is the camera’s displacement error vector, (t cx ,t cy ,t cz ) are the coordinate parameters of the X-axis, Y-axis and Z-axis of the camera in the three-dimensional model of the substation.

2. The method for calculating the position and posture error of a substation inspection robot according to claim 1, characterized in that: Obtain a three-dimensional model map of the substation, and pre-mark an inspection route on the three-dimensional model map of the substation, calibrate inspection points on the inspection route, and calibrate the position of the inspection robot pan / tilt and the position of the camera at each inspection point.

3. The method for calculating the position and posture error of the substation inspection robot according to claim 1, characterized in that: Reposition the inspection robot, obtain the coordinates of the inspection robot in the three-dimensional model map of the substation, and calculate the displacement error vector t of the chassis. h , the posture error of the inspection robot is compensated, and the inspection robot is navigated to a predetermined inspection point.

Images