Large multi-degree-of-freedom robot trajectory planning method for cold source tunnel cleaning

Abstract

The present application belongs to the field of industrial robots, and particularly relates to a large multi-degree-of-freedom mechanical arm trajectory planning method for cold source tunnel cleaning. The method comprises the following steps: confirming inherent properties of the tunnel, vehicle position information, mechanical arm position information, controlled variables and motion targets; trajectory planning; using laser ranging to measure the actual distance from the G point to the wall surface in real time, or measuring the actual pressure through a pressure sensor, so as to use the actual distance or the actual pressure as a control item of the telescopic cylinder to control the telescopic arm; and limiting the sensor wall surface adaptation through the planned trajectory, while avoiding complete sensor wall surface adaptation from losing control due to sensor measurement errors. The method has the beneficial effects that: the safe and redundant design can judge the safety of the operation by judging the difference between the theoretical trajectory and the actual adaptive trajectory, and when the sensor measurement data is unstable, the trajectory planning operation is used to ensure the safety of autonomous operation.

Description

Technical Field

[0001] This invention belongs to the field of industrial robots, specifically relating to a trajectory planning method for a large multi-degree-of-freedom robotic arm used for cleaning cold source tunnels. Background Technology

[0002] The final heat sink for a nuclear power unit during operation is the seawater cold source, which is responsible for removing core preheating and cooling of various nuclear safety equipment during shutdown. It plays a crucial role in nuclear power plant operation, and the condition of the intake directly affects the safe operation and reliability of the power plant. The heat generated during unit operation can cause marine organisms to attach to the cold source tunnel. Currently, the main methods to reduce marine organism attachment in the tunnel are chlorine spraying during unit operation, protective coating spraying on tunnel walls, and manual or mechanical cleaning during overhauls. However, protective coating spraying on tunnel walls has environmental impacts, and chlorine spraying alone during unit operation cannot completely prevent marine organism growth. Manual or mechanical cleaning during overhauls becomes a necessary method to maintain the cleanliness of the seawater cold source tunnel. Manual cleaning of the cold source tunnel carries risks such as working in confined spaces and at heights, and the efficiency of single-person cleaning is low. Existing mechanical cleaning treats the tunnel cross-section as a standard circle, but due to construction errors, the tunnel cross-section is not a standard circle, and the chassis carrying the robotic arm has positioning errors, which can cause the robotic arm's working cross-section to become elliptical. These factors lead to poor mechanical cleaning results and may even damage the tunnel wall. Summary of the Invention

[0003] The purpose of this invention is to provide a trajectory planning method for a large multi-degree-of-freedom robotic arm used for cleaning cold source tunnels. This method can effectively solve the problem of robotic arm trajectory planning and control during the cleaning process of cold source tunnel walls in nuclear power plants. It fully considers flat-bottomed and round-bottomed tunnels, the irregularity of tunnel walls, the positioning information of the robotic arm carrier, and operational safety, thereby achieving the goal of fully autonomous cleaning of tunnel walls.

[0004] The technical solution of this invention is as follows: a trajectory planning method for a large multi-degree-of-freedom robotic arm used for cleaning cold source tunnels, comprising the following steps:

[0005] Step 1: Confirm the inherent properties of the tunnel, vehicle position information, robotic arm position information, controlled variables, and moving targets;

[0006] Step 2: Trajectory planning;

[0007] Step 3: Use laser rangefinder to measure the actual distance from point D to the wall in real time, or use a pressure sensor to measure the actual pressure, and use this as the control item for the telescopic cylinder to control the extension and retraction of the telescopic arm;

[0008] Step 4: Limit sensor wall adaptation by planning the trajectory, while avoiding complete sensor wall adaptation from going out of control due to sensor measurement errors.

[0009] The inherent properties of the tunnel include the tunnel diameter. The point is the center point of the tunnel, and the height from the flat bottom to the center of the tunnel.

[0010] The vehicle body position information includes: The center point of rotation of the robotic arm and the yaw angle of the vehicle body. Horizontal offset Longitudinal offset ,pass The distance between the origins of the two coordinate systems can be calculated as follows: The angle between the lines connecting the two centers in the robot arm's coordinate system is... .

[0011] The robotic arm position information includes: the current robotic arm phase angle. Current robotic arm flip angle The length of the telescopic arm in its fully retracted state .

[0012] The controlled quantity , that is, the amount of change of the telescopic cylinder;

[0013] The stated operational objective remains The point should remain in contact with the inner wall or flat bottom.

[0014] Step 2 includes the following:

