Modular mobile control system for a tool changer robot

Abstract

The application discloses a modular mobile control system of a tool changing robot, relates to the technical field of automatic equipment, and comprises a global formulation module, a simulation planning module, a driving execution module and an analysis feedback module. The global formulation module establishes a kinematic model through a D-H method, receives different task signals, and determines a starting joint pose group and an end base coordinate to combine and construct a path planning task. The simulation planning module acquires the path planning task, controls the tool changing robot to move without collision in internal environment modeling of a shield machine to generate a joint motion scheme. The driving execution module sequentially adjusts the joint pose group according to the joint motion scheme to complete the path planning task, and autonomously decides whether to shoot a visual image and a depth image, acquires a joint fine adjustment scheme, and fine adjusts the joint pose group to complete a corresponding operation task. The analysis feedback module acquires the visual image and the depth image, calculates a deviation angle and an offset through a target feature pose analysis method, and generates the joint fine adjustment scheme through reverse tracing, so that high-precision control of the tool changing robot without collision is realized.

Description

Technical Field

[0001] This invention relates to the field of automation equipment technology, and in particular to a modular mobile control system for a tool-changing robot. Background Technology

[0002] In underground tunnel engineering, tunnel boring machines are widely used, but replacing worn-out cutters presents many difficulties. Traditional manual cutter replacement methods require entering the pressurized working area behind the cutterhead to disassemble the old cutter and install the new one. This process is dangerous, time-consuming, and inefficient. Furthermore, manual cutter replacement is prone to safety accidents. The emergence of cutter replacement robots has provided a solution to these problems. Cutter replacement robots can safely replace cutters in narrow and harsh working environments through the cooperation of robotic arms and actuators.

[0003] A 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, and providing control parameters for 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 a mutation vector; performing a cross operation between the mutation vector and the target vector to generate a test vector; comparing the test vector and the target vector, and selecting the better individual as the new target vector; determining whether a preset stopping criterion is met. If so, the process ends and the optimal individual is output as the joint change matrix of the robotic arm; otherwise, the mutation operation is returned. This method ensures the pose accuracy of the robotic arm while obtaining the shortest stroke solution, thus improving operational efficiency.

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

[0005] To address the shortcomings of existing technologies, this invention proposes a modular mobile control system for a tool-changing robot. This system provides path planning for the tool-changing robot that takes into account obstacles and provides effective feedback fine-tuning, ensuring that the tool-changing robot can complete tool changes accurately and efficiently.

[0006] The technical solution to achieve the purpose of this invention is as follows:

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

[0008] The global planning module establishes a kinematic model of the tool-changing robot using the DH method, receives different task signals and determines the endpoint 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 acquires the path planning task, uses a path planning algorithm based on a dual detection strategy to simulate and adjust the joint pose group, simulates and completes the path planning task, and generates the corresponding joint motion scheme.

[0010] The drive execution module adjusts the joint pose group sequentially according to the joint motion scheme to complete the path planning task. It autonomously decides whether to capture visual and depth images and send them to the analysis and feedback module based on the joint motion scheme. It then obtains the joint fine-tuning scheme and further fine-tunes the joint pose group to complete the corresponding operation task.

[0011] The analysis and feedback module acquires visual and depth images, and calculates the target coordinate system in the visual image using the target feature pose analysis method. Relative to joint coordinate system The rotation angle and offset are used to generate a joint fine-tuning scheme through reverse backtracking and then fed back to the drive execution module.

[0012] Furthermore, the global formulation module establishes the kinematic model of the tool-changing robot using the DH method, including the following specific steps:

[0013] Count the total number of joints of the tool-changing robot The starting point of the moving guide rail, the direction perpendicular to and pointing towards the tool changing plane, and the direction perpendicular to the moving plane and upward are respectively used as the base coordinate system. The origin, Axial direction and The base coordinate system is determined by the right-hand screw rule along the axis direction. of Axial direction;

[0014] Based on the The execution function of each joint determines the joint coordinate system. of Axial direction, based on Axis and the first Each joint The common perpendicular of the axes determines the joint coordinate system. The origin and The joint coordinate system is determined by the right-hand screw rule along the axis direction. of In the axial direction, obtain the first joint to the second joint. The joint coordinate system of each joint;

[0015] With the actuator front end plane center, perpendicular to the front end plane and pointing towards the gripper, and the joint coordinate system of The directions of the axes are respectively used as the first Joint coordinate system of each joint The origin, Axial direction and The joint coordinate system is determined by the right-hand screw rule along the axis direction. of axial direction, construct the first Joint coordinate system of each joint ;

[0016] Based on joint coordinate system With joint coordinate system The relationship between the common perpendiculars and the rotation and offset between the corresponding coordinate axes are defined as follows: Link parameters of each link;

[0017] Calculated from the base coordinate system using the Jacobian matrix To the joint coordinate system Transformation matrix .

[0018] Furthermore, constructing the first Joint coordinate system of each joint The specific steps include the following:

[0019] Determine the first The function of each joint is rotation; if the function is rotation, then the joint coordinate system... of The axial direction is the first If the function of a joint is linear motion, then the joint coordinate system... of The axial direction is the first The direction of linear motion of each joint , The total number of joints;

[0020] The first Each joint Axis and the first Each joint The common perpendicular of the axis is The foot of the perpendicular on the axis serves as the joint coordinate system. The origin;

[0021] Parallel to shaft and The common perpendicular of the axis and by Axis direction The direction of the axis is used as the joint coordinate system of Axial direction;

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

[0023] Furthermore, the first The link parameters of each link include the link length. Linkage angle Linkage offset and joint angle ;

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

[0025] Linkage angle for Axis winding Rotate the axis to the same The rotation angle when the axis is parallel;

[0026] Linkage offset Joint coordinate system From the origin to shaft and The distance between the common perpendiculars of the axes;

[0027] Joint angle for Axis winding Rotate the axis to the same The rotation angle when the axis is parallel.

[0028] Furthermore, the transformation matrix is ​​derived using the Jacobian matrix. The specific steps include the following:

[0029] base coordinate system To the joint coordinate system The transformation process is broken down into: Each single-step transformation involves changing the base coordinate system. Or transform the joint coordinate system of the current joint to the joint coordinate system of the next joint;

[0030] For joint coordinate system , joint coordinate system Around Axis rotation is equivalent to connecting rod rotation angle Angle, making shaft and Parallel axes, obtain the first transformation joint coordinate system Define the transformation matrix of this sub-step as: ;

[0031] Transform the joint coordinate system once along Length of axial translation link ,make shaft and Coincident axes, obtain the joint coordinate system of the second transformation. Define the transformation matrix of this sub-step as: ;

