Modularized movement control system of tool changing robot

Abstract

The invention discloses a modular movement control system of a tool changing robot, and relates to the technical field of automation equipment, a global formulation module establishes a kinematics model through a D-H method, receives different task signals, determines an initial joint pose group and an end point base coordinate, and combines and constructs a path planning task; the simulation planning module obtains a path planning task, and controls the tool changing robot to perform collision-free movement in the shield tunneling machine internal environment modeling so as to generate a joint movement scheme; the driving execution module sequentially adjusts the joint pose groups according to the joint movement scheme to complete a path planning task, autonomously decides whether to shoot a visual image and a depth image or not, obtains a joint fine adjustment scheme and finely adjusts the joint pose groups to complete a corresponding operation task; the analysis feedback module obtains a visual image and a depth image, a target feature pose analysis method is adopted to calculate a deviation angle and offset, a joint fine adjustment scheme is generated through reverse backtracking, and collision-free high-precision control over the tool changing robot is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of automation equipment, and in particular to a modular mobile control system for a tool-changing robot. Background Art

[0002] Shield machines are widely used in underground tunnel projects, but tool replacement faces many difficulties after tool wear. The traditional manual tool replacement method requires entering the pressurized working area behind the cutterhead to remove the old tool and install a new one. The process is dangerous, time-consuming and inefficient, and manual tool replacement is prone to safety accidents. The emergence of tool changing robots has made it possible to solve these problems. Tool changing robots can safely replace tools in narrow and harsh working environments through the cooperation of robotic arms and actuators.

[0003] The existing Chinese patent with authorization announcement number CN109623814B discloses a robotic arm control method, including the following steps: establishing a forward kinematic model of the robotic arm as system input, giving control parameters of an adaptive mutation differential evolution algorithm; randomly initializing a population in the parameter space; randomly selecting individuals from the population for mutation operations to generate mutation vectors; cross-operating the mutation vectors with the target vectors to generate test vectors; comparing the test vectors with the target vectors to select the better individual as the new target vector; judging whether a preset stopping criterion is met, if so, ending and outputting the best individual as the joint variation matrix of the robotic arm, otherwise returning to the mutation operation, thereby achieving the goal of obtaining the shortest stroke solution while ensuring the posture accuracy of the robotic arm, thereby improving work efficiency.

[0004] However, existing robotic arm control technology does not take into account obstacles in the environment and lacks effective feedback fine-tuning methods, and cannot be directly applied to shield machine tool replacement work. Summary of the Invention

[0005] In response to the shortcomings of the existing technology, the present invention proposes a modular mobile control system for a tool changing robot to provide path planning for the tool changing robot taking into account obstacle occlusion and effective feedback fine-tuning, ensuring that the tool changing robot can accurately and efficiently complete tool replacement.

[0006] The technical solutions for achieving the purpose of the present invention are:

[0007] A modular mobile control system for a tool-changing robot, comprising a global formulation module, a simulation planning module, a drive execution module, and an analysis and feedback module;

[0008] The global formulation module establishes the tool-changing robot kinematic model through the DH method, receives different task signals and determines the end point base coordinates, uses the current joint pose group as the starting joint pose group, and combines them to construct the corresponding path planning task;

[0009] The simulation planning module obtains the path planning task, uses the path planning algorithm based on the dual detection strategy to simulate and adjust the joint posture group, simulates the completion of the path planning task and generates the corresponding joint motion plan;

[0010] The drive execution module adjusts the joint posture group in sequence according to the joint motion plan to complete the path planning task. Based on the joint motion plan, it independently decides whether to capture visual images and depth images and send them to the analysis and feedback module. It obtains the joint fine-tuning plan and further fine-tunes the joint posture group to complete the corresponding operation task.

[0011] The analysis feedback module obtains visual images and depth images, and calculates the target coordinate system in the visual image through target feature pose analysis Relative to the joint coordinate system The rotation angle and offset are calculated and the joint fine-tuning scheme is generated through reverse backtracking and fed back to the drive execution module.

[0012] Furthermore, the global formulation module establishes the tool-changing robot kinematic model through the DH method, which includes the following specific steps:

[0013] Count the total number of tool changing robot joints The starting point of the moving guide rail, the direction perpendicular to the tool change plane, and the direction perpendicular to the moving plane and upward are used as the base coordinate system. The origin, Axis direction and Axis direction, determine the base coordinate system by the right-hand screw rule of Axis direction;

[0014] Based on the The joint coordinate system is determined by the execution function of each joint of Axis direction, based on Axis and joints The common perpendicular line of the axis determines the joint coordinate system The origin and Axis direction, determine the joint coordinate system by the right-hand screw rule of Axis direction, get the direction from the first joint to the The joint coordinate system of each joint;

[0015] The center of the actuator front plane, perpendicular to the front plane and pointing to the direction of the gripper and the joint coordinate system of The directions of the axes are respectively Joint coordinate system of each joint The origin, Axis direction and Axis direction, determine the joint coordinate system by the right-hand screw rule of Axis direction, construct the Joint coordinate system of each joint ;

[0016] Based on the joint coordinate system and joint coordinate system The relationship between the common perpendicular lines and the rotation and offset between the corresponding coordinate axes are defined in the first Connecting rod parameters of each connecting rod;

[0017] Derived from the basis coordinate system by Jacobian matrix To joint coordinate system The transformation matrix .

[0018] Furthermore, construct Joint coordinate system of each joint The specific steps include:

[0019] Determine the The execution function of a joint. If the execution function is rotation, the joint coordinate system of Axis direction is The joint axis direction of each joint. If the execution function is linear motion, the joint coordinate system of Axis direction is The linear motion direction of each joint, , is the total number of joints;

[0020] The first joints Axis and joints The common perpendicular line of the axis is The perpendicular foot on the axis is used as the joint coordinate system The origin of

[0021] will be parallel to Axis and The common perpendicular of the axis and Axis direction The direction of the axis as the joint coordinate system of Axis direction;

[0022] Determine the joint coordinate system using the right-hand screw rule of Axis direction, joint coordinate system Build completed.

[0023] Furthermore, The connecting rod parameters include the connecting rod length , connecting rod angle , connecting rod offset and joint angles ;

[0024] Connecting rod length for Axis to The distance between the common perpendiculars of the axes;

[0025] Connecting rod angle for Shaft winding The shaft rotates to The angle of rotation when the axes are parallel;

[0026] Connecting rod offset is the joint coordinate system Origin to Axis and The distance from the common perpendicular to the axis;

[0027] joint angle for Shaft winding The shaft rotates to The rotation angle when the axes are parallel.

[0028] Furthermore, the transformation matrix is ​​calculated through the Jacobian matrix The specific steps include:

[0029] The base coordinate system To joint coordinate system The transformation process is decomposed into Single-step transformation, each single-step transformation is to transform the base coordinate system Or the joint coordinate system of the current joint is transformed to the joint coordinate system of the next joint;

[0030] For the joint coordinate system , the joint coordinate system Around Shaft rotation is equivalent to connecting rod rotation angle angle, so that Axis and Axis parallel, obtain a transformed joint coordinate system , define the transformation matrix of this sub-step as ;

[0031] Transform the joint coordinate system once along Axial translation link length ,make Axis and Axis coincidence, obtain the secondary transformation joint coordinate system , define the transformation matrix of this sub-step as ;

[0032] Transform the joint coordinate system twice Around Axis rotation is equivalent to joint angle angle, so that Axis and Axis parallel, obtain the three-dimensional transformation joint coordinate system , define the transformation matrix of this sub-step as ;