[0015] Step 21: First, calculate the coordinates of the cutter head in the tunnel coordinate system. Point coordinate expression

[0016]

[0017] Step 22: Based on the chassis yaw angle, the polar coordinate expression of the inner wall elliptic curve is as follows:

[0018]

[0019] Step 23: From the actual height H of the robotic arm above the ground, the expression for the flat-bottomed straight line can be obtained.

[0020]

[0021] Step 24: Substituting the point expression into the curve and line expressions respectively, we get... Curve solution and linear solution At phase angle When, the curve solution That's correct, at the phase angle. When, the curve solution and linear solution The smaller value in is correct, when When, the curve solution and linear solution The smaller value in the middle must be a curve solution. By modifying parameters, a seamless connection can be achieved in trajectory calculation for tunnels with round or flat bottoms.

[0022] Step 3 includes the following:

[0023] Step 31: Measure the actual distance or pressure from point D to the wall using a sensor, and perform low-pass filtering on the measured data to reduce data oscillation;

[0024] Step 32: Combine the filtered data with calibration data of distance or pressure, and calculate the real-time data with constant pressure or constant distance as the control target. ;

[0025] Step 33: By adjusting the preset distance or pressure, different pressures are applied to the tunnel wall by the cutter head.

[0026] Step 4 includes the following:

[0027] Step 41: Obtain the corresponding telescopic arm through theoretical trajectory planning and sensor wall adaptation respectively. The control quantity, in which pressure feedback adjustment is added as feedforward to the extension and retraction of the robotic arm;

[0028] Step 42: Calculate the difference between the theoretical trajectory and the actual adapted trajectory, and compare it with the safety threshold. When the difference is less than the threshold, the robot arm runs according to the actual adapted trajectory. If the difference is greater than the threshold, the robot arm walks according to the theoretical trajectory.

[0029] The beneficial effects of this invention are as follows: The theoretical trajectory planning takes into account the orientation deviation of the carrier, transforms the working section of the robotic arm into an ellipse, and integrates the offset information between the center of the robotic arm and the center of the tunnel section obtained by measurement, so that the walking path of the robotic arm is more consistent with the actual situation; The actual wall adaptation can make real-time trajectory planning based on the undulation of the actual wall measured by laser. By setting a certain amount of pre-compression (or pre-pressure), the cleaning effect can be guaranteed while protecting the tunnel wall; The safety redundancy design can judge the safety of the operation by judging the difference between the theoretical trajectory and the actual adapted trajectory. When the sensor measurement data is unstable, it runs according to the trajectory planning to ensure the safety of autonomous operation. Attached Figure Description

[0030] Figure 1 A schematic diagram of the trajectory planning for a tunnel with a round bottom and a flat bottom.

[0031] Figure 2This is a control flowchart of the trajectory planning method for a large multi-degree-of-freedom robotic arm used for cleaning cold source tunnels, provided by the present invention. Detailed Implementation

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

[0033] The trajectory planning method for a large-scale multi-degree-of-freedom robotic arm used for cleaning cold source tunnels provided by this invention takes into account both cleaning efficiency and effect on the tunnel wall. It can autonomously plan trajectories and execute cleaning operations for both circular and flat-bottomed cold source tunnels. The fully autonomous trajectory planning mainly includes three steps: theoretical trajectory calculation, actual wall adaptation, and safety redundancy design. The theoretical trajectory calculation integrates tunnel diameter information, robotic arm carrier positioning information, and robotic arm rotation center offset information to plan the motion coordinate trajectory of the robotic arm cleaning head in the robotic arm coordinate system relative to an ideal circular tunnel. Based on the inherent dimensions of the robotic arm, the extension of the telescopic arm and the folding amount of the folding arm can be calculated. The actual wall adaptation is mainly based on the theoretical trajectory calculation. The extension and folding amounts of the robotic arm are calculated using laser ranging information (or tensile and compressive force measurement information). Feedback control is used to bring the robotic arm close to the actual tunnel wall with a certain amount of compression, ensuring cleaning effect while protecting the tunnel wall. Finally, a threshold is set between the theoretical trajectory and the actual wall-adapted trajectory. When the difference between the two trajectories exceeds the threshold, the robot operates according to the planned trajectory to ensure the safety of autonomous operation.

[0034] The robotic arm in this invention has two symmetrical working arms, mainly including three degrees of freedom. The first degree of freedom is the rotation of the main rotation and the rotation of the folding arm. The second degree of freedom is the extension and retraction of the working wall telescopic cylinder. The third degree of freedom is achieved by the forward and backward movement of the carrier.