[0032] Secondary transformation of the joint coordinate system Around Axis rotation is equivalent to joint angle Angle, making shaft and Parallel axes, obtain the joint coordinate system of the three transformations. Define the transformation matrix of this sub-step as: ;

[0033] Transform the joint coordinate system three times along Axial translation link offset Make the joint coordinate system undergo three transformations. With joint coordinate system Overlapping, the transformation matrix of this sub-step is defined as follows: ;

[0034] The joint coordinate system is obtained by multiplying the transformation matrices of the four sub-steps in the single-step transformation process. To the joint coordinate system single-step transformation matrix Further, from the base coordinate system To the joint coordinate system The transformation matrix is ​​obtained by multiplying each single-step transformation matrix in the sequence. .

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

[0036] Receive bolt removal signal and extract tool change number to obtain tool box base coordinates The operating device is determined to be a wrench, and the tool box base coordinates are set. of Axis value Subtract the wrench size and safe distance Generate the first endpoint base coordinates , construct path planning task one with the default joint pose group;

[0037] Receive the old tool disassembly signal, determine that the operating device is a gripper, and acquire the joint pose group one and the tool box base coordinates. Set the toolbox base coordinates of Axis value Subtract the gripper size and safe distance Generate the second endpoint base coordinates And combine it with joint pose group one to construct path planning task two;

[0038] Receive the old knife retrieval signal, determine that the operating device is a gripper, and acquire the joint pose group two and the base coordinates of the old knife retrieval box. The base coordinates of the old knife recycling box of Axis value Subtract the gripper size and safe distance Generate the third endpoint base coordinates And combine it with joint pose group two to construct path planning task three;

[0039] Receive the new tool installation signal, determine that the operating device is a gripper, and acquire the joint pose group three and the second endpoint base coordinates. Set the second endpoint base coordinates Directly used as the fourth endpoint base coordinate And combine it with the joint pose group three to construct path planning task four;

[0040] Receive bolt installation signal, identify wrench as the operating device, and acquire joint pose group four and first endpoint base coordinates. Set the first endpoint base coordinates Directly used as the fifth endpoint base coordinate And combine it with the joint pose group four to construct path planning task five;

[0041] Upon receiving the task completion signal, the operating device is identified as a wrench. Joint pose group five and the default joint pose group are acquired. Based on the default joint pose group, the default base coordinates of the wrench are determined and used as the sixth endpoint base coordinates. Combined with joint pose group five, construct path planning task six.

[0042] Specifically, the internal environment modeling of the tunnel boring machine and the cutterhead robot modeling can be pre-established using existing scanning technology combined with existing 3D reconstruction technology, based on a base coordinate system. The simulation software is used to simulate the movement path of the tool-changing robot. The internal environment of the tunnel boring machine is modeled as 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 acquires the path planning task and uses a path planning algorithm to generate the corresponding joint motion scheme, including the following specific steps:

[0044] Obtain the path planning task and the corresponding starting joint pose group and ending 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 included angle of the axis;

[0046] Set the maximum number of iterations and the search radius of the joints. Based on the path planning constraints set by the operating device, if the operating device is a wrench, the following requirements apply: shaft and cosine value of the included angle of the axis The value is -1. If the operating device is a gripper, the requirement is... shaft and cosine value of the included angle of the axis =1;

[0047] In each iteration of the path planning algorithm, the target base coordinates for this iteration are randomly generated. Using the kinematic model of the tool-changing robot, 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 a dual 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 the child node of the nearest node into the random tree and establish the corresponding edge, and start 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 to determine whether it simultaneously satisfies the first condition and the path planning constraint. The first condition is that the Euclidean distance between the target base coordinates and the destination base coordinates of the node is less than the distance error threshold.

[0050] Obtain all nodes that satisfy the first condition and path planning constraints. Backtrack 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 in the forward order according to the parent-child relationship to generate the joint motion scheme.

[0051] Preferably, as an improvement, a multilayer 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 set corresponding to the target base coordinates, thereby solving the multi-solution problem in the inverse kinematics algorithm. The bottleneck structure is achieved by inserting a fully connected layer with half the number of neurons between two fully connected layers with the same number of neurons. The singular value penalty term is equal to the product of the minimum singular value of the Jacobian matrix of the target joint pose set and a negative learning multiplier. This is superimposed on the traditional loss function of the multilayer perceptron to increase the stability and subsequent adjustability of the target joint pose set output by the multilayer perceptron. The traditional loss function is defined as the mean square error between the target base coordinates and the target joint pose set output by the network and the coordinates obtained by substituting them into the established kinematic model of the tool-changing robot.

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

[0053] Starting from the nearest node, determine the joints to adjust based on the joint sequence;

[0054] For joint adjustments, 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 are any occupied grid cells in the cubic grid covered by the current pose model of the tool-changing robot in the model of the tunnel boring machine's internal environment.

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

[0057] If the value is greater than or equal to the adjustment limit, start a new round of iteration; if the value is less than the adjustment limit, add a search radius to the current adjustment amount. And re-adjust the joints and perform collision detection;

[0058] If it does not exist, then perform a joint exhaustive check to determine the joint. Have all joints been adjusted?

[0059] If there are still unadjusted joints, the next joint in the joint sequence will be used as the adjustment joint, and the joint adjustment and collision detection will continue.

[0060] like All joints have been adjusted. Determine whether the Euclidean distance between the nearest node after adjustment and the target joint pose group is less than the pose error threshold.

[0061] If it is greater than or equal to the pose error threshold, Among the joints, the joint that satisfies the condition that the adjustment amount is less than the upper limit of adjustment and is the first in the sequence is selected as the adjustment joint. The search radius is then added to the current adjustment amount of the adjustment joint. To continue joint adjustments and collision detection;

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

[0063] Preferably, as an improvement, an adaptive joint sequencing strategy is introduced into the dual detection strategy for the tool-changing robot. Extract the joint from the Jacobian matrix. The Jacobian vector of the column and the calculation of the first... The sum of the absolute values ​​of the elements of the Jacobian vectors in the column will be the first... The sum of the absolute values ​​of the elements of the Jacobian vector of a column, superimposed with a tiny positive number and then the reciprocal is taken to generate the first Jacobian vector. The priority of the first joint is to reflect the priority of the second joint. The pose changes of each joint affect the position coordinates of the end effector. The improved dual detection strategy determines the joints to be adjusted according to the priority from high to low.