[0033] Transform the joint coordinate system three times along Axial translation link offset , so that the joint coordinate system is transformed three times and joint coordinate system Coincidentally, define the transformation matrix of this sub-step as ;

[0034] Multiply the transformation matrices of the four sub-steps in the single-step transformation process to obtain the joint coordinate system To joint coordinate system The single-step transformation matrix , further from the base coordinate system To joint coordinate system Each single-step transformation matrix in is multiplied to obtain the transformation matrix .

[0035] Furthermore, the global formulation module receives different task signals and constructs corresponding path planning tasks including:

[0036] Receive the bolt removal signal and extract the tool change number to obtain the tool box base coordinates , determine that the operating device is a wrench, and change the tool box base coordinates of Axis value Subtract wrench size and safe distance , generate the first end point base coordinates , and the default joint pose group to construct path planning task one;

[0037] Receive the old tool removal signal, determine that the operating device is a clamp, and obtain the joint posture group 1 and the tool box base coordinates , the tool box base coordinates of Axis value Subtract jaw size and safe distance , generate the second end point base coordinates And combined with joint posture group 1 to construct path planning task 2;

[0038] Receive the old tool recovery signal, determine that the operating device is a gripper, obtain the joint posture group 2 and the base coordinates of the old tool recovery box , the old knife recycling box base coordinates of Axis value Subtract jaw size and safe distance , generate the third end point base coordinates And combined with joint pose group 2 to construct path planning task 3;

[0039] Receive the new tool installation signal, determine that the operating device is a clamp, and obtain the joint posture group three and the second end point base coordinates , the base coordinates of the second endpoint Directly used as the fourth end point base coordinate And combined with joint pose group three to construct path planning task four;

[0040] Receive the bolt installation signal, determine that the operating device is a wrench, and obtain the joint pose group 4 and the first end point base coordinates , the first end point base coordinate Directly used as the fifth endpoint base coordinate And combined with joint pose group 4 to construct path planning task 5;

[0041] Receive the task end signal, determine that the operating device is a wrench, obtain joint posture group 5 and the default joint posture group, determine the wrench default base coordinates based on the default joint posture group and use them as the sixth end point base coordinates , combined with joint pose group five to construct path planning task six.

[0042] Specifically, the shield machine internal environment modeling and tool changing robot modeling can be pre-established by combining the existing scanning technology with the existing 3D reconstruction technology, based on the base coordinate system. It is placed in the simulation software to simulate and plan the movement path of the tool changing robot. The internal environment model of the shield machine is divided into a cubic grid. If there is an obstacle in the cubic grid, the grid status is marked as occupied. If there is no obstacle in the cubic grid, the grid status is marked as idle.

[0043] Furthermore, the simulation planning module obtains the path planning task and uses the path planning algorithm to generate the corresponding joint motion plan, including the following specific steps:

[0044] Get the path planning task and obtain the corresponding starting joint pose group and end point base coordinates;

[0045] Create an empty random tree, set the starting joint pose group as the root node, and define Axis About The cosine of the angle between the axes is ,in, for Axis About The angle between the axes;

[0046] Set the maximum number of iterations and the search radius for the joint , set the path planning constraints according to the operating device. If the operating device is a wrench, it is required Axis and Cosine of the angle between the axes If the operating device is a gripper, it is required to be -1. Axis and Cosine of the angle between the axes is 1;

[0047] In each iteration of the path planning algorithm, the target base coordinates of this iteration are randomly generated. With the help of the tool changing robot kinematic model, the inverse kinematics algorithm is used to solve the target joint pose group corresponding to the target base coordinates.

[0048] Search the random tree for the node with the smallest Euclidean distance to the target joint pose group as the nearest node. Use the double detection strategy to determine whether the nearest node can be adjusted to the target joint pose group. If it cannot be adjusted to the target joint pose group, start a new round of iteration. If it can be adjusted to the target joint pose group, add the target joint pose group as a child node of the nearest node into the random tree and establish the corresponding edge, starting a new round of iteration.

[0049] Until the maximum number of iterations is reached, all nodes of the random tree are traversed, and each node is judged in turn whether it satisfies both the first condition and the path planning constraint condition. The first condition is that the Euclidean distance between the target base coordinate and the end point base coordinate corresponding to the node is less than the distance error threshold;

[0050] Obtain all nodes that meet the first condition and path planning constraints, trace back to the root node based on the parent-child relationship of each node to generate the path corresponding to the node, select the path with the smallest total side length and arrange the nodes forward according to the parent-child relationship to generate a joint motion plan.

[0051] Preferably, as an improvement, a multilayer perceptron that introduces a singular value penalty term and a bottleneck structure is used instead of the inverse kinematics algorithm to solve the target joint pose group corresponding to the target base coordinates to solve the multiple solution problem when the inverse kinematics algorithm is solved, wherein the bottleneck structure is achieved by inserting a fully connected layer with only half the number of neurons of the fully connected layers on both sides between two fully connected layers with the same number of neurons, and the singular value penalty term is equal to the product of the minimum singular value of the Jacobian matrix of the target joint pose group and a negative learning multiplier. By superimposing it with the traditional loss function of the multilayer perceptron to increase the stability and subsequent adjustability of the target joint pose group output by the multilayer perceptron, the traditional loss function is defined as the mean square error between the coordinates obtained by substituting the target base coordinates and the target joint pose group output by the network into the established tool changing robot kinematic model.

[0052] Furthermore, the dual detection strategy involved in the path planning algorithm includes the following specific steps:

[0053] Starting from the nearest node, adjust the joints according to the joint order;

[0054] For adjusting joints, based on the search radius Determine the adjustment amount to adjust the joints and change the posture of the tool changing robot model accordingly;

[0055] Perform collision detection to determine whether there is an occupied grid in the cubic grid covered by the shield machine internal environment modeling in the current tool changing robot modeling posture;

[0056] If it exists, perform an exhaustive search to determine whether the adjustment amount is greater than or equal to the upper limit of the adjustment;

[0057] If it is greater than or equal to the upper limit of adjustment, a new round of iteration will be started. If it is less than the upper limit of adjustment, a search radius will be added to the current adjustment amount. And re-perform joint adjustment and collision detection;

[0058] If it does not exist, then execute the joint exhaustive judgment and judge Whether all joints have been adjusted;

[0059] If there are still unadjusted joints, the next joint is used as the adjustment joint according to the joint order, and joint adjustment and collision detection are continued;

[0060] like All joints have been adjusted, and the Euclidean distance between the adjusted nearest node and the target joint pose group is determined to be less than the pose error threshold;

[0061] If it is greater than or equal to the pose error threshold, The joint that satisfies the adjustment amount less than the adjustment upper limit and has the first order is used as the adjustment joint, and the search radius is superimposed on the current adjustment amount of the adjustment joint. To continue joint adjustment and collision detection;

[0062] If it is less than the pose error threshold, the target joint pose group is added to the random tree as a child node of the nearest node, and the edge between the nodes is generated based on the Euclidean distance between the target joint pose group and the nearest node, starting a new round of iteration.

[0063] Preferably, as an improvement, an adaptive joint sorting strategy is introduced into the dual detection strategy, for the first joints, extract the first The Jacobian vector of the column and calculate the The sum of the absolute values ​​of the elements of the Jacobian vector of the column The sum of the absolute values ​​of the elements of the Jacobian vector of the column is added to a small positive number and the reciprocal is taken to generate the The priority of each joint reflects the The posture change of each joint affects the position coordinates of the end effector. The improved dual detection strategy determines the adjusted joints in descending order of priority.

