Positioning method and system for multi-sensor fusion of cable traction robot in narrow cable trench

Abstract

The invention discloses a narrow cable trench cable traction robot multi-sensor fusion positioning method, which comprises the following steps: S1, establishing a digital three-dimensional model of a whole cable trench, the digital three-dimensional model at least comprising a starting point and an ending point of each cable, a theoretical path of the cable, a position for pasting an ArUco code and an accurate coordinate of the ArUco code; s2, initializing a system, and acquiring prior information of the digital three-dimensional model; s3, initial positioning is carried out, and a universal coordinate system reference is established; s4, generating a smooth path which can be executed by the cable traction robot; and S5, the cable pulling robot is controlled to move. The invention further provides a positioning system for multi-sensor fusion of the cable traction robot in the narrow cable trench.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of power system construction, and particularly relates to a positioning method and system for a pulling robot in a narrow cable trench through multi-sensor fusion. BACKGROUND

[0002] In a substation, a factory or a large power facility, there is a kind of thin wire called "secondary cable" (for control, signal, protection, etc.), which needs to be pulled from one device to another. These cables are usually laid in underground or overground cable trenches, but these cable trenches have the characteristics of being narrow and long, with identical walls on both sides, poor GPS signals, dim light and few features.

[0003] The traditional method is for workers to drill in and pull the cable section by section by manpower, which is low in efficiency, high in intensity and prone to mispulling. SUMMARY

[0004] In view of the above-mentioned technical problems, the present application provides a positioning method and system for a pulling robot in a narrow cable trench through multi-sensor fusion, which can enable the robot to accurately position, automatically navigate and complete the cable laying task in a narrow, repeated and GPS-free cable trench.

[0005] The positioning method for a pulling robot in a narrow cable trench through multi-sensor fusion provided by the present application comprises the following steps: Step S1: Establish a digital three-dimensional model of the entire cable trench, which at least includes the starting point and the ending point of each cable, the theoretical path that the cable should take, the position of the ArUco code and its accurate coordinates; Step S2: System initialization and acquisition of prior information of the above-mentioned digital three-dimensional model; Step S3: Initial positioning, establishment of a general coordinate system reference; Step S4: Generation of a smooth path executable by the pulling robot, specifically comprising: Sub-step S41: Take out the original geometric path from the digital three-dimensional model; Sub-step S42: Perform global path planning on the basis of ensuring that each turning radius is not less than the minimum turning radius of the pulling robot; Sub-step S43: Collision detection to ensure that the path does not hit the wall or collide with the support; Sub-step S44: Smooth the entire path with a B-spline curve, and output a smooth, continuous and traceable trajectory; Step S5: Control the pulling robot to move, specifically comprising: Sub-step S51: According to the data of the wheeled odometer of the towing robot, the data of the inertial sensor, and the image data obtained by the camera module, the foregoing data is fused by extended Kalman filtering to obtain a predicted position, and the absolute position of the towing robot at this time is calculated according to the ArUco code obtained by the camera module of the towing robot, so as to correct the prediction error, and finally obtain the high-precision pose of the towing robot in the world coordinate system at present; Sub-step S52: According to the high-precision pose of the foregoing towing robot in the world coordinate system at present and the target point on the smooth path, the lateral deviation and the heading deviation are calculated, and the control parameters of the towing robot are obtained, at least including: the front wheel steering angle and the chassis acceleration; Sub-step S53: According to the control parameters calculated, the towing robot is controlled to move and pull the cable.

[0006] Further, the multi-sensor fusion positioning method of the narrow cable trench towing robot further comprises: Step S6: It is judged whether the end point coordinate of the path is reached, if the end point coordinate of the path is reached, the towing robot stops moving and releases the cable.

[0007] Further, the multi-sensor fusion positioning method of the narrow cable trench towing robot further comprises: Step S7: The task list is inquired to judge whether there is a cable to be laid, if there is a cable to be laid, the start point coordinate of the next cable is obtained, and step S4 is re-executed.