[0064] Specifically, the drive execution module receives the joint motion scheme and adjusts it sequentially according to the node order. For each node, it acquires the joint pose of a single joint in sequence according to the joint order and calculates the joint pose adjustment amount. It generates drive power through a PID control algorithm to adjust the single joint to the corresponding joint pose until the path planning task corresponding to the joint motion scheme is completed. It further determines whether the joint motion scheme is joint motion scheme six. If it is joint motion scheme six, no operation is required. If it is not joint motion scheme six, the rear structured light camera or the front structured light camera is activated according to the operating device, which is a wrench or a gripper, to acquire visual and depth images and send them to the analysis feedback module.

[0065] Specifically, the drive execution module obtains the joint fine-tuning scheme and sequentially acquires the joint pose adjustment amount of each individual joint according to the joint sequence. It then generates drive power using a PID control algorithm to adjust the individual joint. After each joint is adjusted, the operation is performed according to the preset operating procedure to complete the corresponding operation task.

[0066] Furthermore, the analysis and feedback module processes the visual and depth images using target feature pose analysis and then backtracks to generate a joint fine-tuning scheme, including the following specific steps:

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

[0068] An object detection network is used to identify targets in a filtered visual image and generate target contours. Based on the target contours, the pixel coordinates of the target's center axis and center point are determined. The target center depth is obtained from the depth image. ;

[0069] Calculate the target centerline and the two-dimensional pixel coordinate system In The angle between the imaging plane and the axis, and used as the target coordinate system. of Axis and Joint Coordinate Systems of The deviation angle 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 Image center pixel coordinates of Shaft deviation and The axis deviation is back-projected to obtain the target center camera coordinates. ;

[0071] Determine the joint coordinate system based on whether the optical structure camera is a front-facing or rear-facing optical structure camera. To the front camera coordinate system or rear camera coordinate system The single-step transformation matrix will transform the target center camera coordinates. Multiply by the coordinate system of the front camera The inverse of the single-step transformation matrix, or by multiplying it sequentially by the coordinates of the rear camera. The inverse matrix and transformation matrix of the single-step transformation matrix The inverse matrix is ​​used to reconstruct and obtain the target center base coordinates. ;

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

[0073] Generate a zero-joint fine-tuning scheme, starting from the first... Starting with each joint, select the joints that can achieve the deviation angle in sequence. 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 into the corresponding position in the all-zero joint fine-tuning scheme to generate the joint fine-tuning scheme.

[0074] Compared with existing technologies, this invention pre-establishes models of the cutter-changing robot and the internal environment of the tunnel boring machine. It introduces cubic grid division and labeling into the path planning algorithm. Based on a dual detection strategy, it controls the cutter-changing robot to move without collision within the tunnel boring machine's internal environment model through iterative random search, constructing a random tree. The optimal joint motion scheme is selected by combining factors such as the total path length and path planning constraints. Furthermore, after the drive execution module completes the path planning task, visual and depth images are captured. The target's central axis and center pixel coordinates are obtained using target feature pose analysis. The deviation angle is calculated based on the target's central axis. The target's center base coordinates are generated inversely through back projection transformation and the transformation matrix between the camera coordinate system and the base coordinate system, and the offset is calculated. Further, a joint fine-tuning scheme is generated through reverse backtracking, achieving collision-free movement of the cutter-changing robot and precise control based on feedback adjustment. Attached Figure Description

[0075] Figure 1 This is a schematic diagram of a hydraulically driven nine-DOF tool-changing robot to which the present invention can be applied.

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

[0077] Figure 3 This is a schematic diagram of the joint coordinate system construction in this invention;

[0078] Figure 4 This is a side view of the joint coordinate system when the DH method is applied to a hydraulically driven nine-DOF tool-changing robot in this invention.

[0079] Figure 5 This is a schematic diagram illustrating the definition of link parameters in this invention.

[0080] Reference numerals: 1. Robot base; 2. Translation structure; 3. Lifting structure; 4. Fine-tuning structure; 5. Bidirectional rotation structure; 6. Wrist structure; 7. Actuator. Detailed Implementation