[0064] Specifically, the drive execution module receives the joint motion plan and adjusts it in sequence according to the node order. For each node, the joint posture of a single joint is obtained in sequence according to the joint order and the joint posture adjustment amount is calculated. The driving power is generated through the PID control algorithm to adjust the single joint to the corresponding joint posture until the path planning task corresponding to the joint motion plan is completed. It is further judged whether the joint motion plan is joint motion plan six. If it is joint motion plan six, no operation is required. If it is not joint motion plan six, the rear structured light camera or the front structured light camera is started according to whether the operating device is a wrench or a clamp to obtain visual images and depth images and send them to the analysis and feedback module.

[0065] Specifically, the drive execution module obtains the joint fine-tuning scheme and obtains the joint posture adjustment amount of a single joint in sequence according to the joint order, and generates the driving power through the PID control algorithm to adjust the single joint. After each joint is adjusted, the operation is performed according to the preset operation procedure to complete the corresponding operation task.

[0066] Furthermore, the analysis and feedback module processes the visual image and depth image through the target feature pose analysis method and generates a joint fine-tuning solution by reverse backtracking, including the following specific steps:

[0067] A Gaussian filter is used to smooth the visual image to generate a filtered visual image;

[0068] The target detection network is used to identify the target in the filtered visual image and generate the target outline. The target center pixel coordinates of the target axis and the target center point are determined based on the target outline. , obtain the target center depth of the target center point based on the depth image ;

[0069] Calculate the target center axis and two-dimensional pixel coordinate system in The imaging plane angle of the axis is used as the target coordinate system of Axis and joint coordinate systems of The angle of deviation between the axes;

[0070] Get the center pixel coordinates of the image and the focal length of the structured light camera , combined with the target center depth , target center pixel coordinates The pixel coordinates of the center of the image of Axis deviation and The axis deviation is back-projected to obtain the camera coordinates of the target center ;

[0071] Calculate the joint coordinate system based on whether the light structure camera is a front light structure camera or a rear light structure camera To the front camera coordinate system Or rear camera coordinate system The single-step transformation matrix transforms the target center camera coordinates Multiply by the front camera coordinate system The inverse matrix of the single-step transformation matrix, or multiplied by the rear camera coordinate system The inverse matrix of the single-step transformation matrix and the transformation matrix The inverse matrix of the target center is restored to obtain the base coordinates ;

[0072] Calculate the target center base coordinates separately The base coordinates of the endpoint of the corresponding path planning task are in the base coordinate system Next Axis offset, Axis offset and Axis offset;

[0073] Generate all zero-joint fine-tuning solutions, starting from Start with each joint and select the joints that can achieve the deviation angle, Axis offset, Axis offset and The joint whose axis offset is adjusted is used as the execution joint, and the adjustment amount corresponding to the execution joint is filled in the corresponding position in the all-zero joint fine-tuning scheme to generate the joint fine-tuning scheme.

[0074] Compared with the existing technology, the present invention pre-establishes the tool changing robot modeling and the shield machine internal environment modeling, introduces the division and marking of the cube grid into the path planning algorithm, and based on the dual detection strategy, controls the tool changing robot to perform collision-free movement in the shield machine internal environment modeling and constructs a random tree through repeated iterative random search. The optimal joint motion scheme is selected in combination with factors such as the total side length of the path and the path planning constraints. After the driving execution module completes the path planning task, visual images and depth images are captured, and the target feature posture analysis method is used to obtain the target central axis and target center pixel coordinates. The deviation angle is calculated based on the target central axis, and the target center base coordinates are inversely generated and the offset is calculated through back-projection transformation and the transformation matrix of the camera coordinate system and the base coordinate system. The joint fine-tuning scheme is further generated by reverse backtracking, thereby realizing collision-free movement of the tool changing robot and precise control based on feedback adjustment. BRIEF DESCRIPTION OF THE DRAWINGS

[0075] Figure 1 Schematic diagram of the structure of a hydraulically driven nine-degree-of-freedom tool-changing robot applicable to the present invention;

[0076] Figure 2 This is a schematic diagram of a modular mobile control system for a tool changing robot;

[0077] Figure 3 A schematic diagram of the joint coordinate system in the present invention;

[0078] Figure 4 This is a side view of the joint coordinate system when the DH method of the present invention is applied to a hydraulically driven nine-degree-of-freedom tool-changing robot;

[0079] Figure 5 Schematic diagram of connecting rod parameter definition in the present invention.

[0080] Figure numerals: 1. robot base; 2. translation structure; 3. lifting structure; 4. fine-tuning structure; 5. bidirectional rotation structure; 6. wrist structure; 7. actuator. DETAILED DESCRIPTION

[0081] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0082] Example 1:

[0083] As an example, the modular mobile control system of a tool changing robot provided by the present invention can be used in Figure 1The hydraulically driven nine-degree-of-freedom tool-changing robot shown in the figure is applied, and the hydraulically driven nine-degree-of-freedom tool-changing robot includes a robot base 1, a body and an actuator 7;

[0084] The robot base 1 is laid along the shield machine's cabin entrance toward the cutterhead;

[0085] The fuselage includes a translation structure 2, a lifting structure 3, a fine-tuning structure 4, a bidirectional rotation structure 5 and a wrist structure 6. The translation structure 2 is driven by a hydraulic motor to perform translational movement of the fuselage on the robot base 1. The lifting structure 3 is driven by a lifting cylinder to perform pitching movement to control the overall height of the fuselage. The fine-tuning structure 4 realizes the horizontal left and right movement of the rear-end manipulator arm in the tool-changing plane through the fine-tuning slide. The bidirectional rotation structure 5 can control the rear-end manipulator arm to rotate in the tool-changing plane and in a plane perpendicular to the tool-changing plane respectively. The wrist structure 6 can control the movement of the actuator 7 and adjust the angle of the actuator 7 through the telescopic arm and the rotation joint.

[0086] The actuator 7 includes a clamping claw and a front camera installed on the front plane of the actuator 7 and a wrench and a rear camera installed on the back plane of the actuator 7. The wrench and the clamping claw are used to remove bolts and grab tools respectively.

[0087] like Figure 2 As shown, a specific embodiment of the present invention discloses a modular mobile control system for a tool changing robot, including a global formulation module, a simulation planning module, a drive execution module and an analysis feedback module;

[0088] The global formulation module establishes the kinematic model of the tool-changing robot through the DH method (Denavit-Hartenberg), receives different task signals and determines the end base coordinates, takes the current joint posture group of the tool-changing robot as the starting joint posture group, combines the starting joint posture group and the end base coordinates to construct the path planning task corresponding to the task signal. The purpose of the path planning task is to make the operating device reach the end base coordinates and the operating device can be aligned with the operating plane. The operating device is the clamp or wrench in the actuator. The operating plane includes the tool-changing plane and the old tool recovery plane. The joint posture group records the tool-changing robot The joint posture of each joint actually indicates the rotation angle of the joint around a certain direction or the offset distance along a certain direction. is the total number of joints;

[0089] The simulation planning module obtains the path planning task and uses the path planning algorithm to simulate and adjust the joint pose group of the tool changing robot model in the shield machine internal environment model, ensuring that the path planning task is completed and the corresponding joint motion plan is generated without touching the internal obstacles of the shield machine.