[0008] The application also provides a multi-sensor fusion positioning system of a narrow cable trench towing robot, comprising: A digital three-dimensional model, at least comprising: the start point and the end point of each cable, the theoretical path of the cable, the position of the pasted ArUco code and the accurate coordinate thereof; An initialization module for initializing the towing robot; A three-dimensional model acquisition module for acquiring the digital three-dimensional model of the current cable laying task, and further acquiring the prior information of the pasted ArUco code, the start and end points of each cable and the geometric path; An initial positioning module for setting the coordinate system of the towing robot; The path generation module comprises an original geometric path acquisition submodule, a global path planning submodule, a collision detection submodule and a B-spline curve submodule, the original geometric path acquisition submodule is used for taking out an original geometric path from a digital three-dimensional model, the global path planning submodule is used for performing global path planning on the basis of ensuring that each turning radius is not less than the minimum turning radius of the cable-drawing robot to generate a global path, the collision detection submodule is used for performing collision detection on the generated global path to ensure that the path does not collide with a wall and does not collide with a support, and the B-spline curve submodule is used for smoothing the whole path by using a B-spline curve to output a smooth, continuous and traceable trajectory. The control module comprises a multi-sensor fusion positioning submodule, a path tracking control submodule and a motion control submodule, the multi-sensor fusion positioning submodule is used for fusing data of a wheel odometer, data of an inertial sensor and image data obtained by a camera module of the cable-drawing robot by an extended Kalman filter to obtain a predicted position, and calculating an absolute position of the cable-drawing robot at the moment according to an ArUco code obtained by the camera module of the cable-drawing robot to correct a prediction error, and finally obtaining a high-precision pose of the cable-drawing robot in a world coordinate system, the path tracking control submodule is used for calculating a lateral deviation and a heading deviation according to the high-precision pose of the cable-drawing robot in the world coordinate system and a target point on the smooth path, and obtaining control parameters of the cable-drawing robot, at least comprising a front wheel steering angle and a chassis acceleration, and the motion control submodule is used for controlling movement and cable pulling of the cable-drawing robot according to the control parameters.

[0009] Further, the narrow cable trench cable-drawing robot multi-sensor fusion positioning system further comprises an endpoint judgment module for judging whether the endpoint of the path is reached.

[0010] Further, the narrow cable trench cable-drawing robot multi-sensor fusion positioning system further comprises a coordinate query module for querying a task list to judge whether there is a cable to be laid.

[0011] Compared with the prior art, the positioning method and the positioning system have at least the following beneficial effects: 1. By pasting an ArUco code as an "artificial landmark", an absolute position reference is provided for the cable-drawing robot during movement; 2. The cable-drawing robot can work indoors / underground without relying on GPS and by using local sensor fusion; 3. Periodic deviation correction is realized by using the ArUco code to eliminate accumulated errors, so that the cable-drawing robot can still be accurate during long-distance movement; 4. The minimum turning radius is considered during path planning and collision detection is performed, so that the cable-drawing robot can not collide with a wall and can not be stuck during movement; and 5. By smoothing the whole path by using a B-spline curve and combining intelligent LQR control, the cable-drawing robot can move stably and pull smoothly. Attached Figure Description

[0012] The accompanying drawings, which are provided to further illustrate this application and form part of this application, illustrate exemplary embodiments of this application and are used to explain this application, but do not constitute an undue limitation of this application.

[0013] Figure 1 This is a flowchart illustrating a preferred embodiment of a multi-sensor fusion positioning method for a towing robot in a narrow cable trench according to the present invention.

[0014] Figure 2 yes Figure 1 The sub-flowchart of step S4.

[0015] Figure 3 yes Figure 1 The sub-flowchart of step S5.

[0016] Figure 4 This is a block diagram illustrating a preferred embodiment of the multi-sensor fusion positioning system for a narrow cable trench towing robot according to the present invention.