[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 for a tool-changing robot provided by this invention can be used in situations such as... Figure 1The hydraulically driven nine-DOF tool changer shown is used in the robot base 1, body and actuator 7.

[0084] Robot base 1 is laid along the entrance of the tunnel boring machine's cabin towards the cutterhead;

[0085] The robot body 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 make translational movements on the robot base 1. The lifting structure 3 is driven by a lifting cylinder to perform pitch movements and control the overall height of the robot body. The fine-tuning structure 4 uses a fine-tuning slide to enable the rear robotic arm to move horizontally left and right in the tool changing plane. The bidirectional rotation structure 5 can control the rear robotic arm to rotate in the tool changing plane and in a plane perpendicular to the tool changing plane. 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 rotating joint.

[0086] The actuator 7 includes a gripper and a front camera mounted on the front plane of the actuator 7, and a wrench and a rear camera mounted on the back plane of the actuator 7. The wrench and gripper are used to remove bolts and grasp tools, respectively.

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

[0088] The global planning module establishes a kinematic model of the tool-changing robot using the Denavit-Hartenberg (DH) method. It receives different task signals and determines the endpoint coordinates. The current joint poses of the tool-changing robot are used as the starting joint poses. The starting joint poses and the endpoint coordinates are combined to construct the path planning task corresponding to the task signal. The goal of the path planning task is to ensure that the manipulator reaches the endpoint coordinates and is directly facing the operating plane. The manipulator is a gripper or wrench in the actuator. The operating plane includes the tool-changing plane and the old tool retrieval plane. The joint poses record the kinematics of the tool-changing robot. The joint pose of a joint essentially indicates the angle of rotation of the joint about a certain direction or the distance of offset along a certain direction. The total number of joints;

[0089] The simulation planning module acquires the path planning task and uses the path planning algorithm to simulate and adjust the joint pose group of the cutter-changing robot model in the internal environment modeling of the tunnel boring machine, so as to ensure that the path planning task is simulated and completed without touching the obstacles inside the tunnel boring machine and to generate the corresponding joint motion scheme.

[0090] The drive execution module adjusts the joint pose group of the tool changer robot sequentially according to the joint motion scheme to complete the path planning task. It also decides autonomously whether to activate the structured light camera to capture visual and depth images and send them to the analysis feedback module based on the joint motion scheme. The module obtains the joint fine-tuning scheme and further fine-tunes the joint pose group of the tool changer robot to complete the operation task corresponding to the task signal. The structured light camera includes a front structured light camera mounted on the front plane of the actuator and a rear structured light camera mounted on the back plane of the actuator.

[0091] The analysis and feedback module acquires visual and depth images, and calculates the target coordinate system in the visual image using the target feature pose analysis method. Compared to the first Joint coordinate system of each joint The rotation angle and offset are used to generate a joint fine-tuning scheme through reverse backtracking and then fed back to the drive execution module.

[0092] Furthermore, the global formulation module establishes the kinematic model of the tool-changing robot using the DH method, including the following specific steps:

[0093] Count the total number of joints of the tool-changing robot Joints are used to perform rotation or movement in a certain direction to drive the tool-changing robot to rotate or move as a whole or in part. Joints include rotary joints and translating joints.

[0094] Using the starting point of the tool-changing robot's moving guide as the base coordinate system The origin is used as the base coordinate system, with the direction perpendicular to the tool change plane and pointing towards the tool change plane as the base coordinate system. of The coordinate system is based on the axis direction, with the direction perpendicular to the moving plane of the tool-changing robot and vertically upward. of The axis direction is determined by the right-hand screw rule, with the right thumb and index finger perpendicularly representing the base coordinate system. of Axial direction and Using the axis direction as the base coordinate system, with the direction of the naturally curved right middle finger as the base coordinate system. of Axial direction;

[0095] Based on the The execution function of each joint determines the joint coordinate system. of Axial direction, based on the first Each joint Axis and the first Each joint The common perpendicular of the axes determines the joint coordinate system. The origin and The axis direction is determined by the right-hand screw rule to establish the joint coordinate system. of axial direction to construct the first Joint coordinate system of each joint At this point, the first joint to the second joint can be obtained. The joint coordinate system of each joint;

[0096] Using the center of the actuator front plane of the tool-changing robot as the first Joint coordinate system of each joint The origin is defined by a joint coordinate system with the direction perpendicular to the front end plane and pointing towards the gripper. of axial direction, in joint coordinate system of The direction of the axis is used as the joint coordinate system of The axis direction is determined by the right-hand screw rule to establish the joint coordinate system. of axial direction to construct the first 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 perpendiculars and the rotation and offset between the corresponding coordinate axes are defined as follows: The link parameters of the first link, where the first link is... The first link is used to connect the second The joint and the first The first joint, the... The connection parameters of the first link depend specifically on the actual tool changer robot. The joint and the first The connection structure of each joint;

[0098] Calculated from the base coordinate system using the Jacobian matrix To the joint coordinate system Transformation matrix Transformation matrix Describes the base coordinate system base coordinates of any point With joint coordinate system The corresponding number below Joint coordinates The transformation relationship is the first The mathematical representation of the joint pose of a joint.

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

[0100] Determine the first The function of a joint is rotation; if the function is rotation, then the joint is a rotational joint, and the joint coordinate system is... of The axial direction is the first If the joint axis of a joint is oriented in a linear motion, then the joint is a translational joint, and the joint coordinate system is as follows: of The axial direction is the first The direction of linear motion of each joint , The total number of joints;

[0101] The first Each joint Axis and the first Each joint The common perpendicular of the axis is The foot of the perpendicular on the axis serves as the joint coordinate system. The origin;

[0102] Parallel to shaft and The common perpendicular of the axis and by Axis direction The direction of the axis is used as the joint coordinate system of Axial direction;

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

[0104] As an example, Figure 4 This is a side view of the joint coordinate system established using the DH method for the hydraulically driven nine-DOF tool changer robot. The robot comprises 5 rotary joints and 4 translational joints, for a total of 9 joints. , and The first Joint coordinate system of each joint The three coordinate axes, ,exist Figure 4 In the example, The directions of the coordinate axes not marked in the figure in each joint coordinate system are determined automatically according to the joint coordinate system construction method.

[0105] like Figure 5 As shown, further, the first The link parameters of each link include the link length. Linkage angle Linkage offset and joint angle ;

[0106] Linkage length Defined as the first Each joint Axis to the Each joint The distance between the common perpendiculars of the axes;

[0107] Linkage angle Defined as the first Each joint Axis winding Rotate the axis to the same position as the first axis. Each joint The rotation angle when the axis is parallel, wherein the direction of rotation is determined by the right-hand screw rule as the direction of the four fingers wrapping around;

[0108] Linkage offset Defined as joint coordinate system The origin to the first Each joint Axis and the first Each joint The distance between the common perpendiculars of the axes;

[0109] Joint angle Defined as the first Each joint around the axis Each joint Rotate the axis to the same The rotation angle when the axis is parallel, wherein the direction of rotation is determined by the right-hand screw rule as the direction of the four fingers encircling the axis.

[0110] Furthermore, the transformation matrix is ​​derived using the Jacobian matrix. The specific steps include the following:

[0111] Decomposition of transformation matrix From the base coordinate system To the joint coordinate system The transformation process is broken down into: Each single-step transformation involves changing the base coordinate system. Or transform the joint coordinate system of the current joint to the joint coordinate system of the next joint;

[0112] The single-step transformation process can be further broken down into four sub-steps. For the first... Joint coordinate system of each joint , joint coordinate system Around the joint coordinate system of Axis rotation is equivalent to connecting rod rotation angle The angle makes the joint coordinate system of Axis and Joint Coordinate Systems of parallel axes, obtain the first One transformation of the joint coordinate system The transformation matrix of this sub-step is defined as follows: ;

[0113] Transform the joint coordinate system once Along a first transformation joint coordinate system of Length of axial translation link This allows for a single transformation of the joint coordinate system. of Axis and Joint Coordinate Systems of Coincident axis, obtain the first The joint coordinate system undergoes a secondary transformation. The transformation matrix of this sub-step is defined as follows: ;

[0114] Secondary transformation of the joint coordinate system Around the second transformation joint coordinate system of Axis rotation is equivalent to joint angle The angle makes the joint coordinate system undergo a second transformation. of Axis and Joint Coordinate Systems of Parallel axes, obtain the joint coordinate system of the three transformations. The transformation matrix of this sub-step is defined as follows: ;

[0115] Transform the joint coordinate system three times Along the cubic joint coordinate system of Axial translation link offset Make the joint coordinate system undergo three transformations. With joint coordinate system Completely overlapping, the transformation matrix of this sub-step is defined as ;

[0116] The result of multiplying the transformation matrices of the four sub-steps in the single-step transformation process is the first step. Joint coordinate system of each joint To the joint coordinate system single-step transformation matrix The specific formula is as follows:

[0117] ,

[0118] ,

[0119] ,

[0120] ,

[0121] in, and From the joint coordinate system To the joint coordinate system The single-step rotation transformation matrix and the single-step translation transformation matrix, Represents the transpose of a matrix or vector. and Let these represent the cosine function and the sine function, respectively. It is a vector of all zeros;

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

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

[0124] The system receives bolt removal signals and extracts cutter change numbers. Since the cutter boxes are fixed and arranged regularly within the tunnel boring machine's cutterhead, the base coordinates of the cutter boxes can be determined based on the cutter change numbers. The operating device is determined to be a wrench, and the tool box base coordinates are set. of Axis value Subtract the wrench size and safe distance Generate the first endpoint base coordinates Task 1 involves constructing a path planning system based on the default joint pose group of the tool-changing robot. The default joint pose group refers to the pose group of the tool-changing robot at each startup. The path planning task one involves adjusting the wrench of the tool changer robot to be near the tool box, with the wrench directly facing the tool changing plane, while maintaining a safe distance. The dimensions need to be determined in advance based on the internal space of the tunnel boring machine and the size of the cutter-changing robot to ensure that the cutter-changing robot can be fine-tuned;

[0125] Receive the old tool disassembly signal, switch the operating device to a gripper, and acquire the joint pose group one and the tool box base coordinates. Set the toolbox base coordinates of Axis value Subtract the gripper size and safe distance Generate the second endpoint base coordinates Combine the second endpoint base coordinates Task 2 involves constructing a path planning system based on joint pose group 1. In this task, the joint pose group 1 refers to the tool-changing robot adjusting its position according to joint motion scheme 1 and joint fine-tuning scheme 1. The joint pose group after each joint, namely joint motion scheme one and joint fine adjustment scheme one, is used to complete path planning task one. The purpose of path planning task two is to adjust the gripper of the tool changing robot to the vicinity of the tool box and the gripper is exactly facing the tool changing plane.

[0126] Upon receiving the old knife retrieval signal, the operating device remains a gripper, acquiring the second joint pose group and the base coordinates of the old knife retrieval box. The base coordinates of the old knife recycling box of Axis value Subtract the gripper size and safe distance Generate the third endpoint base coordinates Combine the third endpoint base coordinates Task 3 involves constructing a path planning system based on joint pose group 2. In this task, the joint pose group 2 refers to the tool-changing robot adjusting its position according to joint motion scheme 2 and joint fine-tuning scheme 2. The joint pose group after each joint, namely joint motion scheme two and joint fine-tuning scheme two, is used to complete path planning task two. The purpose of path planning task three is to adjust the gripper of the tool changing robot to the old tool recycling box and make the gripper exactly face the plane where the old tool recycling box is located.

[0127] Upon receiving the new blade installation signal, the operating device remains a gripper, and the joint pose group three and the second endpoint base coordinates are acquired. Set the second endpoint base coordinates Directly used as the fourth endpoint base coordinate Combine the fourth endpoint base coordinates Task 4 involves constructing a path planning system based on joint pose group 3. Specifically, the joint pose group 3 for the knife-changing robot adjusts its position according to joint motion scheme 3 and joint fine-tuning scheme 3. The joint pose group after each joint, namely joint motion scheme three and joint fine-tuning scheme three, is used to complete path planning task three. The purpose of path planning task four is to readjust the gripper of the tool changing robot back to the vicinity of the tool box and make the gripper exactly face the tool changing plane.

[0128] Receive bolt installation signal, switch operating device to wrench, and acquire joint pose group four and first endpoint base coordinates. Set the first endpoint base coordinates Directly used as the fifth endpoint base coordinate Combine the fifth endpoint base coordinates Task 5 involves constructing a path planning system based on joint pose group 4. In this task, the joint pose group 4 finger-changing robot adjusts its position according to joint motion scheme 4 and joint fine-tuning scheme 4. The joint pose group after each joint, namely joint motion scheme four and joint fine-tuning scheme four, is used to complete path planning task four. The purpose of path planning task five is to readjust the wrench of the tool changing robot back to the vicinity of the tool box and make sure the wrench is directly facing the tool changing plane.

[0129] Upon receiving the task completion signal, the operating device remains a wrench. The fifth joint pose group and the default joint pose group are acquired. Based on the default joint pose group and the dimensions of each joint of the tool-changing robot, the default base coordinates of the wrench before the tool-changing robot's startup are determined and used as the sixth endpoint base coordinates. Combine the sixth endpoint base coordinates Task 6 involves constructing a path planning system based on joint pose group 5. Specifically, the joint pose group 5 for the knife-changing robot adjusts its position according to joint motion scheme 5 and joint fine-tuning scheme 5. The joint pose group after each joint, namely joint motion scheme five and joint fine-tuning scheme five, is 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 does not require fine-tuning, the subsequent path planning algorithm will only generate the corresponding joint motion scheme six.

[0130] Specifically, the internal environment modeling of the tunnel boring machine and the cutterhead robot modeling can be pre-established using existing scanning technology combined with existing 3D reconstruction technology, and based on the base coordinate system. Placed in simulation software, it can be used to pre-simulate and plan the movement path of the cutter-changing robot inside the tunnel boring machine. The internal environment of the tunnel boring machine is modeled as a cubic grid, 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 acquires the path planning task and uses a path planning algorithm to generate the corresponding joint motion scheme, including the following specific steps:

[0132] Obtain the path planning task and its corresponding starting joint pose group and ending coordinate system. The path planning task includes Path Planning Task 1, Path Planning Task 2, Path Planning Task 3, Path Planning Task 4, Path Planning Task 5, and Path Planning Task 6. The starting joint pose group includes the default joint pose group, Joint Pose Group 1, Joint Pose Group 2, Joint Pose Group 3, Joint Pose Group 4, and Joint Pose Group 5. The ending coordinate system includes the first ending coordinate system. Second endpoint base coordinates Third endpoint base coordinates Fourth endpoint base coordinates Fifth endpoint base coordinates and the sixth endpoint base coordinates .

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

[0134] The initial joint pose group in the path planning task is set as the root node of a random tree. The root node encompasses the angles of all rotational joints and the displacements of all translational joints in the initial joint pose group. The first... 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 included angle of the axis;

[0135] To prevent the path planning algorithm from getting stuck in an infinite loop, a maximum number of iterations is set. In this embodiment, the maximum number of iterations is set to 1000.

[0136] Set the search radius of the joint For rotating joints, the search radius The search radius is the maximum rotation angle between a node and its corresponding child node in the random tree, and for a moving joint, it is... This represents the maximum distance that can be moved between a node and its corresponding child node in a random tree.

[0137] Path planning constraints are set so that when the tool changer's manipulator reaches the endpoint base coordinates, the path planning task determines whether the manipulator is a wrench or a gripper. If the manipulator is a wrench, the actuator's back plane should ideally be aligned with the tool change plane. In this case, the joint coordinate system... of Axis and base coordinate system of cosine value of the included angle of the axis =-1, that is shaft and If the axes are opposite, and the operating device is a gripper, then it is desirable for the front plane of the actuator to be directly opposite the tool change plane. In this case, the joint coordinate system is required to be... of Axis and base coordinate system of cosine value of the included angle of the axis It is 1, that is shaft and The axes are in the same direction;

[0138] In each iteration of the path planning algorithm, the target base coordinates for that iteration are randomly generated in the modeling of the tunnel boring machine's interior. Using the established kinematic model of the cutter-changing robot, an inverse kinematics algorithm is employed to solve for the target joint pose set corresponding to the target base coordinates. The target joint pose set records the modeling of the cutter-changing robot when the operating device reaches the target base coordinates. Regarding the joint pose of each joint, in this embodiment, the inverse kinematics algorithm adopts the Newton-Raphson numerical iteration method, which is existing technology and will not be elaborated on further.

[0139] Search the random tree for the node with the smallest Euclidean distance to the target joint pose group as the nearest node. Use a dual 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 the child node of the nearest node into the random tree and establish the corresponding edge, and start a new round of iteration.

[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. The first condition is that the Euclidean distance between the target base coordinates and the destination base coordinates of the node is less than the distance error threshold.

[0141] Obtain all nodes that satisfy the first condition and path planning constraints. Backtrack to the root node based on the parent-child relationship of each node to generate the corresponding path. Sum the total edge length of each path, select the path with the smallest total edge length, and arrange all nodes on the path forward according to the parent-child relationship to generate joint motion schemes. These 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. Corresponding to 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, determine the joints to adjust based on the joint sequence;

[0144] For joint adjustments, based on the search radius Determine the adjustment amount of the joint to perform the joint adjustment, and change the posture of the tool-changing robot model in the simulation software accordingly;

[0145] Perform collision detection to determine whether there is an occupied cubic grid in the grid covered by the model of the current tool-changing robot's pose in the model of the tunnel boring machine's internal environment.

[0146] If there is a cube grid in the grid state that is occupied, it indicates that the tool-changing robot in the current posture will collide with the obstacle inside the tunnel boring machine. Further exhaustive search judgment is performed to determine whether the adjustment amount is greater than or equal to the adjustment upper limit value. 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 adjustment limit, it indicates that the nearest node cannot be adjusted to the target joint pose group without colliding with obstacles inside the tunnel boring machine, and a new round of iteration is started.

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

[0149] If no cube grid is found to be occupied, perform a joint exhaustive check to determine the appropriate tool-changing robot. Have all joints been adjusted?

[0150] If the tool-changing robot There are still some unadjusted joints among the joints. Based on the joint sequence, the next joint is designated as the adjusted joint, and the joint adjustment and collision detection are continued.

[0151] If the tool-changing robot All joints have been adjusted. Determine whether the Euclidean distance between the nearest node after adjustment and the target joint pose group is less than the pose error threshold.

[0152] If the error is greater than or equal to the pose error threshold, it indicates that the nearest node has not been adjusted to the target joint pose group. Among the joints, the joint that satisfies the condition that the adjustment amount is less than the upper limit of adjustment and is the first in the sequence is selected as the adjustment joint. The search radius is then added to the current adjustment amount of the adjustment joint. To continue joint adjustments and collision detection;

[0153] If the error 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 as a child node of the nearest node into the random tree. The edges between the nodes are 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 receiving the joint motion scheme, the drive execution module, since the joint motion scheme includes multiple nodes, each node uniquely corresponds to a specific joint pose group of a tool-changing robot, adjusts the tool-changing robot sequentially according to the node order in the joint motion scheme. For each node in the joint motion scheme, the joint pose of a single joint is obtained sequentially according to the joint order, and the difference between the single joint pose and the current joint pose is calculated to obtain the joint pose adjustment amount. The drive power of the driver is generated by combining the joint pose adjustment amount with the PID control algorithm. The driver is run with the drive power to adjust the single joint to the corresponding joint pose. This is executed step by step to complete the path planning task corresponding to the joint motion scheme. Furthermore, it is determined whether the joint motion scheme is joint motion scheme six. If it is joint motion scheme six, there is no need to start the front or rear camera. If it is joint motion scheme one to joint motion scheme five, the rear structured light camera or the front structured light camera is started according to the operating device, which is a wrench or a gripper, to obtain visual and depth images and send them to the analysis feedback module.

[0155] Specifically, the drive execution module acquires the joint fine-tuning scheme. Since the joint fine-tuning scheme is a single adjustment after the corresponding joint motion scheme has been adjusted, meaning there is only one unique joint pose group in the joint fine-tuning scheme, the joint pose adjustment amount of each individual joint in the joint fine-tuning scheme is acquired sequentially according to the joint sequence. The drive power of the actuator is generated by combining the joint pose adjustment amount with a PID control algorithm, and the actuator is run with the drive power to adjust the individual joint. When the tool changing robot... Once all joints are adjusted, it indicates that the wrench or gripper of the tool changing robot has been successfully aligned with the target. The corresponding preset operation program is then executed to complete the corresponding operation task. The target includes bolts, old tools, old tool recycling port, new tool mounting port, and bolt mounting port. The preset operation program is used to perform bolt removal, old tool removal, old tool recycling, new tool installation, and bolt installation, respectively.

[0156] Furthermore, the analysis and feedback module processes the visual and depth images using target feature pose analysis and then backtracks to generate a joint fine-tuning scheme, including the following specific steps:

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

[0158] An object detection network is used to identify targets in a filtered visual image and generate target contours. Based on the target contours, the pixel coordinates of the target's center axis and center point are determined. , and The target center point in the two-dimensional pixel coordinate system In Axis values ​​and The axis value is used to further obtain the target center depth in the depth image. In this embodiment, the target detection network adopts the existing Faster-RCNN network;

[0159] Calculate the target centerline and the two-dimensional pixel coordinate system In The angle between the imaging plane and the axis is used as the target coordinate system. of Axis and Joint Coordinate Systems of The deviation angle between axes, target coordinate system of Axis and Joint Coordinate Systems of With the axes aligned, and since the target is located on the tool changing plane or the plane where the old tool retrieval 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 aligned and parallel, so the target's centerline is parallel to the two-dimensional pixel coordinate system. In The included angle of the axes is always equal to that of the target coordinate system. of Axis and Joint Coordinate Systems of The deviation angle between axes, i.e., the joint coordinate system of Axis winding After rotating the axis by the deviation angle, it can be aligned with the target coordinate system. of The axes are in the same direction 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 Image center pixel coordinates of Shaft deviation and The axis deviation is back-projected to obtain the target center camera coordinates. , , and The target center point in the camera coordinate system In Axis value, Axis values ​​and The specific formulas for axis values ​​and back projection transformation are as follows:

[0161] ,

[0162] ,

[0163] ,

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

[0165] If it is the coordinate system of the front camera Since the front-mounted optical structure camera is installed on the front-end plane of the actuator and the optical center of the front-end optical structure camera coincides with the center of the front-end plane, when the front-mounted optical structure camera is facing the tool changing plane or the plane where the old tool recycling box is located, according to the first... Joint coordinate system Definition of joint coordinate system and front camera coordinate system The coordinates of the front camera coincide; therefore, the coordinate system of the front camera... It can be directly regarded as a joint coordinate system Joint coordinate system To the front camera coordinate system single-step transformation matrix As a unit array, the target center camera coordinates Directly multiply by the single-step transformation matrix The inverse matrix can be used to reconstruct and obtain the target center base coordinates. ;

[0166] If it is the coordinate system of the rear camera Since the rear optical structure camera is mounted on the back plane of the actuator and its optical center coincides with the center of the back plane, when the rear optical structure camera is facing the tool changing plane or the plane where the old tool recycling box is located, according to the first... Joint coordinate system Definition of rear camera coordinate system Joint coordinate system along In the negative direction of the axis, with the actuator thickness Perform a translation, and then rotate around the translated area. Obtained by rotating the axis 180 degrees; at this point, the joint coordinate system... To the rear camera coordinate system 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 inverse matrix and transformation matrix The inverse matrix can be used to reconstruct and obtain the target center base coordinates. ;

[0169] Calculate the target center coordinates The endpoint coordinates 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 schemes, starting from the first... Each joint begins a reverse backtracking process, sequentially selecting those capable of achieving rotation. Rotational joints of the axis of rotation, along Translational joints along axis The translational joints along the axis and The translational joint is used as the actuating joint to control the deviation angle. Axis offset, Axis offset and The axis offsets are filled into the corresponding positions of the joints in the all-zero joint fine-tuning scheme to generate joint fine-tuning schemes. The joint fine-tuning schemes correspond one-to-one with the joint motion schemes, 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 an improvement on Embodiment 1, and is basically the same as Embodiment 1. Modules that are identical will not be described in detail. The path planning algorithm disclosed in Embodiment 1 solves for the target joint pose set corresponding to the target base coordinates using an inverse kinematics algorithm, which is prone to multiple solutions. As an improvement, preferably, a multilayer perceptron with a singular value penalty term and a bottleneck structure is used instead of the inverse kinematics algorithm to solve for the target joint pose set corresponding to the target base coordinates. The bottleneck structure is achieved by inserting a fully connected layer with half the number of neurons between two fully connected layers with the same number of neurons. This compression and expansion of dimensions, combined with the fully connected layer... The nonlinear activation function enhances the feature representation capability. The traditional loss function of the multilayer perceptron is defined as the mean square error between the target base coordinates and the target joint pose set output by the network and the coordinates obtained by substituting them into the established kinematic model of the tool-changing robot. On the basis of the traditional loss function, a singular value penalty term is superimposed. The singular value penalty term is equal to the product of the minimum singular value of the Jacobian matrix of the target joint pose set and a negative learning multiplier. Since the singular value of the Jacobian matrix is ​​too large, it indicates that the degree of freedom or motion stability of the tool-changing robot is poor. Therefore, by introducing the singular value penalty term, the stability and subsequent adjustability of the target joint pose set output by the multilayer perceptron are increased.

[0173] Example 3:

[0174] This embodiment is an improvement on embodiments 1 and 2, and is basically the same as embodiments 1 and 2. Identical modules will not be described in detail. The path planning algorithms disclosed in embodiments 1 and 2 involve a dual detection strategy, requiring starting from the nearest node and adjusting joints according to the joint sequence. However, for the tool-changing robot... The influence of each joint on the end effector is not the same. Adjusting the joint traversal ensures 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 for the first joint of the tool-changing robot. Extract the joint from the Jacobian matrix. The Jacobian vector of the column, and calculate the first... The sum of the absolute values ​​of the elements of the Jacobian vectors in the column will be the first... The sum of the absolute values ​​of the elements of the Jacobian vector of a column, superimposed with a tiny positive number and then the reciprocal is taken to generate the first Jacobian vector. The priority of the nth joint, due to the Jacobian matrix... The Jacobian vector of the column corresponds to the first... The first joint reflects the first The improved dual detection strategy determines the joints to be adjusted based on their priority from highest to lowest, considering the impact of the pose changes of each joint on the position coordinates of the end effector. Since the joints with the greatest impact are selected for adjustment first, collision detection can identify collisions as early as possible, thus initiating a new round of iterations and reducing the complexity of the dual detection strategy.

[0175] This invention discloses a modular mobile control system for a tool-changing robot, comprising a global planning module, a simulation planning module, a drive execution module, and an analysis feedback module. The global planning module establishes a kinematic model of the tool-changing robot using the DH method, receives different task signals, determines the initial joint pose group and the endpoint base coordinates, and combines them to construct a path planning task. The simulation planning module acquires the path planning task and uses a path planning algorithm based on a dual detection strategy to control the tool-changing robot to move without collision within the modeled environment of the tunnel boring machine to generate a joint motion scheme. The drive execution module adjusts the joint pose group sequentially according to the joint motion scheme to complete the path planning task, and autonomously decides whether to capture visual and depth images, acquires a joint fine-tuning scheme, and further fine-tunes the joint pose group to complete the corresponding operation task. The analysis feedback module acquires visual and depth images, uses target feature pose analysis to calculate the deviation angle and offset, and generates a joint fine-tuning scheme through reverse backtracking, achieving high-precision, collision-free control 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 embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing 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 a global planning module, a simulation planning module, a drive execution module, and an analysis and feedback module; The global planning module constructs a joint coordinate system using the DH method and obtains the transformation matrix based on the Jacobian matrix; it receives task signals, determines the starting joint pose group and the ending base coordinates, and constructs the path planning task. The simulation planning module acquires the path planning task, adopts the path planning algorithm, establishes obstacle constraints through the gridded environment, takes the starting joint pose group as the root node, iteratively generates the target base coordinates and solves the corresponding joint pose group, searches for the nearest node and executes a dual detection strategy to determine whether to build a random tree as a child node, selects the path with the smallest total side length, satisfies the constraints and reaches the destination base coordinates, and generates the joint motion scheme. The drive execution module executes the joint motion plan, decides whether to capture visual and depth images, and acquires and executes the joint fine-tuning plan. The analysis and feedback module acquires visual and depth images, uses a target detection network to obtain the target's central axis and the target's center pixel coordinates, calculates the deviation angle based on the target's central axis, generates the target's center camera coordinates by combining the target's center depth and focal length with back projection, generates the target's center base coordinates by inversely generating the target's center base coordinates and calculating the offset based on the transformation matrix between the camera coordinate system and the base coordinate system, and backtracks to generate a joint fine-tuning scheme. The dual detection strategy starts from the nearest node, sequentially determines the joints to be adjusted, determines the adjustment amount based on the search radius, and executes it; it performs collision detection to determine whether the current tool-changing robot model has a collision; if a collision occurs and the adjustment amount is greater than or equal to the adjustment upper limit, the next iteration is started; if a collision occurs and the adjustment amount is less than the adjustment upper limit, the search radius is added to the current adjustment amount and joint adjustment and collision detection are performed; if there is no collision and there are unadjusted joints, the next joint to be adjusted is determined sequentially and joint adjustment and collision detection are performed; if there is no collision, all joints are adjusted, and the Euclidean distance between the adjusted nearest node and the target joint pose group is greater than or equal to the pose error threshold, the joint that satisfies the condition that the adjustment amount is less than the adjustment upper limit and is the first in the sequence is selected as the adjusted joint, and the search radius is added to the corresponding adjustment amount to perform joint adjustment and collision detection; otherwise, the target joint pose group is placed as a child node of the nearest node into a random tree and an edge is generated, and the next iteration is started. The analysis and feedback module performs Gaussian filtering on the visual image and generates a target contour through a target detection network, determines the target center pixel coordinates between the target's central axis and the target center point, and obtains the target center depth based on the depth image; it also calculates the coordinates of the target's central axis and pixel coordinates. The angle between the imaging plane and the axis is used as the target coordinate system. Axis and Joint Coordinate Systems of The deviation angle of the axis; obtain the image center pixel coordinates and focal length, based on the target center pixel coordinates and the image center pixel coordinates. Shaft deviation and The axis deviation is back-projected and multiplied by the joint coordinate system. The target center base coordinates are obtained by taking the inverse of the single-step transformation matrix and the inverse of the transformation matrix from the front camera coordinate system or the rear camera coordinate system; the target center base coordinates and the corresponding endpoint base coordinates of the path planning task are calculated in the base coordinate system. Axis offset, Axis offset and Axis offset, from the first Starting with each joint, select executable joints sequentially to generate a joint fine-tuning plan.

2. The modular mobile control system for a tool-changing robot as described in claim 1, characterized in that, The simulation planning module also models the internal environment of the tunnel boring machine and the cutter-changing robot using scanning and 3D reconstruction technologies, simulates and plans the movement path of the cutter-changing robot, divides the internal environment of the tunnel boring machine into cubic grids, and marks the grid status as occupied or idle based on whether there are obstacles in the cubic grids to assist the dual detection strategy in determination.

3. The modular mobile control system for a tool-changing robot as described in claim 1, characterized in that, The simulation planning module uses a path planning algorithm to generate corresponding joint motion schemes, including the following steps: Obtain 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, and restrict 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 set is solved using an inverse kinematics algorithm; Search for the nearest node in the random tree. Based on the dual detection strategy, 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. Once the maximum number of iterations is reached, the path corresponding to each node that satisfies the first condition and the path planning constraint is generated by backtracking in reverse according to the parent-child relationship. The path with the smallest total side length is selected to generate the joint motion scheme. The first condition is that the Euclidean distance between the target base coordinates and the endpoint base coordinates of the node is less than the distance error threshold.

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

5. A modular mobile control system for a tool-changing robot as described in any one of claims 3 to 4, characterized in that, An adaptive joint sorting strategy is introduced into the dual detection strategy for the first joint of the tool-changing robot. Extract the joint from the Jacobian matrix. The Jacobian vector of the column and the calculation of the first... The sum of the absolute values ​​of the elements of the Jacobian vectors in the column will be the first... The sum of the absolute values ​​of the elements of the Jacobian vector of a column is added to a positive number and then the reciprocal is taken to generate the first Jacobian vector. The priority of the first joint is to reflect the priority of the second joint. The pose changes of each joint affect the position coordinates of the end effector. The improved dual detection strategy determines the joints to be adjusted according to the priority from high to low.

6. The modular mobile control system for a tool-changing robot as described in claim 1, characterized in that, The global designation module constructs the joint coordinate system using the DH method, including the following steps: Count the total number of joints The origin of the base coordinate system is taken as the starting point of the moving guide rail, the direction perpendicular to and pointing towards the tool change plane, and the direction perpendicular to the moving plane and upward, respectively. Axial direction and The axis direction is determined by the right-hand screw rule to establish the base coordinate system. Axial direction; Based on the The execution function of the first joint is determined. Joint coordinate system of each joint Axial direction, based on Axis and the first Each joint The common perpendicular of the axis determines the first The origin and joint coordinate system of each joint The axial direction is determined by the right-hand screw rule. Joint coordinate system of each joint In the axial direction, obtain the first joint to the second joint. The joint coordinate system of each joint; With the center of the actuator front end plane, perpendicular to the front end plane and pointing towards the gripper, and the first Joint coordinate system of each joint The directions of the axes are respectively used as the first The origin of the joint coordinate system of each joint. Axial direction and The axial direction is determined by the right-hand screw rule. Joint coordinate system of each joint axial direction, construct the first Joint coordinate system for each joint.

7. The modular mobile control system for a tool-changing robot as described in claim 6, characterized in that, Construct the first The joint coordinate system for each joint includes the following specific steps: According to the If the function of each joint is rotation or linear motion, then The axial direction is the first The direction of the joint axis or the direction of linear motion of each joint; The first Each joint Axis and the first Each joint The common perpendicular of the axis is The foot of the perpendicular on the axis is the first The origin of the joint coordinate system of each joint; Parallel to shaft and The common perpendicular of the axis and by Axis direction The direction of the axis as Axial direction; Determined by the right-hand screw rule axial direction, the first The joint coordinate system for each joint has been constructed.

8. The modular mobile control system for a tool-changing robot as described in claim 1, characterized in that, The global designation module, which constructs the joint coordinate system using the DH method, also includes the following steps: Based on the The joint coordinate system of the first joint and the joint coordinate system of the second joint The relative relationship of the joint coordinate systems of the nth joint is defined. The link parameters of the first link, the second link The link parameters of each link include link length, link rotation angle, link offset, and joint angle, where the link length is... Axis to The distance between the common perpendiculars of the axes, and the rotation angle of the connecting rod. Axis winding Rotate the axis to the same The rotation angle when the axis is parallel, and the linkage offset is the first... The origin of the joint coordinate system of each joint to shaft and The distance between the common perpendiculars of the axes, and the joint angle is... Axis winding Rotate the axis to the same The rotation angle when the axis is parallel.

Images