[0090] The driving execution module sequentially adjusts the joint posture group of the tool changing robot according to the joint motion plan to complete the path planning task, and autonomously decides whether to enable the structured light camera to capture visual images and depth images based on the joint motion plan and sends them to the analysis and feedback module, obtains the joint fine-tuning plan and further fine-tunes the joint posture group of the tool changing robot to complete the operation task corresponding to the task signal, wherein the structured light camera includes a front structured light camera installed on the front plane of the actuator and a rear structured light camera installed on the back plane of the actuator;

[0091] The analysis feedback module obtains visual images and depth images, and calculates the target coordinate system in the visual image through target feature pose analysis Relative to the Joint coordinate system of each joint The rotation angle and offset are calculated and the joint fine-tuning scheme is generated through reverse backtracking and fed back to the drive execution module.

[0092] Furthermore, the global formulation module establishes the tool-changing robot kinematic model through the DH method, which includes the following specific steps:

[0093] Count the total number of tool changing robot joints ,The joint is used to perform rotation or movement in a certain direction to drive the tool changing robot to rotate or move as a whole or part of it. The joint includes rotating joint and moving joint;

[0094] The starting point of the tool changing robot's moving guide rail is used as the base coordinate system The origin of the tool change plane is perpendicular to the tool change plane and points to the direction of the tool change plane as the base coordinate system of Axis direction, with the direction perpendicular to the tool changing robot's moving plane and vertically upward as the base coordinate system of Axis direction, and through the right-hand screw rule, vertically set up the thumb and index finger of the right hand and represent the base coordinate system respectively of Axis direction and Axis direction, the direction of the natural bend of the right middle finger is used as the base coordinate system of Axis direction;

[0095] Based on the The joint coordinate system is determined by the execution function of each joint of Axis direction, based on joints Axis and joints The common perpendicular line of the axis determines the joint coordinate system The origin and Axis direction, and determine the joint coordinate system by the right-hand screw rule of Axis direction to construct the Joint coordinate system of each joint , you can get the first joint to the The joint coordinate system of each joint;

[0096] The center of the front plane of the tool changing robot actuator is used as the first Joint coordinate system of each joint The origin of the joint coordinate system is perpendicular to the front plane and points to the gripper. of Axis direction, in joint coordinate system of The direction of the axis is used as the joint coordinate system of Axis direction, and determine the joint coordinate system by the right-hand screw rule of Axis direction to construct the Joint coordinate system of each joint ;

[0097] Based on the Joint coordinate system of each joint With the Joint coordinate system of each joint The relationship between the common perpendicular lines and the rotation and offset between the corresponding coordinate axes are defined in the first The connecting rod parameters of the connecting rod, among which the A connecting rod is used to connect the joints and joints, The connection parameters of the first link depend on the actual tool changing robot. joints and The connecting structure of the joints;

[0098] Derived from the basis coordinate system by Jacobian matrix To joint coordinate system The transformation matrix , the transformation matrix Describes the base coordinate system The base coordinates of any point and joint coordinate system The corresponding Joint coordinates The transformation relationship is The mathematical representation of the joint pose of each joint.

[0099] like Figure 3As shown, further, construct the Joint coordinate system of each joint The specific steps include:

[0100] Determine the The execution function of a joint. If the execution function is rotation, the joint is a revolute joint, and the joint coordinate system of Axis direction is The joint axis direction of each joint. If the execution function is linear motion, the joint is a moving joint, then the joint coordinate system of Axis direction is The linear motion direction of each joint, , is the total number of joints;

[0101] The first joints Axis and joints The common perpendicular line of the axis is The perpendicular foot on the axis is used as the joint coordinate system The origin of

[0102] will be parallel to Axis and The common perpendicular of the axis and Axis direction The direction of the axis as the joint coordinate system of Axis direction;

[0103] Determine the joint coordinate system using the right-hand screw rule of Axis direction, Joint coordinate system of each joint Build completed.

[0104] As an example, Figure 4 This is a side view of the joint coordinate system established by the DH method for the hydraulically driven nine-degree-of-freedom tool-changing robot. The hydraulically driven nine-degree-of-freedom tool-changing robot includes five rotating joints and four moving joints, a total of nine joints, wherein: 、 and Respectively Joint coordinate system of each joint The three coordinate axes, ,exist Figure 4 In the example, The directions of the coordinate axes in each joint coordinate system that are not marked in the figure are determined by themselves according to the construction method of the joint coordinate system.

[0105] like Figure 5 As shown, further, The connecting rod parameters include the connecting rod length , connecting rod angle , connecting rod offset and joint angles ;

[0106] Connecting rod length Defined as joints Axis to joints The distance between the common perpendiculars of the axes;

[0107] Connecting rod angle Defined as joints Shaft winding The shaft rotates to joints The rotation angle when the axes are parallel, where the direction of rotation is determined by the right-hand screw rule as the four-finger winding direction;

[0108] Connecting rod offset Defined as joint coordinate system From the origin to the joints Axis and joints The distance from the common perpendicular to the axis;

[0109] joint angle Defined as joints Axis around joints The shaft rotates to The rotation angle when the axes are parallel, where the direction of rotation is determined by the right-hand screw rule as the four-finger winding direction.

[0110] Furthermore, the transformation matrix is ​​calculated through the Jacobian matrix The specific steps include:

[0111] Decomposing the transformation matrix , from the base coordinate system To joint coordinate system The transformation process is decomposed into Single-step transformation, each single-step transformation is to transform the base coordinate system Or the joint coordinate system of the current joint is transformed to the joint coordinate system of the next joint;

[0112] Further decompose the single-step transformation process into 4 sub-steps. Joint coordinate system of each joint , the joint coordinate system Coordinate system around joint of Shaft rotation is equivalent to connecting rod rotation angle The angle of the joint coordinate system of Axis and joint coordinate systems of Axis parallel, get One transformation of the joint coordinate system , the transformation matrix of this sub-step is defined as ;

[0113] Transform the joint coordinate system once Transform the joint coordinate system once of Axial translation link length , so that the joint coordinate system is transformed once of Axis and joint coordinate systems of Axis coincidence, get the Secondary transformation of joint coordinate system , the transformation matrix of this sub-step is defined as ;

[0114] Transform the joint coordinate system twice Secondary transformation of joint coordinate system of Axis rotation is equivalent to joint angle Angle, so that the secondary transformation joint coordinate system of Axis and joint coordinate systems of Axis parallel, obtain the three-dimensional transformation joint coordinate system , the transformation matrix of this sub-step is defined as ;

[0115] Transform the joint coordinate system three times Transform the joint coordinate system along the cubic of Axial translation link offset , so that the joint coordinate system is transformed three times and joint coordinate system Completely coincident, the transformation matrix of this sub-step is defined as ;

[0116] Multiply the transformation matrices of the four sub-steps in the single-step transformation process to obtain the Joint coordinate system of each joint To joint coordinate system The single-step transformation matrix , the specific formula is as follows:

[0117] ,

[0118] ,

[0119] ,

[0120] ,

[0121] in, and From the joint coordinate system To joint coordinate system The single-step rotation transformation matrix and single-step translation transformation matrix, represents the transpose of a matrix or vector, and denote the cosine and sine functions respectively, is an all-zero vector;

[0122] Further from the base coordinate system To joint coordinate system The transformation matrix can be obtained by multiplying the single-step transformation matrix of each step .

[0123] Furthermore, the task signals include bolt removal signal, old tool removal signal, old tool recovery signal, new tool installation signal, bolt installation signal and task end signal. The global formulation module receives different task signals and constructs corresponding path planning tasks including:

[0124] Receive the bolt removal signal and extract the tool change number. Since the tool box is fixed in the shield machine cutter head and arranged regularly, the tool box base coordinates can be determined according to the tool change number. , determine that the operating device is a wrench, and change the tool box base coordinates of Axis value Subtract wrench size and safe distance , generate the first end point base coordinates , and the default joint pose group of the tool changing robot to construct the path planning task 1, wherein the default joint pose group refers to the default joint pose group when the tool changing robot is started each time. The purpose of the path planning task 1 is to adjust the wrench of the tool changing robot to the vicinity of the tool box and the wrench is just facing the tool changing plane, with a safe distance It needs to be predetermined based on the internal space size of the shield machine and the size of the tool-changing robot to ensure that the tool-changing robot can be fine-tuned;

[0125] Receive the old tool removal signal, switch the operating device to the clamp, and obtain the joint posture group 1 and the tool box base coordinates , the tool box base coordinates of Axis value Subtract jaw size and safe distance , generate the second end point base coordinates , combined second endpoint base coordinates And joint posture group 1 constructs path planning task 2, among which joint posture group 1 refers to the tool changing robot adjusting according to joint motion plan 1 and joint fine-tuning plan 1 The joint posture group after the joints, namely joint motion plan 1 and joint fine-tuning plan 1, are used to complete path planning task 1. The purpose of path planning task 2 is to adjust the tool changing robot's gripper to the vicinity of the tool box and the gripper is just facing the tool changing plane;

[0126] Receive the old tool recovery signal, the operating device is still determined to be the gripper, obtain the joint posture group 2 and the old tool recovery box base coordinates , the old knife recycling box base coordinates of Axis value Subtract jaw size and safe distance , generate the third end point base coordinates , combined third endpoint base coordinates And joint posture group 2 constructs path planning task 3, among which the joint posture group 2 refers to the tool changing robot adjusted according to joint motion scheme 2 and joint fine-tuning scheme 2 The joint posture group after the joints, namely joint motion plan 2 and joint fine-tuning plan 2, are used to complete path planning task 2. The purpose of path planning task 3 is to adjust the tool changing robot's gripper to the old tool recycling box and make the gripper just face the plane where the old tool recycling box is located;

[0127] Receive the new tool installation signal, the operating device is still determined to be the clamp, and obtain the joint posture group three and the second end point base coordinates , the base coordinates of the second endpoint Directly used as the fourth end point base coordinate , combined fourth endpoint base coordinates And joint posture group three to construct path planning task four, among which the three-finger tool changing robot of the joint posture group is adjusted according to joint motion scheme three and joint fine-tuning scheme three The joint posture group after the first joint, namely joint motion plan three and joint fine-tuning plan three, is used to complete path planning task three. The purpose of path planning task four is to readjust the tool changing robot's gripper back to the vicinity of the tool box and make sure the gripper is facing the tool changing plane.

[0128] Receive the bolt installation signal, switch the operating device to the wrench, and obtain the joint pose group 4 and the first end point base coordinates , the first end point base coordinate Directly used as the fifth endpoint base coordinate , combined fifth endpoint base coordinates And joint posture group 4 constructs path planning task 5, among which the four-finger tool changing robot of joint posture group is adjusted according to joint motion scheme 4 and joint fine-tuning scheme 4 The joint posture group after the first joint, namely joint motion plan 4 and joint fine-tuning plan 4, is used to complete path planning task 4. The purpose of path planning task 5 is to adjust the wrench of the tool changing robot back to the vicinity of the tool box and make sure the wrench is facing the tool changing plane.

[0129] After receiving the task completion signal, the operating device is still determined to be a wrench. The joint posture group 5 and the default joint posture group are obtained. According to the default joint posture group and the size of each joint of the tool changer robot, the default base coordinates of the wrench before the tool changer robot is started are determined and used as the sixth end point base coordinates. , combined sixth endpoint base coordinates And joint posture group five to construct path planning task six, among which the five-finger tool changing robot of the joint posture group is adjusted according to joint motion scheme five and joint fine-tuning scheme five The joint posture group after each joint, namely joint motion plan five and joint fine-tuning plan five, are used to complete path planning task five. The purpose of path planning task six is ​​to move the tool changing robot back to the starting position and adjust it to the default posture. Since path planning task six only needs to return to the default posture and no fine-tuning is required, only the corresponding joint motion plan six will be generated in the subsequent path planning algorithm.

[0130] Specifically, the shield machine internal environment modeling and tool change robot modeling can be pre-established by combining the existing scanning technology with the existing 3D reconstruction technology, and based on the base coordinate system Placed in the simulation software, it can be used to pre-simulate and plan the movement path of the tool changing robot inside the shield machine. The internal environment model of the shield machine is divided into cubic grids, and the grid status is marked for each cubic grid. If there is an obstacle in the cubic grid, the grid status is marked as occupied. If there is no obstacle in the cubic grid, the grid status is marked as idle.

[0131] Furthermore, the simulation planning module obtains the path planning task and uses the path planning algorithm to generate the corresponding joint motion plan, including the following specific steps:

[0132] Get the path planning task and the corresponding starting joint pose group and end point base coordinates. The path planning tasks include path planning task one, path planning task two, path planning task three, path planning task four, path planning task five and path planning task six. The starting joint pose group includes the default joint pose group, joint pose group one, joint pose group two, joint pose group three, joint pose group four and joint pose group five. The end point base coordinates include the first end point base coordinates. , the second end point base coordinates , the third end point base coordinates , the fourth end point base coordinates , fifth endpoint base coordinates and the sixth endpoint base coordinates .

[0133] Create an empty random tree to store the tool change robots found during the search process. The joint pose groups of each joint are recorded as nodes in the random tree. Each node in the random tree uniquely corresponds to the tool changing robot. A specific joint pose group of each joint, and the edge between nodes is the Euclidean distance between two nodes;

[0134] The starting joint pose group in the path planning task is set as the root node of the random tree. The root node covers the angles of all rotating joints and the displacements of all moving joints in the starting joint pose group. Define the Joint coordinate system of each joint of Axes about the base coordinate system of The cosine of the angle between the axes is ,in, for Axis About The angle between the axes;

[0135] Set the maximum number of iterations to prevent the path planning algorithm from falling into an infinite loop. In this embodiment, the maximum number of iterations is set to 1000;

[0136] Set the search radius for joints , for revolute joints, the search radius is the maximum rotation angle between a node and its corresponding child node in the random tree. For mobile joints, the search radius is the maximum moving distance between a node and its corresponding child node in the random tree;

[0137] Set the path planning constraints. When the operating device of the tool change robot reaches the end base coordinate, it is expected that the operating device is a wrench or a clamp according to the path planning task. If the operating device is a wrench, the back plane of the actuator is expected to be opposite to the tool change plane. At this time, the joint coordinate system is required to be of Axis and base coordinate system of Cosine of the angle between the axes is -1, that is Axis and The axis direction is opposite. If the operating device is a clamp, it is expected that the front plane of the actuator is opposite to the tool change plane. In this case, the joint coordinate system is required to be of Axis and base coordinate system of Cosine of the angle between the axes is 1, that is Axis and The axis directions are the same;

[0138] In each round of the path planning algorithm, the target base coordinates of this iteration are randomly generated in the internal modeling of the shield machine. With the help of the established kinematic model of the tool change robot, the inverse kinematics algorithm is used to solve the target joint pose group corresponding to the target base coordinates. The target joint pose group records the position of the tool change robot model when the operating device reaches the target base coordinates. The joint posture of each joint, in this embodiment, the inverse kinematics algorithm adopts the Newton-Raphson numerical iteration method, which is a prior art and will not be elaborated on in detail;