[0017] Figure 5 for Figure 4 A block diagram of the path generation module.

[0018] Figure 6 for Figure 4 A block diagram of the control module. Detailed Implementation

[0019] The following specific examples illustrate the implementation of the present invention. The illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0020] Please refer to Figure 1 As shown, the positioning method of a multi-sensor fusion robot for pulling a cable in a narrow cable trench according to the present invention includes the following steps: Step S1: Create a digital 3D model of the entire cable trench and paste ArUco codes at the corresponding locations.

[0021] Before construction, engineers first use a computer to create a digital 3D model of the entire cable trench (the trench where secondary cables will be laid). This digital 3D model includes at least: the start and end points of each cable; the theoretical path the cable should take; the location of the ArUco codes and their precise coordinates. In this embodiment, ArUco codes are used for annotation. ArUco codes are a visual marking system based on black and white square patterns, commonly used in computer vision tasks such as pose estimation, augmented reality, and robot navigation. Other identifiers may be used in other embodiments.

[0022] After that, according to the accurate coordinates of each ArUco code included in the aforementioned digital three-dimensional model, the ArUco codes are pasted on the walls at the corresponding positions of the cable trench.

[0023] In this step, the purpose of pasting the ArUco codes is to provide the towing robot with an absolute position reference point, so that it does not need to rely on GPS for positioning.

[0024] Step S2: The system is initialized, and the prior information of the aforementioned digital three-dimensional model is obtained.

[0025] In this step, during the system initialization process, the towing robot starts the ROS system and checks whether each node has an abnormality. If there is an abnormality, the abnormality problem is first checked, and then the system is reinitialized. The towing robot obtains the prior information of the ArUco code pasting according to the digital three-dimensional model of the current cable laying task. The ArUco code is pre-planned to be pasted at a position by the digital three-dimensional model, and then manually pasted at the corresponding position in the actual cable trench. The towing robot can obtain the coordinates of the ArUco code through the upper design platform. In addition, the towing robot will also obtain the start and end points and the path of each cable. For each cable, the towing robot obtains the start point, end point coordinates and geometric path connecting the start and end points through the cable label from the upper design platform. The ArUco code can use multiple 7x7, 100mm side length ArUco codes as marker codes pasted on the inner wall of the cable trench.

[0026] Step S3: Initial positioning, that is, setting the origin. The purpose is to establish a unified "coordinate system reference".

[0027] In this embodiment, the system sets the towing robot to take the start point of a certain cable in the digital three-dimensional model as the origin of the world coordinate system, and then places the towing robot at this position to initialize its inertial navigation module and camera module. Taking the initial robot coordinate system of the towing robot as the world coordinate system, the initial positioning of the towing robot can be realized.

[0028] Step S4: Generating a smooth path executable by the towing robot. Specifically, this step includes the following specific sub-steps, as shown in Figure 2

[0029] Step S41: Extracting the original geometric path from the digital three-dimensional model.

[0030] ​Since the geometric paths are obtained from the digital three-dimensional model, these paths are usually planned by Dijkstra and other methods, which are based on the shortest paths of discrete grids or graph structures. The paths usually contain many sharp turns and discontinuous points, which are the shortest on the discrete grid, but for the actual towing robot, the sharp turns and discontinuous points can be difficult to track, and these paths lack smoothness, which can cause the towing robot to shake or be unstable during tracking.

[0031] Step S42: global path planning. The key point of this step is to ensure that each turning radius is not less than the minimum turning radius of the towing robot.

[0032] In this embodiment, the path planning method RRT (Rapidly-exploring Random Tree) can be used for global path planning, and the kinematic model is used to verify the search path and calculate the next sampling point of the qualified path. In the RRT sampling process, each sampling is constrained by the minimum turning radius of the towing robot, and if the sampled point exceeds the kinematic constraint, the calculated turning angle is limited by the maximum turning angle allowed by the kinematic model, and then the limited turning angle is substituted into the kinematic model to calculate a new iteration path point that meets the kinematic model to extend the path.