[0035] First, the trajectory of the robotic arm's cutter head in the theoretical tunnel is planned. Using measuring equipment mounted on the chassis, the angular deviation between the normal to the robotic arm's working section and the normal to the tunnel's cross-section can be obtained, as well as the offset between the robotic arm's rotation center and the center point of the tunnel's cross-section. Using the center of the tunnel cross-section and the robotic arm's rotation center as the coordinate origins, a world coordinate system and a robotic arm coordinate system are established. Since the robotic arm exhibits both extension and rotational motions, assuming the extension and folding amounts, the position of the robotic arm's cutter head can be calculated based on the rotation angle and offset information of the main rotary motor. By placing the position in the world coordinate system and substituting that coordinate into the ellipse equation (it's worth noting that in a flat-bottomed tunnel, when the Y-value of the current extension and folding point is less than the Y-value of the straight line, the extension and folding points are solved using the straight line equation), we can obtain equations for the extension and folding points. There are two ways to solve these equations. The first method is to fix one variable and solve for the other. The second method is to assign different weights to the extension and folding points and solve for both values ​​simultaneously. After obtaining the two control values, we can perform tracking control on the robotic arm according to the solved values.

[0036] However, in reality, tunnel walls often exhibit undulations due to construction errors, deviating from the ideal circular or elliptical shape. Simply following the theoretical trajectory would result in inconsistent compression between the cutter head and the wall, affecting cleaning effectiveness and potentially damaging the tunnel wall. To address this, a distance (or force) sensor is added to the cutter head of the robotic arm to measure the distance (or pressure) between the cutter head and the tunnel wall in real time. By controlling the extension and retraction to maintain a consistent compression (or pressure), the cutter head remains in contact with the tunnel wall, thus ensuring both effective cleaning and protection of the tunnel wall.

[0037] Based on the complementarity of the two methods mentioned above, it is necessary to integrate the two trajectory planning approaches to achieve the goal of safe operation. Specifically, the theoretical trajectory planning is used as a basis, and the extension / retraction amount calculated through sensor feedback is compared. A safety threshold is set at 3 / 5 of the compressible amount of the robotic arm's cutter head, which is ±180mm. When the deviation between the extension / retraction amount obtained from the theoretical trajectory planning and the extension / retraction amount calculated through sensor feedback is greater than or less than the threshold, the robotic arm operates according to the trajectory calculation result, ensuring the safety of the robotic arm operation.

[0038] A trajectory planning method for a large multi-degree-of-freedom robotic arm used for cleaning cold source tunnels includes the following steps:

[0039] Step 1: Confirm the inherent properties of the tunnel, vehicle position information, robotic arm position information, controlled variables, and moving targets;

[0040] The inherent properties of a tunnel include: tunnel diameter. The point is the center point of the tunnel, and the height from the flat bottom to the center of the tunnel.

[0041] Vehicle location information includes: The center point of rotation of the robotic arm and the yaw angle of the vehicle body. Horizontal offset Longitudinal offset ,pass The distance between the origins of the two coordinate systems can be calculated as follows: The angle between the lines connecting the two centers in the robot arm's coordinate system is... .

[0042] The robotic arm's position information includes: the current robotic arm phase angle. Current robotic arm flip angle (Angle adjustable), length of the telescopic arm in its fully retracted state .

[0043] Controlled quantity: , that is, the amount of change of the telescopic cylinder;

[0044] Operational objective: Maintain contact between point D and the inner wall or flat bottom.

[0045] Step 2: Trajectory planning;

[0046] The specific process is as follows:

[0047] Step 21: First, calculate the coordinates of the cutter head in the tunnel coordinate system. Point coordinate expression

[0048]

[0049] Step 22: Based on the chassis yaw angle, the polar coordinate expression of the inner wall elliptic curve is as follows:

[0050]

[0051] Step 23: From the actual height H of the robotic arm above the ground, the expression for the flat-bottomed straight line can be obtained.

[0052]

[0053] Step 24: Substituting the expression for point D into the expressions for the curve and the line respectively, we can obtain... Curve solution and linear solution Combining the graph, it can be seen that at the phase angle When, the curve solution That's correct, at the phase angle. When, the curve solution and linear solution The smaller value in is correct, especially when When, the curve solution and linear solution The smaller value in the middle must be a curve solution. Therefore, by modifying the parameters, a seamless connection can be achieved in the trajectory calculation of circular-bottomed and flat-bottomed tunnels.

[0054] Step 3: Use a laser rangefinder to measure the actual distance from point D to the wall in real time, or use a pressure sensor to measure the actual pressure. Use this as the control item for the telescopic cylinder to control the extension and retraction of the telescopic arm. The specific steps are as follows:

[0055] Step 31: Measure the actual distance or pressure from point D to the wall using a sensor, and perform low-pass filtering on the measured data to reduce data oscillation;

[0056] Step 32: Combine the filtered data with calibration data of distance or pressure, and calculate the real-time data with constant pressure or constant distance as the control target. ;

[0057] Step 33: By adjusting the preset distance or pressure, different pressures can be applied to the tunnel wall by the cutter head, thereby achieving different cleaning effects.

[0058] Step 4: Limit sensor wall adaptation by planning the trajectory, while avoiding complete sensor wall adaptation from going out of control due to sensor measurement errors;

[0059] The specific implementation steps are as follows:

[0060] Step 41: Obtain the corresponding telescopic arm through theoretical trajectory planning and sensor wall adaptation respectively. The control quantity, in which pressure feedback adjustment is added as feedforward to the extension and retraction of the robotic arm;

[0061] Step 42: Calculate the difference between the theoretical trajectory and the actual adapted trajectory, and compare it with the safety threshold (set the safety threshold to 3 / 5 of the compressibility of the robotic arm cutter head, which is ±180mm). When the difference is less than the threshold, the actual adapted trajectory is used to ensure real-time adaptation to the wall surface and obtain better cleaning results. If the difference is greater than the threshold, the robotic arm follows the theoretical trajectory to ensure the safety of the robotic arm operation.

Claims

1. A trajectory planning method for a large multi-degree-of-freedom robotic arm used for cleaning cold source tunnels, characterized in that, Includes the following steps: Step 1: Confirm the inherent properties of the tunnel, vehicle position information, robotic arm position information, controlled variables, and moving targets; Step 2: Trajectory planning; Step 2 includes the following: Step 21: First, calculate the coordinates of the cutter head in the tunnel coordinate system. Point coordinate expression ， Step 22: Based on the chassis yaw angle, the polar coordinate expression of the inner wall elliptic curve is as follows: ， Step 23: From the actual height H of the robotic arm above the ground, the expression for the flat-bottomed straight line can be obtained. ， Step 24: Substituting the point expression into the curve and line expressions respectively, we get... Curve solution and linear solution At phase angle When, the curve solution That's correct, at the phase angle. When, the curve solution and linear solution The smaller value in is correct, when When, the curve solution and linear solution The smaller value in the middle must be a curve solution. By modifying parameters, a seamless connection between trajectory calculation for round-bottomed and flat-bottomed tunnels can be achieved. Step 3: Use laser rangefinder to measure the actual distance from point G to the wall in real time, or use a pressure sensor to measure the actual pressure, and use this as the control item for the telescopic cylinder to control the extension and retraction of the telescopic arm; Step 3 includes the following: Step 31: Measure the actual distance or pressure from point G to the wall using a sensor, and perform low-pass filtering on the measured data to reduce data oscillation; Step 32: Combine the filtered data with calibration data of distance or pressure, and calculate the real-time data with constant pressure or constant distance as the control target. ; Step 33: By adjusting the preset distance or pressure, different pressures are applied to the tunnel wall by the cutter head; Step 4: Limit sensor wall adaptation by planning the trajectory, while avoiding complete sensor wall adaptation from going out of control due to sensor measurement errors; Step 4 includes the following: Step 41: Obtain the corresponding telescopic arm through theoretical trajectory planning and sensor wall adaptation respectively. The control quantity, in which pressure feedback adjustment is added as feedforward to the extension and retraction of the robotic arm; Step 42: Calculate the difference between the theoretical trajectory and the actual adapted trajectory, and compare it with the safety threshold. When the difference is less than the threshold, the robot arm runs according to the actual adapted trajectory. If the difference is greater than the threshold, the robot arm walks according to the theoretical trajectory. The inherent properties of the tunnel include the tunnel diameter. The point is the center point of the tunnel, and the height from the flat bottom to the center of the tunnel. H ; The vehicle body position information includes: The center point of rotation of the robotic arm and the yaw angle of the vehicle body. Horizontal offset Longitudinal offset ,pass The distance between the origins of the two coordinate systems can be calculated as follows: The angle between the lines connecting the two centers in the robot arm's coordinate system is... ; The robotic arm position information includes: the current robotic arm phase angle. Current robotic arm flip angle The length of the telescopic arm in its fully retracted state ; The controlled quantity , that is, the amount of change of the telescopic cylinder; The stated operational objective remains The point should remain in contact with the inner wall or flat bottom.

Images