[0139] Search the random tree for the node with the smallest Euclidean distance to the target joint pose group as the nearest node. Use the double detection strategy to determine whether the nearest node can be adjusted to the target joint pose group. If it cannot be adjusted to the target joint pose group, a new round of iteration is started. If it can be adjusted to the target joint pose group, the target joint pose group is added to the random tree as a child node of the nearest node and the corresponding edge is established, and a new round of iteration is started.

[0140] Until the number of iterations reaches the maximum number of iterations, traverse all nodes of the random tree and determine whether each node satisfies both the first condition and the path planning constraint condition. The first condition is that the Euclidean distance between the target base coordinate and the end point base coordinate corresponding to the node is less than the distance error threshold.

[0141] Get all nodes that meet the first condition and path planning constraints, trace back to the root node according to the parent-child relationship of each node to generate the path corresponding to the node, and sum the total side length of each path, select the path with the smallest total side length and arrange all nodes on the path forward according to the parent-child relationship to generate joint motion schemes. The joint motion schemes include joint motion scheme 1, joint motion scheme 2, joint motion scheme 3, joint motion scheme 4, joint motion scheme 5 and joint motion scheme 6. Among them, the joint motion scheme Corresponding to the path planning task , .

[0142] Furthermore, the dual detection strategy involved in the path planning algorithm includes the following specific steps:

[0143] Starting from the nearest node, adjust the joints according to the joint order;

[0144] For adjusting joints, based on the search radius Determine the adjustment amount of the joint to perform the joint adjustment, and correspondingly change the posture of the tool changing robot modeled in the simulation software;

[0145] Perform collision detection to determine whether there is a cube grid with an occupied grid state in the grid covered by the shield machine internal environment modeling in the posture of the current tool changing robot modeling;

[0146] If there is a cubic grid with an occupied grid state, it indicates that the tool changing robot in the current posture will collide with the internal obstacle of the shield machine. Further exhaustive search judgment is performed to determine whether the adjustment amount is greater than or equal to the adjustment upper limit value, where the adjustment upper limit value is determined according to the specific structure of the adjustment joint;

[0147] If the adjustment amount is greater than or equal to the upper limit of the adjustment, it means that the nearest node cannot be adjusted to the target joint pose group without colliding with obstacles inside the shield machine, and a new round of iteration is started;

[0148] If the adjustment amount is less than the upper limit, the search radius is added to the current adjustment amount. And re-perform joint adjustment and collision detection;

[0149] If there is no cube grid with grid status of occupied, perform joint exhaustion judgment to determine the tool changing robot Whether all joints have been adjusted;

[0150] If the tool changing robot If there are still unadjusted joints among the joints, the next joint will be used as the adjustment joint according to the joint order, and joint adjustment and collision detection will continue;

[0151] If the tool changing robot All joints have been adjusted, and the Euclidean distance between the adjusted nearest node and the target joint pose group is determined to be less than the pose error threshold;

[0152] If it is greater than or equal to the pose error threshold, it means that the nearest node has not been adjusted to the target joint pose group. The joint that satisfies the adjustment amount less than the adjustment upper limit and has the first order is used as the adjustment joint, and the search radius is superimposed on the current adjustment amount of the adjustment joint. To continue joint adjustment and collision detection;

[0153] If it is less than the pose error threshold, it indicates that the nearest node has been adjusted to the target joint pose group. The target joint pose group is added to the random tree as a child node of the nearest node. The edge between the nodes is generated based on the Euclidean distance between the target joint pose group and the nearest node, and a new round of iteration is started.

[0154] Specifically, after the drive execution module receives the joint motion plan, since the joint motion plan includes multiple nodes, each node uniquely corresponds to a specific joint posture group of a tool changing robot, and the tool changing robot is adjusted in sequence according to the node order in the joint motion plan, wherein, for each node in the joint motion plan, the joint posture of a single joint is obtained in sequence according to the joint order, and the difference with the current joint posture of the single joint is calculated to obtain the joint posture adjustment amount, and the driving power of the driver is generated by combining the joint posture adjustment amount through the PID control algorithm, and the driver is operated with the driving power to adjust the single joint to the corresponding joint posture, and the path planning task corresponding to the joint motion plan is completed step by step, and it is further judged whether the joint motion plan is joint motion plan six. If it is joint motion plan six, there is no need to start the front camera or the rear camera. If it is joint motion plan one to joint motion plan five, the rear structured light camera or the front structured light camera is started according to whether the operating device is a wrench or a clamp to obtain visual images and depth images and send them to the analysis and feedback module.

[0155] Specifically, the drive execution module obtains the joint fine-tuning plan. Since the joint fine-tuning plan is a single adjustment after the corresponding joint motion plan is adjusted, that is, there is only one joint posture group in the joint fine-tuning plan, the joint posture adjustment amount of a single joint in the joint fine-tuning plan is obtained in sequence according to the joint sequence, and the driving power of the driver is generated by combining the joint posture adjustment amount through the PID control algorithm. The driver is operated with the driving power to adjust a single joint. When the tool changing robot After each joint is adjusted, it indicates that the wrench or clamp of the tool changing robot has been successfully aligned with the target, and the operation is performed according to the corresponding preset operation procedure to complete the corresponding operation task. Among them, the targets include bolts, old tools, old tool recovery ports, new tool installation ports and bolt installation ports. The preset operation procedures are used to perform bolt removal, old tool removal, old tool recovery, new tool installation and bolt installation respectively.

[0156] Furthermore, the analysis and feedback module processes the visual image and depth image through the target feature pose analysis method and generates a joint fine-tuning solution by reverse backtracking, including the following specific steps:

[0157] A Gaussian filter is used to smooth the visual image to reduce noise interference and generate a filtered visual image;

[0158] The target detection network is used to identify the target in the filtered visual image and generate the target outline. The target center pixel coordinates of the target axis and the target center point are determined based on the target outline. , and The target center point is in the two-dimensional pixel coordinate system in Axis value and Axis value, further obtain the target center depth of the target center point in the depth image ,In this embodiment, the target detection network adopts the existing Faster-RCNN network;

[0159] Calculate the target center axis and two-dimensional pixel coordinate system in The imaging plane angle of the axis is used as the target coordinate system of Axis and joint coordinate systems of Deviation angle between axes, target coordinate system of Axis and joint coordinate systems of The axes are in the same direction. Since the target is located on the tool change plane or the plane where the old tool recycling box is located, it is exactly parallel to the imaging plane of the actuator's optical structure camera, and the camera coordinate system of Axis and joint coordinate systems of The axes remain in the same direction and parallel, so the target axis and the two-dimensional pixel coordinate system in The angle between the axes is always equal to the target coordinate system of Axis and joint coordinate systems of The deviation angle between the axes, that is, the joint coordinate system of Shaft winding After the axis rotates the deviation angle, it can be aligned with the target coordinate system of The axes are co-directional and parallel;

[0160] Get the center pixel coordinates of the image and the focal length of the structured light camera The image center refers to the projection point of the optical center of the structured light camera in the visual image, combined with the target center depth , target center pixel coordinates The pixel coordinates of the center of the image of Axis deviation and The axis deviation is back-projected to obtain the camera coordinates of the target center , 、 and The target center point is in the camera coordinate system in Axis value, Axis value and Axis value, the specific formula for back projection transformation is as follows:

[0161] ,