[0033] Specifically, the kinematic model is:

[0034] wherein is the linear velocity of the chassis at the previous time step, is the time step, is the average of the two front wheel steering angles, is the front wheel track, , are the x-coordinate positions of the previous time state and the current time state, respectively, , are the y-coordinate positions of the previous time state and the current time state, respectively, , are the heading angles of the previous time state and the current time state, respectively.

[0035] In the formula, some variables are defined as follows, wherein , are the angles between the left front wheel and the right front wheel and the motion direction, respectively, is the tire radius.

[0036]

[0037] Considering the minimum turning radius constraint of the towing robot, the curvature of the path of the towing robot The maximum allowable value (i.e. related to the minimum turning radius) must be met. The calculation formula of the curvature is: ; The constraint condition is:

[0038] Wherein is the minimum turning radius of the towing robot.

[0039] Step S43: collision detection is performed to ensure that the path does not hit the wall and does not collide with the support.

[0040] When performing global path planning, the environmental boundary condition of the towing robot also needs to be considered. First, a polygon model of the towing robot is constructed. When optimizing the environmental boundary condition in the path planning process, the position of the towing robot at the next time is calculated according to the current position and heading angle of the towing robot, and then it is checked whether the polygon of the towing robot overlaps with the boundary of the cable trench. If there is, the process is terminated and the next iteration is performed. The next iteration repeats the above process of sampling, kinematic constraint, and collision checking.

[0041] Step S44: the whole path is smoothed by a B-spline curve (B-spline Curve), and a smooth, continuous and traceable trajectory is output.

[0042] The above step S4 is performed only once at the beginning of each new cable task, rather than repeatedly in real time.

[0043] Step S5: controlling the towing robot to move. This step is the most core step in the present application, and the purpose is to control the towing robot to walk, position and control at the same time, so that it can adhere to the smooth path in the early stage to lay the cable. Specifically, the step S5 includes the following sub-steps, as shown in Figure 3 .

[0044] Step S51: multi-sensor fusion positioning. In this step, the following understanding can be made first: The input is: the data of the wheeled odometer of the towing robot, the data of the inertial sensor, and the image data obtained by the camera module.

[0045] The processing is: the data of the wheeled odometer and the data of the inertial sensor are fused by using the extended Kalman filter to obtain the predicted position. If the ArUco code is captured by the camera module and is directly opposite (for example, the included angle <1°), the absolute position of the towing robot at this time is calculated by using it to correct the prediction error.

[0046] The output is: the high-precision pose (x, y, θ) of the towing robot in the world coordinate system.

[0047] Since the inertial navigation itself has errors, and the errors will accumulate over time, it is necessary to combine the camera module (depth camera in this embodiment) included in the towing robot to correct the pose information of the towing robot at this time by recognizing the ArUco code pasted on the cable trench wall according to a certain rule, thereby realizing the autonomous positioning function of the towing robot in the cable trench.

[0048] In this embodiment, the data of the wheeled odometry and the inertial sensor are fused through extended Kalman filtering. This method improves the accuracy and robustness of system state estimation through prediction and update processes, and reduces the cumulative error and drift caused by a single sensor. Prediction mainly predicts the robot state at the next time through the data of the inertial sensor and the current state of the towing robot, and the update step mainly uses the wheeled odometry data and the predicted state to correct the state of the robot.

[0049] In this step, the fused data of the wheeled odometry and the inertial sensor is corrected by the camera module mounted on the towing robot through vision. In this method, the coordinate systems are defined as follows: the towing robot coordinate system , the ArUco coordinate system , the world coordinate system , the camera coordinate system , the world coordinate system is defined to coincide with the initial state of the robot coordinate system, and the robot position is defined as the position of the robot in the world coordinate system during movement. The translation and rotation vectors of the ArUco code relative to the camera module can be calculated by the PnP algorithm, and the rotation vector can be converted into a rotation matrix by calculation. The translation and rotation matrix from the ArUco code coordinate system to the camera coordinate system , so as to calculate the pose of the camera relative to the ArUco code. The translation and rotation matrix of the camera coordinate system and the car coordinate system can be obtained through the calibration of the towing robot and the sensor . Secondly, the Euler angles used to represent the robot pose include pitch angle, yaw angle, and roll angle, which are defined as the angles of rotation around the y-axis, z-axis, and x-axis of the robot coordinate system, respectively. The towing robot moves in a two-dimensional plane throughout the entire process.

[0050] The data that can be acquired by the method include: image pictures taken by the camera module, position changes output by the wheel odometer, and angular velocity and linear acceleration of the inertial sensor. First, the data of the wheel odometer and the inertial sensor are fused through an extended Kalman filter to reduce the cumulative error of the aforementioned sensors during the operation of the robot. Meanwhile, during the movement of the towing robot, the camera module detects the ArUco code in real time according to the acquired image. If the ArUco code appears in the field of view of the camera, the position and attitude of the camera module in the ArUco code coordinate system are detected in real time. When the included angle between the towing robot and the z-axis of the ArUco code coordinate system is less than a certain threshold, the detection result of the first frame of ArUco image acquired within the threshold is selected. In this embodiment, 1° is used as the threshold. Through the coordinate system transformation matrix, the pose of the robot in the ArUco code coordinate system is first calculated, and then the pose of the robot in the world coordinate system is calculated according to the prior information of the coordinates of the ArUco code in the world coordinate system. The estimated pose of the inertial navigation in the world coordinate system is corrected, and then the motion trajectory of the robot is corrected according to the deviation between the pose and the expected pose.

[0051] Step S52: path tracking control. In this step, the following understanding can be made simply: Input: current pose (from step S51), target point on the smoothed path (query according to current progress).