[0162] ,

[0163] ,

[0164] Among them, the camera coordinate system Including the front camera coordinate system and the rear camera coordinate system ;

[0165] If it is the front camera coordinate system Since the front light structure camera is installed on the front plane of the actuator and the optical center of the front light structure camera coincides with the center of the front plane, when the front light structure camera is facing the tool change plane or the plane where the old tool recycling box is located, according to the Joint coordinate system of the joint Definition of joint coordinate system and the front camera coordinate system Coincident, therefore, the front camera coordinate system Can be directly regarded as the joint coordinate system , joint coordinate system To the front camera coordinate system The single-step transformation matrix As the unit matrix, the target center camera coordinates Directly multiply by the single-step transformation matrix The inverse matrix can be restored to obtain the target center base coordinates ;

[0166] If it is the rear camera coordinate system Since the rear light structure camera is installed on the back plane of the actuator and the optical center of the rear light structure camera coincides with the center of the back plane, when the rear light structure camera is facing the tool change plane or the plane where the old tool recycling box is located, according to the Joint coordinate system of the joint Definition of the rear camera coordinate system is the joint coordinate system along Negative axis direction is based on the thickness of the actuator Translate and move around the translated The axis is rotated 180 degrees to obtain the joint coordinate system. To the rear camera coordinate system The single-step transformation matrix The specific form is as follows:

[0167] ,

[0168] The target center camera coordinates Multiply by the single-step transformation matrix in sequence The inverse matrix and transformation matrix The inverse matrix can be restored to obtain the target center base coordinates ;

[0169] Calculate the target center base coordinates The base coordinates of the endpoint of the corresponding path planning task are calculated in the base coordinate system Axis offset, Axis offset and Axis offset;

[0170] Generate a set of all-zero joint fine-tuning solutions, starting from Start backtracking from each joint and select the joints that can be bypassed. Axis rotation joint, along Axis translation of the moving joint, along Axis translation of the moving joint and along The moving joint of the axis translation is used as the execution joint, and the deviation angle, Axis offset, Axis offset and The axis offsets are filled into the corresponding positions of the corresponding execution joints in the all-zero joint fine-tuning scheme to generate a joint fine-tuning scheme. The joint fine-tuning scheme corresponds one-to-one to the joint motion scheme, including joint fine-tuning scheme one, joint fine-tuning scheme two, joint fine-tuning scheme three, joint fine-tuning scheme four and joint fine-tuning scheme five.

[0171] Example 2:

[0172] This embodiment is improved on the basis of embodiment 1, and is basically the same as embodiment 1. The same modules will not be described in detail. The path planning algorithm disclosed in embodiment 1 solves the target joint pose group corresponding to the target base coordinates through the inverse kinematics algorithm, which is prone to multiple solutions. As an improvement, preferably, a multi-layer perceptron with a singular value penalty term and a bottleneck structure is used instead of the inverse kinematics algorithm to solve the target joint pose group corresponding to the target base coordinates, wherein the bottleneck structure is achieved by inserting a fully connected layer with half the number of neurons in the fully connected layers on both sides between two fully connected layers with the same number of neurons, and through dimensional compression and re-expansion, combining the fully connected layers The nonlinear activation function enhances the feature expression ability. The traditional loss function of the multilayer perceptron is defined as the mean square error between the coordinates obtained by substituting the target base coordinates and the target joint pose group output by the network into the established tool changing robot kinematic model. A singular value penalty term is superimposed on the traditional loss function. The singular value penalty term is equal to the product of the minimum singular value of the Jacobian matrix of the target joint pose group and a negative learning multiplier. Since the singular value of the Jacobian matrix is ​​too large, it indicates that the freedom or movement ability of the tool changing robot is not stable. Therefore, the introduction of the singular value penalty term increases the stability and subsequent adjustability of the target joint pose group output by the multilayer perceptron.

[0173] Example 3:

[0174] This embodiment is improved on the basis of embodiment 1 and 2, and is basically the same as embodiment 1 and 2. The same modules will not be described in detail. The path planning algorithm disclosed in embodiment 1 and 2 involves a double detection strategy, which requires starting from the nearest node and adjusting the joints according to the joint sequence. However, the tool changing robot The influence of each joint on the end effector is different. The traversal of the joints is adjusted so that the dual detection strategy in Examples 1 and 2 always maintains the highest computational complexity. As an improvement, an adaptive joint sorting strategy is introduced. joints, extract the first The Jacobian vector of the column and calculate the The sum of the absolute values ​​of the elements of the Jacobian vector of the column The sum of the absolute values ​​of the elements of the Jacobian vector of the column is added to a small positive number and the reciprocal is taken to generate the The priority of the joints is due to the The Jacobian vector of the column corresponds to the joints, reflecting the The posture change of each joint affects the position coordinates of the end effector. The improved dual detection strategy determines the adjusted joints in descending order of priority. At this time, since the joints with greater influence are adjusted first, the collision can be identified as early as possible through collision detection to start a new round of iteration, reducing the complexity of the dual detection strategy.

[0175] The present invention discloses a modular mobile control system for a tool changing robot, comprising a global formulation module, a simulation planning module, a drive execution module and an analysis feedback module; the global formulation module establishes a kinematic model of the tool changing robot by using the DH method, receives different task signals and determines the starting joint posture group and the end point base coordinates, and constructs a path planning task in combination; the simulation planning module obtains the path planning task, and uses a path planning algorithm based on a dual detection strategy to control the tool changing robot to perform collision-free movement in the internal environment modeling of a shield machine to generate a joint motion plan; the drive execution module adjusts the joint posture group in sequence according to the joint motion plan to complete the path planning task, and independently decides whether to capture visual images and depth images, obtains a joint fine-tuning plan and further fine-tunes the joint posture group to complete the corresponding operation task; the analysis feedback module obtains visual images and depth images, uses a target feature posture analysis method to calculate the deviation angle and offset, and generates a joint fine-tuning plan through reverse backtracking, thereby realizing high-precision control without collision of the tool changing robot.

[0176] The above description is merely a preferred embodiment of the present invention. The scope of protection of the present invention is not limited to the above embodiment. All technical solutions based on the concept of the present invention are within the scope of protection of the present invention. It should be noted that for those skilled in the art, various improvements and modifications that do not depart from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A modular mobile control system for a tool changing robot, characterized in that: It includes global formulation module, simulation planning module, drive execution module and analysis feedback module; The global formulation module constructs the joint coordinate system through the DH method and obtains the transformation matrix based on the Jacobian matrix; it receives the task signal, determines the starting joint pose group and the end point base coordinates, and constructs the path planning task; The simulation planning module obtains the path planning task and uses a path planning algorithm to establish obstacle constraints in a rasterized environment. Taking the starting joint pose group as the root node, it iteratively generates the target base coordinates and solves the corresponding joint pose group. It searches for the nearest node and uses a double detection strategy to determine whether to construct a random tree as a child node. It selects the path with the smallest total side length, satisfies the constraints, and reaches the end base coordinates to generate a joint motion plan. The driver execution module executes the joint motion plan, decides whether to capture visual images and depth images, and obtains and executes the joint fine-tuning plan; The analysis and feedback module obtains visual images and depth images, uses the target detection network to obtain the target central axis and target center pixel coordinates, calculates the deviation angle based on the target central axis, combines the target center depth and focal length back projection to generate the target center camera coordinates, and inversely generates the target center base coordinates based on the transformation matrix between the camera coordinate system and the base coordinate system and calculates the offset, and back-generates the joint fine-tuning plan.