[0052] Processing: calculate lateral deviation (i.e., how far the towing robot is from the path) and heading deviation (i.e., how much the towing robot's car head is tilted); use an LQR (Linear Quadratic Regulator) controller to calculate the following parameters of the towing robot: how many degrees the front wheel should be turned, whether the chassis should be accelerated or decelerated.

[0053] Output: control instructions sent to the chassis motor and steering mechanism of the towing robot.

[0054] The principle of this step will be described in detail as follows: The longitudinal control uses a given linear velocity, and the lateral control uses an LQR controller to control the angular velocity. The LQR method finds a state feedback gain matrix K to minimize the performance index:

[0055]

[0056] where is the system state vector, is the control input, is a positive definite or semi-positive definite matrix used to measure the state deviation, ​is a positive definite matrix that measures the control energy.

[0057] The state vector is denoted as: , are the lateral and longitudinal coordinates, the heading angle, and the linear velocity of the towing robot, respectively.

[0058] The control vector is denoted as: where is the front wheel steering angle, is the linear acceleration.

[0059] According to the kinematic equation of the towing robot, the above kinematic model can be linearized and represented in the standard state-space form. The kinematic equation of the towing robot is:

[0060] where L is the wheelbase of the towing robot. After linearizing the nonlinear model, it can be represented in the standard state-space form:

[0061] where

[0062]

[0063] To adapt to the narrow environment of the cable trench and achieve more precise control, reinforcement learning is used to dynamically adjust the Q matrix and R matrix of the LQR controller in real time. The Actor-Critic algorithm is used to achieve this, with the optimization goal being to maximize the long-term cumulative reward obtained by the agent in the environment through collaborative optimization of the policy (Actor) and value evaluation (Critic). The Critic network is used to predict the long-term expected return under the current policy, and the value function is as follows:

[0064] where π is the current policy of the Actor network, γ is the discount factor, is the immediate reward at the kth step in the future.

[0065] The reward function is defined as

[0066] is the lateral tracking error penalty term, is the heading angle error penalty term, is the control amount penalty term, is the collision penalty term.

[0067] By minimizing the TD error, the Critic network learns to evaluate the long-term rewards of different adjustment strategies, and the Actor network adjusts the parameter output to be better by the gradient direction provided by the Critic network and , update Q and R.

[0068] Then solve the Riccati equation, in the case of continuous time:

[0069] Calculate the symmetric positive definite matrix P.

[0070] Finally, use P to calculate K:

[0071] Finally, use the control rate to calculate

[0072] Implement feedback control.

[0073] Step S53: According to the instruction calculated in step S52, control the cable robot to move and pull the cable. Specifically, the chassis of the cable robot advances according to the control instruction, and the steering angle of the front wheel is controlled, that is, the motion state of the entire cable robot is controlled. At the same time, the mechanical arm or traction device of the cable robot synchronously drags the cable and maintains appropriate tension. In addition, the camera module of the cable robot will continuously scan the wall surface of the cable trench, wait for the appearance of the next ArUco code, and repeat the above steps S51 and S52 to correct the motion of the cable robot in real time.

[0074] Step S6: Determine whether the end point coordinate of this path has been reached. If it has been reached, the cable robot stops moving and releases the cable, and sends a "task completed" signal to the upper computer.

[0075] Step S7: Query the task list to determine whether there is a cable to be laid. If there is, obtain the starting point coordinate of the next cable, and re-execute step S4.

[0076] During the reciprocating motion of the cable robot, the robot obtains the coordinate of the starting point of the next cable to be laid (usually the coordinate of the starting point of the last cable, or a new laying path) from the digital three-dimensional model, and the coordinate of the current point. Repeat the above steps to navigate the cable robot to the laying starting point of the next cable, and repeat the above process to pull the next cable to the end point.

[0077] Through the above steps, the path tracking of the cable pulling robot is completed, so that the cable pulling robot can complete the smoothing of the geometric path according to the given starting point set and the ending point set, pull the cable from the starting point to the ending point, and then reach the laying starting point of the next cable according to the next laying task, continue to complete the cable laying task of the next cable, until the cable laying task is completed.

[0078] Compared with the prior art, the positioning method of the narrow cable trench pulling robot multi-sensor fusion has at least the following beneficial effects: 1. By pasting ArUco codes as "artificial road signs", absolute position reference is provided for the cable pulling robot during travel; 2. Not dependent on GPS, local sensor fusion is adopted, so that the cable pulling robot can work indoors / underground; 3. Periodic deviation correction is realized by using ArUco codes, and accumulated errors are cleared, so that the cable pulling robot can still be accurate during long-distance travel; 4. The minimum turning radius is considered during path planning and collision detection, so that the cable pulling robot can not bump into walls or get stuck during travel; 5. By smoothing the whole path with a B-spline curve, and combining with intelligent LQR control, the cable pulling robot can travel stably and pull smoothly.

[0079] Please continue to refer to Figure 4 As shown in the drawings, the positioning system of the narrow cable trench pulling robot multi-sensor fusion of the present application comprises a digital three-dimensional model 1, an initialization module 2, a three-dimensional model acquisition module 3, an initial positioning module 4, a path generation module 5, a control module 6, an end point judgment module 7, and a query module 8.

[0080] As described in the above positioning method, the digital three-dimensional model 1 in the positioning system at least comprises: the starting point and the ending point of each cable; the theoretical path that the cable should take; the position of the pasted ArUco code, and the accurate coordinates thereof.

[0081] The initialization module 2 is used for initializing the cable pulling robot. Specifically, the cable pulling robot starts the ROS system, and checks whether each node has an abnormality. If there is an abnormality, the abnormality problem is first checked, and then reinitialized. The three-dimensional model acquisition module 3 is used for acquiring the digital three-dimensional model of the current cable laying task, and further acquiring the prior information of the pasted ArUco code, the starting and ending points of each cable, and the geometric path. The initial positioning module 4 is used for setting the coordinate system of the cable pulling robot. Specifically, the initial positioning module 4 sets the starting point of a certain cable in the digital three-dimensional model as the origin of the world coordinate system, and then places the cable pulling robot at the position to initialize the inertial navigation module and the camera module of the cable pulling robot. With the initial robot coordinate system of the cable pulling robot as the world coordinate system, the initial positioning of the cable pulling robot is realized.

[0082] The path generation module 5 is used for generating a smooth path executable by the cable pulling robot. More specifically, as described in the above positioning method, the path generation module 5 is used for generating a smooth path executable by the cable pulling robot.Figure 5 As shown in the figure, the path generation module 5 comprises an original geometric path acquisition submodule 51, a global path planning submodule 52, a collision detection submodule 53 and a B-spline curve submodule 54. The original geometric path acquisition submodule 51 is used to take out the original geometric path from the digital three-dimensional model. The global path planning submodule 52 is used to perform global path planning on the basis of ensuring that each turning radius is not less than the minimum turning radius of the cable-drawing robot, so as to generate a global path. The collision detection submodule 53 is used to perform collision detection on the aforementioned generated global path, so as to ensure that the path does not bump into walls and does not collide with supports. The B-spline curve submodule 54 is used to smooth the whole path by using a B-spline curve, and outputs a smooth, continuous and traceable trajectory. The specific details have been described in detail in the aforementioned method, and thus will not be described here again.

[0083] The control module 6 is used to control the movement of the cable-drawing robot along the aforementioned global path. Specifically, please refer to Figure 6 As shown in the figure, the control module 6 comprises a multi-sensor fusion positioning submodule 61, a path tracking control submodule 62 and a motion control submodule 63. The multi-sensor fusion positioning submodule 61 is used to fuse the data of the wheel odometer of the cable-drawing robot, the data of the inertial sensor and the image data obtained by the camera module through extended Kalman filtering, so as to obtain a predicted position, and calculate the absolute position of the cable-drawing robot at this time according to the ArUco code obtained by the camera module of the cable-drawing robot, so as to correct the prediction error, and finally obtain the high-precision pose of the cable-drawing robot in the world coordinate system at present. The path tracking control submodule 62 is used to calculate the lateral deviation and the heading deviation according to the aforementioned high-precision pose of the cable-drawing robot in the world coordinate system at present and the target point on the smooth path, and obtain the control parameters of the cable-drawing robot, at least including: the front wheel steering angle and the chassis acceleration. The motion control submodule 63 is used to control the movement and the pulling of the cable of the cable-drawing robot according to the aforementioned calculated control parameters.

[0084] The end point judgment module 7 is used to judge whether the end point of the path is reached. If it is reached, the cable-drawing robot stops moving and releases the cable, and sends a “task completed” signal to the upper computer. The coordinate query module 8 is used to query the task list to judge whether there is a cable to be laid.

[0085] The above embodiment only expresses the preferred embodiment of the present application, which is described in detail and specifically, but cannot be understood as the limitation of the patent scope of the present application. It should be noted that, for those skilled in the art, without departing from the concept of the present application, a number of modifications and improvements can be made, which all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application should be subject to the appended claims.

Claims

1. A positioning method of a narrow cable trench pulling robot multi-sensor fusion, characterized in that, Comprise: Step S1: establish a digital three-dimensional model of the entire cable trench, which at least includes: the starting point and the end point of each cable, the theoretical path that the cable should take, the position of the ArUco code pasted and its accurate coordinates; Step S2: system initialization, and obtain the prior information of the above-mentioned digital three-dimensional model; Step S3: initial positioning, establish a general coordinate system reference; Step S4: generate a smooth path executable by the pulling robot, specifically including: Substep S41: take out the original geometric path from the digital three-dimensional model; Substep S42: global path planning is carried out on the basis of ensuring that the radius of each turn is not less than the minimum turning radius of the pulling robot; Substep S43: collision detection is carried out to ensure that the path does not hit the wall and does not collide with the support; Substep S44: use B-spline curve to smooth the whole path, output a smooth, continuous and traceable trajectory; Step S5: control the pulling robot to move, specifically including: Substep S51: according to the data of the wheel odometer of the pulling robot, the data of the inertial sensor and the image data obtained by the camera module, the foregoing data is fused by extended Kalman filter to obtain the predicted position, and the absolute position of the pulling cable robot is calculated according to the ArUco code photographed by the camera module of the pulling cable robot to correct the prediction error, and finally the high-precision pose of the pulling cable robot in the world coordinate system is obtained; Substep S52: according to the high-precision pose of the foregoing pulling cable robot in the world coordinate system and the target point on the smooth path, the lateral deviation and the heading deviation are calculated, and the control parameters of the pulling cable robot are obtained, at least including: front wheel steering angle and chassis acceleration; Substep S53: control the pulling cable robot to move and pull the cable according to the control parameters calculated above.

2. The method of claim 1, wherein the method is a narrow cable trench guiding robot multi-sensor fusion positioning method, characterized in that, Further comprising: Step S6: judge whether the end point coordinate of this path is reached, if it is reached, the pulling cable robot stops moving and releases the cable.

3. The method of claim 1, wherein, Further comprising: Step S7: query the task list to determine whether there is a cable to be laid, if there is, get the starting point coordinates of the next cable, and re-execute step S4.

4. A narrow cable trench pulling robot multi-sensor fusion positioning system, characterized by, Comprise: Digital three-dimensional model, at least including: the starting point and the end point of each cable, the theoretical path that the cable should take, the position of the ArUco code pasted and its accurate coordinates; Initialization module, used for initializing the pulling cable robot; Three-dimensional model acquisition module, used for acquiring the digital three-dimensional model of the current cable laying task, and further acquiring the prior information of the ArUco code pasted, the starting and ending points of each cable and the geometric path; Initial positioning module, used for setting the coordinate system of the pulling robot; The path generation module comprises an original geometric path acquisition submodule, a global path planning submodule, a collision detection submodule and a B-spline curve submodule. The original geometric path acquisition submodule is used to take out an original geometric path from a digital three-dimensional model. The global path planning submodule is used to perform global path planning on the basis of ensuring that each turning radius is not smaller than the minimum turning radius of the cable-drawing robot, so as to generate a global path. The collision detection submodule is used to perform collision detection on the global path generated in the foregoing, so as to ensure that the path does not collide with a wall or a support. The B-spline curve submodule is used to smooth the entire path by using a B-spline curve, and output a smooth, continuous and traceable trajectory. The control module comprises a multi-sensor fusion positioning submodule, a path tracking control submodule and a motion control submodule. The multi-sensor fusion positioning submodule is used to fuse data of a wheeled odometer, data of an inertial sensor and image data obtained by a camera module of the cable-drawing robot by using an extended Kalman filter, so as to obtain a predicted position. An absolute position of the cable-drawing robot at the moment is calculated according to an ArUco code obtained by the camera module of the cable-drawing robot, so as to correct a prediction error. Finally, a high-precision pose of the cable-drawing robot in a world coordinate system is obtained. The path tracking control submodule is used to calculate a lateral deviation and a heading deviation according to the high-precision pose of the cable-drawing robot in the world coordinate system and a target point on the smooth path, and obtain control parameters of the cable-drawing robot, at least including a front wheel steering angle and a chassis acceleration. The motion control submodule is used to control movement and cable drawing of the cable-drawing robot according to the control parameters calculated in the foregoing.

5. A narrow cable trench jockey robot multi-sensor fusion positioning system as claimed in claim 4, characterized in that: The end point judgment module is further included, which is used to judge whether the end point of the path is reached.

6. A narrow cable trench jockey robot multi-sensor fusion positioning system as claimed in claim 4, characterized by: The coordinate query module is further included, which is used to query a task list, so as to judge whether there is a cable to be laid.

Images