2. A modular mobile control system for a tool changing robot according to claim 1, characterized in that: The simulation planning module also models the internal environment of the shield machine and the tool-changing robot through scanning technology and three-dimensional reconstruction technology, simulates and plans the movement path of the tool-changing robot, divides the internal environment model of the shield machine into cubic grids, and marks the grid status as occupied or idle based on whether there are obstacles in the cubic grid to assist in the dual detection strategy judgment.

3. The modular mobile control system for a tool changing robot according to claim 1, characterized in that: The simulation planning module uses the path planning algorithm to generate the corresponding joint motion plan, which includes the following steps: Get the starting joint pose group and the end point base coordinates, and create a random tree with the starting joint pose group as the root node; Set the maximum number of iterations, search radius, and path planning constraints to limit the joint coordinate system according to the operating device of Axis and base coordinate system of The cosine of the angle between the axes; In each iteration, the target base coordinates are randomly generated and the corresponding target joint pose group is solved using the inverse kinematics algorithm; Search for the nearest node in the random tree, and use the dual detection strategy to determine whether the Euclidean distance between the adjusted nearest node and the target joint pose group is less than the pose error threshold. Then decide whether to add the target joint pose group as a child node and start the next iteration. When the maximum number of iterations is reached, the path corresponding to each node that meets the first condition and path planning constraints is generated based on the parent-child relationship. The path with the smallest total side length is selected to generate the joint motion plan. The first condition is that the Euclidean distance between the target base coordinate and the end base coordinate of the node is less than the distance error threshold.

4. A modular mobile control system for a tool changing robot according to claim 3, characterized in that: The dual detection strategy includes the following steps: Starting from the nearest node, the adjustment joints are determined in order, and the adjustment amount is determined and executed based on the search radius; Perform collision detection to determine whether the current tool changing robot model is in collision; If there is a collision and the adjustment amount is greater than or equal to the upper limit of the adjustment, the next iteration is started; If there is a collision and the adjustment amount is less than the upper limit of the adjustment, the search radius is superimposed on the current adjustment amount and joint adjustment and collision detection are performed; If there is no collision and there are unadjusted joints, determine the next joint to be adjusted and perform joint adjustment and collision detection; If there is no collision, all joints are adjusted, and the Euclidean distance between the nearest node after adjustment and the target joint pose group is greater than or equal to the pose error threshold, the joint with the adjustment amount less than the adjustment upper limit and the first order is used as the adjustment joint, and the search radius is superimposed on the corresponding adjustment amount to perform joint adjustment and collision detection; In other cases, the target joint pose group is placed in the random tree as a child node of the nearest node and an edge is generated to start the next iteration.

5. The modular mobile control system for a tool changing robot according to claim 4, characterized in that: In the path planning algorithm, a multilayer perceptron that introduces a singular value penalty term and a bottleneck structure is used instead of the inverse kinematics algorithm to solve the target joint pose group corresponding to the target base coordinates, wherein the bottleneck structure is achieved by inserting a fully connected layer with only half the number of neurons in the fully connected layers on both sides between two fully connected layers with the same number of neurons, and superimposing a singular value penalty term on the traditional loss function of the multilayer perceptron. The traditional loss function is defined as the mean square error between the coordinates obtained by substituting the target base coordinates and the target joint pose group output by the network into the established tool changing robot kinematic model. The singular value penalty term is equal to the product of the minimum singular value of the Jacobian matrix of the target joint pose group and a negative learning multiplier.

6. A modular mobile control system for a tool changing robot according to any one of claims 3 to 5, characterized in that: Introducing the adaptive joint sorting strategy into the dual detection strategy, the tool changing robot joints, extract the first The Jacobian vector of the column and calculate the The sum of the absolute values ​​of the elements of the Jacobian vector of the column The sum of the absolute values ​​of the elements of the Jacobian vector of the column is added to a positive number and the reciprocal is taken to generate the The priority of each joint reflects the The posture change of each joint affects the position coordinates of the end effector. The improved dual detection strategy determines the adjusted joints in descending order of priority.

7. The modular mobile control system for a tool changing robot according to claim 1, characterized in that: The analysis and feedback module generates a joint fine-tuning plan, which includes the following specific steps: Perform Gaussian filtering on the visual image and generate the target outline through target detection network recognition, determine the target center pixel coordinates of the target axis and the target center point, and obtain the target center depth based on the depth image; Calculate the target axis and pixel coordinate system The imaging plane angle of the axis is used as the target coordinate system Axis and joint coordinate systems of The deviation angle of the axis; Get the image center pixel coordinates and focal length, based on the target center pixel coordinates and image center pixel coordinates Axis deviation and The axis deviation is back-projected and multiplied by the joint coordinate system The inverse matrix of the single-step transformation matrix to the front camera coordinate system or the rear camera coordinate system and the inverse matrix of the transformation matrix are used to obtain the target center base coordinates; Calculate the target center base coordinates and the corresponding path planning task end base coordinates in the base coordinate system Axis offset, Axis offset and Axis offset, from Start by selecting executable joints one by one and generate joint fine-tuning plans.

8. The modular mobile control system for a tool changing robot according to claim 1, characterized in that: The global formulation module constructs the joint coordinate system by the DH method, including the following steps: Count the total number of joints The starting point of the moving guide rail, the direction perpendicular to the tool change plane, and the direction perpendicular to the moving plane and upward are respectively used as the origin of the base coordinate system. Axis direction and Axis direction, determine the base coordinate system by the right-hand screw rule Axis direction; Based on the The executive function of each joint determines the The joint coordinate system of the joint Axis direction, based on Axis and joints The common perpendicular line of the axis determines the The origin of the joint coordinate system and The direction of the axis is determined by the right-hand screw rule. The joint coordinate system of the joint Axis direction, get the direction from the first joint to the The joint coordinate system of each joint; The center of the front plane of the actuator, the direction perpendicular to the front plane and pointing to the gripper, and the The joint coordinate system of the joint The directions of the axes are respectively The origin of the joint coordinate system of each joint, Axis direction and The direction of the axis is determined by the right-hand screw rule. The joint coordinate system of the joint Axis direction, construct the The joint coordinate system of each joint.

9. The modular mobile control system for a tool-changing robot according to claim 8, characterized in that: Build the The joint coordinate system of each joint includes the following specific steps: According to If the function of a joint is rotation or linear motion, then Axis direction is The direction of the joint axis or linear motion of each joint; The first joints Axis and joints The common perpendicular line of the axis is The foot of the perpendicular on the axis is the The origin of the joint coordinate system of each joint; will be parallel to Axis and The common perpendicular of the axis and Axis direction The direction of the axis is Axis direction; Determined by the right-hand screw rule Axis direction, The joint coordinate system of each joint is constructed.

10. The modular mobile control system for a tool changing robot according to claim 1, characterized in that: The global formulation module constructs the joint coordinate system by the DH method and further comprises the following steps: Based on the The joint coordinate system of the first joint is the same as that of the The relative relationship of the joint coordinate system of each joint is defined as The connecting rod parameters of the first connecting rod, The link parameters of a link include link length, link angle, link offset and joint angle, among which the link length is Axis to The distance from the common perpendicular line of the axis, the connecting rod angle is Shaft winding The shaft rotates to The rotation angle when the axis is parallel, the connecting rod offset is The origin of the joint coordinate system of each joint is Axis and The distance from the common perpendicular line of the axis, the joint angle is Shaft winding The shaft rotates to The rotation angle when the axes are parallel.

Images